matlab非锐化掩蔽和高提升滤波_6.2 各种滤波器的仿真及代码

截至上篇文章，这个专栏已经介绍了三种 IMU 姿态估计的算法：Mahony 的互补滤波器，基于欧拉角的卡尔曼滤波器，和乘法扩展卡尔曼滤波器（MEKF）。这篇文章通过仿真解释为什么 MEKF 是目前最好的姿态估计算法。各种滤波器的实现和仿真的 MATLAB 代码可以在下面的链接找到，感兴趣的朋友可以参考。

WangWeixin442/attitude-kalman-filter​github.com

1. 仿真设定

仿真使用了两种旋转运动，第一种是 roll-pitch-yaw 角度按照正弦曲线变化，频率是 0.35 Hz，幅值分别是 180，90，180 度。这个旋转运动在每一个周期内都都会经历两次欧拉角的奇异点。第二个旋转运动是 IMU 绕体坐标系的 z-轴以 6 rad/s 的角速度旋转，同时体坐标系的 z-轴绕固定坐标系的 y-轴以 1 rad/s 的角速度旋转。

陀螺仪的角度随机行走噪声设置为 0.1

，零偏的随机行走噪声为 0，因为之前介绍的方法还没有涉及如何估计变动的零偏。IMU 的姿态假设可以通过某种方法直接测量，测量的噪声用误差旋转矢量表示，也就是

，其中

和

分别是测量和真实的旋转矩阵，

，

。陀螺仪和姿态测量的采样频率为 200 Hz，仿真时间为 1 分钟。

这次仿真比较了四种滤波器，除了开头提到的三种还有直接使用四元数进行卡尔曼滤波的方法。四种滤波器都是用四元数的一阶积分对角速度积分。滤波器的初始值设置为真实的姿态绕体坐标系的 x-轴旋转

得到的姿态，也就是需要滤波器从一个完全错误的初始值收敛到接近真实值。姿态的初始标准差设置为 100 rad，基本接近于均匀分布，也就是滤波器对初始值的置信度几乎为零。

每种旋转运动都随机生成了 60 组噪声，滤波器的误差使用旋转矢量的模长表示，也就是将估计的姿态旋转到真实姿态需要的角度。误差先在每组仿真内对时间求平均，再对 60 组仿真求平均及标准差。

2. 主要结果

第一种旋转运动的误差比较如下图所示。

四种滤波器在第一种旋转运动中的误差

其中一组仿真的误差随时间变化的曲线如下图所示。

四种滤波器在第一种旋转运动中误差随时间的变化

第二种旋转运动的结果如下面两图所示。

四种滤波器在第二种旋转运动中的误差

四种滤波器在第一种旋转运动中误差随时间的变化

从结果可以看到，四种滤波器都可以很快从错误的初始值收敛到真实值附近，但是 MEKF 的精度要明显好于其它三种滤波器。从误差随时间变化的轨迹中可以明显看到奇异点对欧拉角滤波器的影响，特别是在第一种旋转运动中。

3. MEKF 和欧拉角滤波器

为了更明显地表现欧拉角自锁现象对滤波器的影响，我在第一种旋转运动中尝试了不同的俯仰角（pitch）正弦曲线的幅值，分别为

，

，

和

。仿真结果如下图所示

不同俯仰角极值对欧拉角卡尔曼滤波器精度的影响

可以看到，当俯仰角的最大值为

和

时，欧拉角远离奇异点，利用欧拉角设计的卡尔曼滤波器的精度也相对较高。但是当旋转运动更接近奇异点时，欧拉角滤波器的精度明显下降。但即使在远离奇异点的旋转运动中，欧拉角滤波器的精度也要低于 MEKF。其原因可能是欧拉角的运动学方程没有解析解，因此根据近似解推导出的方差递推公式的精度也受到了影响；但四元数和旋转矩阵的运动学方程有解析解，而 MEKF 的方差递推公式是利用解析解推导得到的，因此精度更高。

4. 互补滤波器和 MEKF

大家可能会质疑在第二节的结果中，我没有给出互补滤波器的增益

的取值，那是不是如果精心挑选一个

，互补滤波器的精度会大幅提升，甚至优于 MEKF ？在下图中，我尝试寻找了了互补滤波器在第一种旋转运动中

的最优值，大概在 0.03 左右。

不同增益值对互补滤波器精度的影响

可以看到，即使在最优的

值附近，互补滤波器的误差也要远大于 MEKF。这其中很大原因是随着

值的增大，互补滤波器的收敛速度变快，但稳态精度变低；而 MEKF 却可以通过预测值和测量值的方差动态调整增益，使估计结果最优。

值对于互补滤波器收敛速度和稳态精度的影响可以由下图误差随时间变化的曲线看出。

不同增益值对互补滤波器收敛速度和稳态精度的影响

那还有一个问题是如果只比较稳态精度，互补滤波器能不能赶得上 MEKF ？为了测试这一点，我把滤波器的初始值设为真实的姿态，MEKF 的初始标准差设为零，也就是对于初始值完全信任。两种滤波器的精度如下图所示

不同增益值对互补滤波器稳态精度的影响

可以看到，如果仔细挑选增益值，互补滤波器的稳态精度与 MEKF 非常接近。这是因为在角速度变化不大的情况下，MEKF 的增益近似趋向于定值，也就是趋向于互补滤波器。但如果对上图的结果用 paired t-test 做统计分析，即使用最好的 k 值（0.001左右）与 MEKF 比较，得到的 p 值也小于 0.001，也就是 MEKF 仍然“显著”优于互补滤波器（这里的“显著”指统计学里的显著差异），但它们均值的差别只有 0.0056 deg，完全可以忽略不记。一种可能的解释是 MEKF 的方差的递推公式与测量得到的角速度有关，因此即使已经趋于稳态，MEKF 增益还是会随着角速度的变化作微小的改变，这些微小的变化可能对精度有微小的提升。

需要注意的是，互补滤波器的稳态最优增益非常小，如果姿态的初始值离真实值较远，或者因为某些剧烈的运动（例如碰撞）造成姿态的误差突然变得很大，互补滤波器的收敛速度会非常慢，因此使用稳态最优增益是非常不好的选择。

5. 总结

这篇文章通过仿真比较了 MEKF，互补滤波器，基于欧拉角和四元数的卡尔曼滤波器。结论是 MEKF 的动态精度要明显优于其他三种滤波器；稳态精度与互补滤波器类似，但要优于其他两种滤波器。在实际应用中，如果不考虑初始的动态响应和运动中角速度较大的波动，互补滤波器不失为一种很好的选择；但如果用稳态最优的

值收敛速度会非常慢。目前嵌入式系统已经完全可以处理卡尔曼滤波器的矩阵运算，因此 MEKF 还是进行 IMU 姿态估计的第一选择。