互补滤波系数_3. 互补滤波器

首先，我们澄清一点很多人都在讨论的问题：卡尔曼滤波器是不是真的优于互补滤波器？答案是肯定的，但有一些前提：传感器的噪声是随机的，服从正态分布的，我们大概知道噪声的协方差矩阵；并且“优于”是在最小均方误差(least mean squared error)的意义上。参考文献 [1] p.284 中对于互补滤波器和卡尔曼滤波器的优劣有非常好的描述，在这里做一摘抄：“在很多实际应用中，我们对于一些测量变量的误差模型很难有准确的估计，……，或者一些误差不是随机的或正态分布的。因此，如果有不需要对测量变量的误差作任何假设的方法可能会更好，从而避免错误的模型带来的巨大的估计错误。这样的方法在最小均方误差的意义上可能会有些损失，但它比在一些不通常的情况下因为模型错误造成的巨大错误要好。”互补滤波器就是一种不需要对误差模型做过多假设的方法。

1，一维互补滤波器

假设一个角度信号

有两种测量方法：

(1)

其中

和

是测量噪声。我们希望对

和

作加权平均，从而使平均后的误差尽可能最小。在 IMU 的姿态估计中，如果我们认为

是陀螺仪积分出的角度，那么

在短时间内是准确的，但长时间后因为漂移误差精度会大幅下降，也就是说

主要是低频噪声；如果我们认为

是加速度计通过和重力对比解算出的角度，那么

在短时间内因为运动造成的加速度是不准确的，但 IMU 不可能长时间沿一个方向加速，所以在长时间内的

平均值是准确的，也就是说

主要是高频噪声。因此一个自然的想法是：我们可以把

通过一个高通滤波器，滤掉低频噪声；把

通过一个低通滤波器，滤掉高频噪声，再做平均，那么得到的结果应该是准确的。这就是互补滤波器的核心想法，也是它为什么叫“互补”的原因。

如果我们把

和

作 Laplace 变换写成频域中的信号，并用

表示低通滤波器，对两个信号进行平均可以得到：

(2)

其中

是对需要测量的角度信号

的估计，

是互补的高通滤波器。因为

是角速度积分得到的，因此可以写成：

(3)

其中

是测量得到的包含误差的角速度。把(3)代入(2)可以得到：

(4)

如果我们使用最简单的一阶低通滤波器，也就是

，代入(4)整理并用 Laplace 逆变换回到时域可以得到：

(5)

再将(5)作一阶积分可以得到：

(6)

公式(6)也就是互补滤波器在时域中的表达式。可以看到，互补滤波器实际是对陀螺仪积分得到的角度和加速度计解算出的角度做了加权平均，权重分别是

和

。需要注意的是，权重是不随时间变化的，这是互补滤波器和卡尔曼滤波器的本质区别。直观上讲，因为加速计的噪声特性不随时间改变，但陀螺仪积分得到的角度随着滤波的进行会越来越准确，因此在卡尔曼滤波器中

得到的权重会越来越小，这是卡尔曼滤波器优于互补滤波器的原因。这样简单的一维互补滤波器在 IMU 姿态估计中也有实际应用。例如在两轮平衡车中，我们只需要估计平衡车倾斜的角度，因此可以把姿态简化为一维角度，使用加速度计和陀螺仪进行估计。

2，SO(3) 上的互补滤波器

在无人机的姿态估计中，一个十分流行的算法是 Mahony 等人提出的在 SO(3) 群上的互补滤波器 [2]。这个滤波器的思路是仿照公式(5)的形式，把其中的加减法替换为 SO(3) 群上的运算。

在上一篇文章中，我们给出了旋转矩阵

的导数，

，其中

是 IMU 的角速度，并且表示在 IMU 体坐标系中。仿照公式(5)，我们需要在

的导数中添加一项：通过陀螺仪积分和其它方式测量得到的姿态的差值，乘以一个比例系数。如果我们把

的估计记为

，其它方式测量得到的姿态记为

，那么两者的“差”在 SO(3) 群上可以表示为

(7)

通过指数映射的逆映射，可以得到

的轴-角表示

，即一个

中的量。现在我们可以写出 SO(3) 群上互补滤波器的表达式：

(8)

需要注意的是，

之所以可以像(5)中那样和

相加，是因为它们都是

中的量，而

是一个线性空间。直观的解释是，

和

都可以理解为三维角速度，因此可以相加。在 Mahony 的文章中，

被替换成

，其中

是

的逆运算。这个替换是无关紧要的，因为

和

是平行的，而它们的系数都可以被吸收进比例系数

中。公式(8)可以积分为在 IMU 姿态估计中可以使用的表达式：

(9)

3，总结

在这篇文章中，我们介绍了简单的一维互补滤波器，和比较复杂的在 SO(3) 群上的互补滤波器。互补滤波器的思路是把一个主要包含高频噪声，和一个主要包含低频噪声的信号分别通过一个低通滤波器和高通滤波器，并做平均，从而使平均后的结果是真实信号较为准确的估计。虽然 Mahony 提出的互补滤波器在无人机领域十分流行，但在最小均方误差的意义下，它是劣于卡尔曼滤波器的。从下篇文章开始，我们逐步介绍卡尔曼滤波器在 IMU 姿态估计中的应用。

参考文献

[1] Brown, Robert Grover, and Patrick YC Hwang.Introduction to random signals and applied Kalman filtering. Vol. 3. New York: Wiley, 1992.

[2] Mahony, Robert, Tarek Hamel, and Jean-Michel Pflimlin. "Nonlinear complementary filters on the special orthogonal group."IEEE Transactions on automatic control53.5 (2008): 1203-1217.