1、首先,简单介绍一下加速度计和陀螺仪的一些特性。
加速度计:静态稳定性好,动态响应较慢,在高频时信号不可用,在运动时其数据相对不可靠。
陀螺仪: 动态性能好,响应快,积分后可测倾角,在低频段信号不好,存在零漂现象。
2、一维互补滤波算法推导
首先,直接给出两个传感器测量表达式:
![\left.\begin{matrix} z_{1}=x+u & \\ z_{2}=x+v& \end{matrix}\right\}](https://private.codecogs.com/gif.latex?%5Cleft.%5Cbegin%7Bmatrix%7D%20z_%7B1%7D%3Dx+u%20%26%20%5C%5C%20z_%7B2%7D%3Dx+v%26%20%5Cend%7Bmatrix%7D%5Cright%5C%7D)
其中,
为真值,设
为零均值量测噪声,
为高频噪声,
为低频噪声,
均为传感器测量值。
接着,设
的估计值为
,按照线性假设有
![\hat{x}=k_{1}z_{1}+k_{2}z_{2}](https://private.codecogs.com/gif.latex?%5Chat%7Bx%7D%3Dk_%7B1%7Dz_%7B1%7D+k_%7B2%7Dz_%7B2%7D)
其中,
为待定的线性加权系数。
设估计偏差为
,其中
,根据无偏估计,有
![E[\tilde{x}]=E[\hat{x}-x] =E[{k_{1}(x+u)+k_{2}(x+v)-x}] =({k_{1}+{k_{2}-1)E(x) =({k_{1}+{k_{2}-1)x=0;](https://private.codecogs.com/gif.latex?E%5B%5Ctilde%7Bx%7D%5D%3DE%5B%5Chat%7Bx%7D-x%5D%20%3DE%5B%7Bk_%7B1%7D%28x+u%29+k_%7B2%7D%28x+v%29-x%7D%5D%20%3D%28%7Bk_%7B1%7D+%7Bk_%7B2%7D-1%29E%28x%29%20%3D%28%7Bk_%7B1%7D+%7Bk_%7B2%7D-1%29x%3D0%3B)
可推得
, 即 ![k_{1}+k_{2}=1](https://private.codecogs.com/gif.latex?k_%7B1%7D+k_%7B2%7D%3D1)
以上可以理解为传统的WLS。
【注】这里如果知道量测噪声是符合高斯分布的话,直接可以算偏差的方差最小,求得估计值跟其噪声的方差的关系。
下面假设
分别为低通(LPF)、高通滤波器(HPF)的传递函数,则有
![\hat{x}(s)=k_{1}(s)z_{1}(s)+k_{2}(s)z_{2}(s)=\frac{1}{fs+1}z_{1}(s)+\frac{fs}{fs+1}z_{2}(s)](https://private.codecogs.com/gif.latex?%5Chat%7Bx%7D%28s%29%3Dk_%7B1%7D%28s%29z_%7B1%7D%28s%29+k_%7B2%7D%28s%29z_%7B2%7D%28s%29%3D%5Cfrac%7B1%7D%7Bfs+1%7Dz_%7B1%7D%28s%29+%5Cfrac%7Bfs%7D%7Bfs+1%7Dz_%7B2%7D%28s%29)
整理上式子
![fs\hat{x}(s)+\hat{x}(s)=z_{1}(s)+fsz_{2}(s)](https://private.codecogs.com/gif.latex?fs%5Chat%7Bx%7D%28s%29+%5Chat%7Bx%7D%28s%29%3Dz_%7B1%7D%28s%29+fsz_%7B2%7D%28s%29)
反变换有
![f\dot{\hat{x}}(t)+\hat{x}(t)=z_{1}(t)+f\dot{\hat{z_{2}}}(t)](https://private.codecogs.com/gif.latex?f%5Cdot%7B%5Chat%7Bx%7D%7D%28t%29+%5Chat%7Bx%7D%28t%29%3Dz_%7B1%7D%28t%29+f%5Cdot%7B%5Chat%7Bz_%7B2%7D%7D%7D%28t%29)
离散处理
![f\frac{\hat{x}(k)-\hat{x}(k-1)}{T}+\hat{x}(k)=z_{1}(k)+f\frac{z_{2}(k)-z_{2}(k-1)}{T}](https://private.codecogs.com/gif.latex?f%5Cfrac%7B%5Chat%7Bx%7D%28k%29-%5Chat%7Bx%7D%28k-1%29%7D%7BT%7D+%5Chat%7Bx%7D%28k%29%3Dz_%7B1%7D%28k%29+f%5Cfrac%7Bz_%7B2%7D%28k%29-z_%7B2%7D%28k-1%29%7D%7BT%7D)
整理过程
![f\hat{x}(k)-f\hat{x}(k-1)+T\hat{x}(k)=Tz_{1}(k)+fz_{2}(k)-fz_{2}(k-1)](https://private.codecogs.com/gif.latex?f%5Chat%7Bx%7D%28k%29-f%5Chat%7Bx%7D%28k-1%29+T%5Chat%7Bx%7D%28k%29%3DTz_%7B1%7D%28k%29+fz_%7B2%7D%28k%29-fz_%7B2%7D%28k-1%29)
![(f+T)\hat{x}(k)=f\hat{x}(k-1)+fz_{2}(k)-fz_{2}(k-1)+Tz_{1}(k)](https://private.codecogs.com/gif.latex?%28f+T%29%5Chat%7Bx%7D%28k%29%3Df%5Chat%7Bx%7D%28k-1%29+fz_%7B2%7D%28k%29-fz_%7B2%7D%28k-1%29+Tz_%7B1%7D%28k%29)
得到有
![\hat{x}(k)=\frac{f}{f+T}[\hat{x}(k-1)+z_{2}(k)-z_{2}(k-1)]+\frac{T}{f+T}z_{1}(k)](https://private.codecogs.com/gif.latex?%5Chat%7Bx%7D%28k%29%3D%5Cfrac%7Bf%7D%7Bf+T%7D%5B%5Chat%7Bx%7D%28k-1%29+z_%7B2%7D%28k%29-z_%7B2%7D%28k-1%29%5D+%5Cfrac%7BT%7D%7Bf+T%7Dz_%7B1%7D%28k%29)
令
![a=\frac{T}{f+T}](https://private.codecogs.com/gif.latex?a%3D%5Cfrac%7BT%7D%7Bf+T%7D)
则有
![\hat{x}(k)=(1-a)[\hat{x}(k-1)+z_{2}(k)-z_{2}(k-1)]+az_{1}(k)](https://private.codecogs.com/gif.latex?%5Chat%7Bx%7D%28k%29%3D%281-a%29%5B%5Chat%7Bx%7D%28k-1%29+z_%7B2%7D%28k%29-z_%7B2%7D%28k-1%29%5D+az_%7B1%7D%28k%29)
如果
分别是加速度计(反正切得到的角度)和陀螺仪(积分出来后角度)的测量值,则算法有
![angle=(1-a)(anglelast+Gyro\cdot dt)+a\cdot Acc](https://private.codecogs.com/gif.latex?angle%3D%281-a%29%28anglelast+Gyro%5Ccdot%20dt%29+a%5Ccdot%20Acc)
其中,Gyro为角速度,Acc为角度。
3、取值信任度问题
这里取值问题将涉及一个信任度的问题,即我们无人机在何种状态信任加速度计和陀螺仪的一个权值大小,比如悬停状态。
![](https://img-blog.csdnimg.cn/2020061923025297.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L01yX2RoeQ==,size_16,color_FFFFFF,t_70)
3、二阶互补滤波
二阶低通滤波器传递函数
![LPF=\frac{a_{2}s+a_{3}}{b_{1}s^{2}+b_{2}s+b3}](https://private.codecogs.com/gif.latex?LPF%3D%5Cfrac%7Ba_%7B2%7Ds+a_%7B3%7D%7D%7Bb_%7B1%7Ds%5E%7B2%7D+b_%7B2%7Ds+b3%7D)
二阶高通滤波器传递函数
![HPF=1-LPF=1-\frac{a_{2}s+a_{3}}{b_{1}s^{2}+b_{2}s+b3}=\frac{a_{1}s^{2}}{b_{1}s^{2}+b_{2}s+b3}](https://private.codecogs.com/gif.latex?HPF%3D1-LPF%3D1-%5Cfrac%7Ba_%7B2%7Ds+a_%7B3%7D%7D%7Bb_%7B1%7Ds%5E%7B2%7D+b_%7B2%7Ds+b3%7D%3D%5Cfrac%7Ba_%7B1%7Ds%5E%7B2%7D%7D%7Bb_%7B1%7Ds%5E%7B2%7D+b_%7B2%7Ds+b3%7D)
![](https://img-blog.csdnimg.cn/20200621144849271.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L01yX2RoeQ==,size_16,color_FFFFFF,t_70)
![\hat{x}(s)=k_{1}(s)z_{1}(s)+k_{2}(s)z_{2}(s)=\frac{a_{2}s+a_{3}}{b_{1}s^{2}+b_{2}s+b3}z_{1}(s)+\frac{a_{1}s^{2}}{b_{1}s^{2}+b_{2}s+b3}z_{2}(s)](https://private.codecogs.com/gif.latex?%5Chat%7Bx%7D%28s%29%3Dk_%7B1%7D%28s%29z_%7B1%7D%28s%29+k_%7B2%7D%28s%29z_%7B2%7D%28s%29%3D%5Cfrac%7Ba_%7B2%7Ds+a_%7B3%7D%7D%7Bb_%7B1%7Ds%5E%7B2%7D+b_%7B2%7Ds+b3%7Dz_%7B1%7D%28s%29+%5Cfrac%7Ba_%7B1%7Ds%5E%7B2%7D%7D%7Bb_%7B1%7Ds%5E%7B2%7D+b_%7B2%7Ds+b3%7Dz_%7B2%7D%28s%29)
emmm暂时就先总结到这把.........
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)