陀螺仪和加速度计互补滤波的一些推导

1、首先,简单介绍一下加速度计和陀螺仪的一些特性。

加速度计:静态稳定性好,动态响应较慢,在高频时信号不可用,在运动时其数据相对不可靠。

陀螺仪:   动态性能好,响应快,积分后可测倾角,在低频段信号不好,存在零漂现象。

2、一维互补滤波算法推导

首先,直接给出两个传感器测量表达式:

\left.\begin{matrix} z_{1}=x+u & \\ z_{2}=x+v& \end{matrix}\right\}

其中,x为真值,设u,v为零均值量测噪声,u为高频噪声,v为低频噪声,z_{1},z_{2}均为传感器测量值。

接着,设x的估计值为\hat{x},按照线性假设有

\hat{x}=k_{1}z_{1}+k_{2}z_{2}

其中,k_{1},k_{2}为待定的线性加权系数。

设估计偏差为\tilde{x},其中\tilde{x}=\hat{x}-x,根据无偏估计,有

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;

可推得{k_{1}+k_{2}-1=0},    即       k_{1}+k_{2}=1

以上可以理解为传统的WLS。

【注】这里如果知道量测噪声是符合高斯分布的话,直接可以算偏差的方差最小,求得估计值跟其噪声的方差的关系。

下面假设k_{1}(s),k_{2}(s)分别为低通(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)

整理上式子

fs\hat{x}(s)+\hat{x}(s)=z_{1}(s)+fsz_{2}(s)

反变换有

f\dot{\hat{x}}(t)+\hat{x}(t)=z_{1}(t)+f\dot{\hat{z_{2}}}(t)

离散处理

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}

整理过程

f\hat{x}(k)-f\hat{x}(k-1)+T\hat{x}(k)=Tz_{1}(k)+fz_{2}(k)-fz_{2}(k-1)

(f+T)\hat{x}(k)=f\hat{x}(k-1)+fz_{2}(k)-fz_{2}(k-1)+Tz_{1}(k)

得到有

\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)

a=\frac{T}{f+T}

则有

\hat{x}(k)=(1-a)[\hat{x}(k-1)+z_{2}(k)-z_{2}(k-1)]+az_{1}(k)

如果z_{1}(k),z_{2}(k)分别是加速度计(反正切得到的角度)和陀螺仪(积分出来后角度)的测量值,则算法有

angle=(1-a)(anglelast+Gyro\cdot dt)+a\cdot Acc

其中,Gyro为角速度,Acc为角度。

3、取值信任度问题

这里取值问题将涉及一个信任度的问题,即我们无人机在何种状态信任加速度计和陀螺仪的一个权值大小,比如悬停状态。

陀螺仪和加速度计互补滤波的一些推导_第1张图片

3、二阶互补滤波

二阶低通滤波器传递函数

LPF=\frac{a_{2}s+a_{3}}{b_{1}s^{2}+b_{2}s+b3}

二阶高通滤波器传递函数

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}

陀螺仪和加速度计互补滤波的一些推导_第2张图片

\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)

emmm暂时就先总结到这把.........

 

你可能感兴趣的:(陀螺仪,加速度计,WLS,互补滤波,无人机)