一阶互补滤波
基本理解与对应公式
- 加速度计数据比较震荡,含有较多高频分量,陀螺仪积分得到角度值,但是时间长了之后会有零漂,因此需要对两者数据进行融合。相当于低通滤波器,滤去加速度计得到的角度的高频分量,同时去除陀螺仪的零漂。
示例代码
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
* 输入
* dt:两次采样时间间隔
* tau:反应惯性滤波的系数
* angleout:上次角度输出值
* newangle:角速度计得到的角度值
* newgyro:陀螺仪的角速度值
* 输出
* 互补滤波计算得到的角度值
优缺点
- 优点:有效去除加速度计数据的抖动
- 缺点:当惯性滤波对于过去的值有较大的权重时,即上式中K值很小时,系统会出现较大的之后效应,当K=0时,完全采用新的加速度计的值,得到的数据抖动较大。
kalman滤波
基本理解与对应公式
- 先验预测值
- X(k+1|k)=A X(k|k)+B U(k)
- 先验预测方差
- P(k+1|k)=A P(k|k) A’+Q(k)
- kalman增益矩阵的计算
- 首先计算量测预测值(这一步好像没有必要,可以省略。。)
- Z(k+1|k) = H X(k+1|k)
- 增益矩阵
- Kg(k+1)= P(k+1|k) H’/ (H P(k+1|k) H’ + R(k+1))
- 首先计算量测预测值(这一步好像没有必要,可以省略。。)
- 后验预测值
- X(k+1|k+1)= X(k+1|k)+Kg(k+1) (Z(k+1)-H X(k+1|k))
- 后验预测方差
- P(k+1|k+1)=(I-Kg(k+1) H)P(k+1|k)
示例代码
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 程序解释
- X(k+1|k) = [angleout , gyro]’,由上一次的角度值和测量得到的角速度组成的列向量
- A = [1 dt ; 0 1] , 2X2的数组
- Q = [Q_angle , 0 ; 0 , Q_gyro] 2X2的协方差数组,表示过程噪声
- R = R_angle:测量噪声
- H = [1 0]’,一个列向量,表示测量参数的矩阵,大概是只需要观测1个角度值,因此只有一维数组即可
- Z(k+1),最近一次的测量值,此处是加速度计直接得到的角度
优缺点
- 优点:逐渐收敛
- 缺点:计算量太大