说透互补滤波(1) - 线性互补滤波器从原理到实现

说透互补滤波(1) - 线性互补滤波器从原理到实现

为什么开源代码看不懂？

说起互补滤波，之前非常的流行，在那个算力不够的年代，这种短小精干的融合算法，风靡一时。

原理也非常简单：

我有两路信号，一个带有高频噪声，一个带有低频噪声，所以我把他们的噪声分别滤除，然后合并，就得到了没有噪声的原始信号。

简单直接，清晰明了，看到这你顿时觉得这个算法你已经会了，甚至连算法的流程都设计好了。

于是你在网上找到了著名的 Madgwick实现的 MahonyAHRS 算法，准备对比一下自己的思路。

function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
    q = obj.Quaternion; 
    if(norm(Accelerometer) == 0), return; end  
    Accelerometer = Accelerometer / norm(Accelerometer);
    v = [2*(q(2)*q(4) - q(1)*q(3))
         2*(q(1)*q(2) + q(3)*q(4))
         q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
    e = cross(Accelerometer, v);
    if(obj.Ki > 0)
        obj.eInt = obj.eInt + e * obj.SamplePeriod;   
    else
        obj.eInt = [0 0 0];
    end            
    Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;            
    qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
    q = q + qDot * obj.SamplePeriod;
    obj.Quaternion = q / norm(q); 
end

你看着开源的代码和自己画的流程图，反复对照了20遍，突然想到了自己上数学课的样子。

于是你又上网看了无数的互补滤波解读教程，始终不理解，为什么算法原理和代码可以没有任何关系？，那这个算法是怎么写成代码的呢？

我直接给出结论吧，造成这样的原因是因为：

网上大部分互补滤波原理介绍的是传统的线性互补滤波(Classical Complementary Filters)，

而Mahony用来算解姿态的滤波是经过改进的非线性互补滤波，

非线性互补滤波里有两种形式：直接互补滤波(Direct complementary filter)和无源互补滤波(Passive complementary filter)，

你在网上看到的开源代码都是基于无源互补滤波器的显式误差版本-显式互补滤波器(Explicit complementary filter).

你拿着两个完全不一样的东西，那肯定对应不上呀。

先从传感器开始说起

但是我就纳闷了，为啥就没人循序渐进的写清楚呢？既然没人那就我来吧。

我有两路姿态，一个带有高频噪声，一个带有低频噪声，所以我把他们的噪声分别滤除，然后合并，就得到了没有噪声的姿态信息。

我有哪两路姿态信息呢？

但凡你在网络上查过资料，他们一定会告诉你，一路陀螺仪的获取的姿态，一路加计获取的姿态。

真的是这样吗？

那我们先从陀螺仪开始吧，陀螺仪可以获得三轴角速度，积分一下就可以得到角度，在小角度假设下，这个角度可以认为就是姿态角。

思路没有任何问题，只是我们可能缺个初始状态。

我们求速度的时候是：

$$v=v_0 + at$$

那我们求角度的时候自然是：

所以如果我们却个初始状态angle0，陀螺仪只有在发生转动的时候才能有数据，所以它是无法提供这个初始状态angle0的。

没关系,起飞前的初始状态非常好分析，因为这时候飞机是受力平衡的，也就是说在地理系下的加速度的读数是[0;0;g]

所以我们可以列出方程：

得到：

那么根据加计得到的初始的roll，pitch角度正确时，我们可以通过公式，得到yaw：

这样，我们有了angle0 和 w ,终于和通过陀螺仪来获取姿态了。

ps:详细推导思路参考：《基于加速度计与磁力计的姿态解算方法》

等等，这样的话我们不就只有一路输入了吗？怎我三个传感器用完才能得到一路姿态呀？

加计不是可以求姿态吗？这时我才想起来，网上流传的加计求姿态公式，其实只能在飞机受力平衡的时候使用，当飞机受力不平衡，飞机在地理系受到的加速度不是[0;0;g]，而是个未知数，上面的方程是没有解的。

连两路输入都没有，还怎么互补滤波？

必要的假设

所以在讨论互补滤波器之前我们要做出几个假设：

1.姿态的更新是线性的即满足公式

2.飞行过程基本受力平衡，接近匀速直线运动，或者悬停，即飞机在地理系下的加计读数为[0;0;g]

3.传感器的测量数据只涉及高频或者低频噪声，即，传感器测量方程如下：

$$\begin{aligned} acc_m(t) &= acc(t) + n_H(t) \\ \omega _m(t) &= \omega (t) + n_L(t) \\ \end{aligned}$$

m下标为传感器测量值，等于真实值加上噪声，所以可以推导出，传感器测量出的角度也满足以下测量方程。

$$\begin{aligned} acc\_ang_m(t) &= acc\_ang_(t) + n_H(t)\\ \omega\_ang_m(t) &= \omega\_ang_(t) + n_L(t) \end{aligned}$$

4.假设初始欧拉角为[0;0;0]

所以传统的线性互补滤波结构如下。

从低通滤波器开始分析

低通滤波器是我们比较熟悉的，之前我们分析过一阶低通滤波器，但是低通滤波器有很多种，为了讨论不同的状态，令低通滤波器函数为LPF。

那高通滤波器是什么呢？这里我们的低通滤波和高通滤波合并后希望能够通过完整的波形，也就是波形完全不变，那这个全通的函数其实就是1，所以我们高通滤波器就可以设计为1-LPF。

以我们最熟悉的一阶低通滤波器为例，它的函数为：

那高通滤波就是：

那这个结论对不对呢？令截止频率wc=1HZ，画出两个函数的伯德图，完全符合预期，一个低通一个高通且截止评率1hz.

ps:一阶滤波器详细分析参考《一阶RC低通滤波算法》

我们再试试二阶滤波器，二阶低通滤波器函数为：

二阶高通滤波器为：

令所有系数为1，a1=a2=a3=b1=b2=b3=1,画出滤波器的伯德图

结果也是符合预期的。

所以，可以看见很过论文把这个过程进行了总结，给出了通用的低通滤波函数：

当C(s) = 常数时，对应的就是一阶滤波器。

当C(s) = a+ b/s 时，对应的就是二阶滤波器 。

(二阶的这个形式正好和kp,ki形式一致，很多地方都会强调这里的KP，Ki有什么做用，又是消除误差，又是调整截止频率，但是我认为在线性互补滤波器中，这里的参数的作用就是构成二阶滤波器，这里的参数也仅仅是二阶滤波器的参数)

所以我们也用这种通用形式来继续分析。

根据框图可得：

是不是有内味了，再根据这个公式，把框图化简成反馈形式。

这就是论文里经常看见的框图，我们来对比一下

是不是一模一样。

那为什么要把它转变成反馈形式呢？这种反馈结构简化了滤波器在代码上的实现过程，同时可以地适应更多的测量，可以很方便的扩展结构如下图：

什么时候会需要扩展呢？例如当你有GPS的时候：

算法实现

ok,我们终于把线性互补滤波器的原理说完了，那就把它实现了吧。

之前我们已经很详细的分析过低通滤波器了，这次还是以一阶滤波器的形式实现。

令C=1/τ

一阶向后差分：

离散化

转换为离散时间的差分形式

截止频率为：

用MATLAB实现算法如下：

function obj = UpdateIMU(obj, w_m,angle_m )
   obj.angle =  (obj.tao/(obj.tao + obj.Ts)) * ( obj.last_angle + obj.Ts*w_m) 
   			+ (obj.Ts/(obj.tao + obj.Ts)) * angle_m;
   obj.last_angle = obj.angle;
end

仿真对比

找一组数据验证一下：

可以看到直接通过加计得到的图形确实噪声很大，符合加计容易受噪声影响的特点。

零初始状态只用陀螺仪积分得到的陀螺仪，则非常平滑，无法感知到高频部分的数据，但是静止时得到的角度有偏离，不是0度，符合陀螺仪动态响应效果好的特点。

传统互补滤波器和Mahony滤波器确实都能够结合两者的特点，在15s处能得到更多的姿态信息。

ps:Mahony算法和测试数据来源于https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

思考

让我们再看一眼，滤波器的框图：

这个框图可以很直接的看到，核心思路就是如果获取一个更精确的角速度，因为本质上我们想使用的就是角速度积分的到姿态，而角速度积分得到姿态就是姿态更新的线性化过程，我想这就是为什么这叫线性互补滤波器的原因，因为一切的起因都这个线性姿态更新算法。

关于经典的线性互补滤波器的原理，算法，结果测试已经全部讲完了，下一次我们就要开始非线性互补滤波器了，算法的改进往往是把之前算法的假设修改的更贴近与实际。

ok，今天就讲这么多，我是zing,一个有趣的飞控算法工程师，我们下期见。

PS：所有的论文资料和测试代码，公众号回复【互补滤波】即可获取。

$$R-R^{T}=\left[\begin{array}{ccc} \cos \theta+u_{x}^{2}(1-\cos \theta) & u_{x} u_{y}(1-\cos \theta)-u_{z} \sin \theta & u_{x} u_{z}(1-\cos \theta)+u_{y} \sin \theta \\ u_{y} u_{x}(1-\cos \theta)+u_{z} \sin \theta & \cos \theta+u_{y}^{2}(1-\cos \theta) & u_{y} u_{z}(1-\cos \theta)-u_{x} \sin \theta \\ u_{z} u_{x}(1-\cos \theta)-u_{y} \sin \theta & u_{z} u_{y}(1-\cos \theta)+u_{x} \sin \theta & \cos \theta+u_{z}^{2}(1-\cos \theta) \end{array}\right] \\-\left[\begin{array}{ccc} \cos \theta+u_{x}^{2}(1-\cos \theta) & u_{x} u_{y}(1-\cos \theta)+u_{z} \sin \theta & u_{x} u_{z}(1-\cos \theta)-u_{y} \sin \theta \\ u_{y} u_{x}(1-\cos \theta)-u_{z} \sin \theta & \cos \theta+u_{y}^{2}(1-\cos \theta) & u_{y} u_{z}(1-\cos \theta)+u_{x} \sin \theta \\ u_{z} u_{x}(1-\cos \theta)+u_{y} \sin \theta & u_{z} u_{y}(1-\cos \theta)-u_{x} \sin \theta & \cos \theta+u_{z}^{2}(1-\cos \theta) \end{array}\right] \\ = \left[\begin{array}{ccc} 0 & -2 u_{z} \sin \theta & 2 u_{y} \sin \theta \\ 2 u_{z} \sin \theta & 0 & -2 u_{x} \sin \theta \\ -2 u_{y} \sin \theta & 2 u_{x} \sin \theta & 0 \end{array}\right]$$