华为官方网站手机商城首页,wordpress返回顶部插件,海南景区网站建设方案,购物类网站都有哪些模块调试 Mahony 滤波算法的思考 1. 说在前面的2.Mahony滤波算法的核心思想3. 易懂的理解 Mahony 滤波算法的过程4. 其他的一些思考5. 民间 9轴评估板 1. 说在前面的
之前调试基于QMI8658 6轴姿态解算的时候#xff0c;我对Mahony滤波的认识还比较浅薄。初次的学习和代码的移植让… 调试 Mahony 滤波算法的思考 1. 说在前面的2.Mahony滤波算法的核心思想3. 易懂的理解 Mahony 滤波算法的过程4. 其他的一些思考5. 民间 9轴评估板 1. 说在前面的
之前调试基于QMI8658 6轴姿态解算的时候我对Mahony滤波的认识还比较浅薄。初次的学习和代码的移植让我对四元数、欧拉角、旋转余弦矩阵有了一定的接触。然而直到我将地磁传感器加入到我的硬件进行9轴姿态解算时我深深地感受到对四元数与姿态阵之间关系的理解是非常重要的。在此之前我检索了网络文章并记录了《MEMS_惯性传感器09 - Mahony姿态解算算法详解》、《MEMS_惯性传感器14 - Mahony滤波算法的代码分析》两篇文章。可能对于大佬、资深工程师来说这些文章的严谨性不够可能会误导别人。但我必须承认我仍然没有扎实的数学计算研究背后更深层次的理论。即使如此我依然希望通过自己的努力在惯性导航算法上学习得更深入一些。同时我也想多记录一些学到的知识多记录一些自己的理解和思考。希望这些记录能帮助到一些初学者。如果我的文章有错误的引导我非常希望大佬们能够指正因为这对我来说是最大的收获我非常乐意倾听和学习。
我愿意自我突围向着理想前进
这次学习检索时发现了一个博文内容硬核、有理有据的博主1路痴导航员。 她的博文 《AHRS互补滤波Mahony算法及开源代码》对我帮助很大。
2.Mahony滤波算法的核心思想
Mahony滤波算法的核心思想是通过将角速度和角度的测量值与历史估计值进行加权平均从而得到更加准确的姿态估计结果。同时滤波算法还利用加速度计的测量值来纠正姿态角度的漂移。 该算法也存在一些缺点例如对于快速旋转和加速度变化较大的情况其姿态估计结果可能会出现较大的误差。
3. 易懂的理解 Mahony 滤波算法的过程
为了更好的理解表达是先不用矩阵的思想在算法的实现的过程中需要将表达是转换成矩阵形式。
① 根据陀螺仪的测量数据计算出角速度的增量即 Δθ 0.5 * (gyro bias) * Δt 其中gyro表示陀螺仪的测量值bias表示陀螺仪的零偏误差Δt表示时间间隔。 ② 根据加速度计的测量数据计算出重力方向的估计值即 g_est q * (0, 0, -1) * q^-1 其中q表示四元数的姿态估计值。 ③ 根据加速度计的测量数据和重力方向的估计值计算出加速度计的误差即 acc_err acc_meas - g_est 其中acc_meas表示加速度计的测量值。 ④ 根据角速度的增量和加速度计的误差更新四元数的姿态估计值即 q q * exp(Δθ - K * acc_err) 其中K为控制增益。 4. 其他的一些思考 传感器EVB板的测量数据以载体坐标系b-框架表示。 欧拉角的直观表示描述了地理坐标系中的角度其中包括重力的影响。 地理坐标系中的重力矢量为[0,0,1]g磁传感器将地球磁场中的强度表示为[cos(θ),0,sin(θ)]guess。因此在算法中加速度和磁数据被归一化确保它们的模是一定大小的。 四元数从地理坐标系(东北天)转换到载体坐标系就得到了旋转矩阵Cnb。 Cnb的最后一列取负即将其转换为载体坐标系中的值。在理想情况下如果没有误差这个矢量应该与测量值相等。然而由于这两者总会存在差异因此我们需要利用它们之间的差值来修正陀螺仪更新的不准确性。 在修正的过程中我们认为加速度计测量的可信度较高还需要使用地磁传感来修正加速度传感的Z轴。磁力计的模的长度是确定的但是两个分量的具体大小无法像重力加速度一样确定。因此我们使用上一次的四元数得到的Cbn从载体到地理坐标系的旋转矩阵来计算测量的磁场强度。理论上此刻的Cbn应该在东向上的分量是零[cos(θ),0,sin(θ)]而北向和地向会有分量。然而由于此刻的Cbn尚未得到我们使用的是上一次的Cbn并加上一些测量误差导致东向上仍会有分量存在。为了处理这种情况我们将xy平面的分量合成到一个方向上使得y轴上的分量为0。这样处理后的地磁信息的地位就与重力加速度00-1相似了。 根据余弦矩阵和欧拉角的定义我们可以将地理坐标系的重力向量转换到载体坐标系中。具体而言将四元数转换成方向余弦矩阵后我们可以得到方向余弦矩阵的第三列的三个元素即vx、vy和vz。这三个元素实际上表示了当前欧拉角即四元数在载体坐标系上换算得到的重力单位向量。
vx 2*(q1q3 - q0q2);
vy 2*(q0q1 q2q3);
vz q0q0 - q1q1 - q2q2 q3q3;ax\ay\az是载体坐标参照系上加速度计测出来的重力向量。vx\vy\vz是陀螺积分后的姿态推算出的重力向量。它们之间的误差向量ex\ey\ez是陀螺积分后的姿态和加计测出来的姿态之间的误差。误差向量可以用向量叉积来表示。叉积向量ex\ey\ez位于载体坐标系上且其大小与陀螺积分误差成正比可用于纠正陀螺。由于陀螺是对机体直接积分所以对陀螺的纠正量会直接体现在对载体坐标系的纠正。
exInt exInt ex * Ki * halfT;
eyInt eyInt ey * Ki * halfT;
ezInt ezInt ez * Ki * halfT;
// 用叉积误差来做PI修正陀螺零偏
gx gx Kpex exInt;gy gy Kpey eyInt;
gz gz Kp*ez ezInt;矢量之间的叉乘公式为err A × B |A| × |B| × sin(β)。当两个矢量重合时误差为零表示没有差异。当两个矢量之间夹角为90°时误差达到最大值表示差异最大。然而在夹角为90°附近的区间内误差的变化是非线性的这可能导致一些准确性问题。
5. 民间 9轴评估板
QMI8658QMC5883L 链接 路痴导航员 ↩︎
