一阶互补滤波
基本理解与对应公式
- 加速度计数据比较震荡,含有较多高频分量,陀螺仪积分得到角度值,但是时间长了之后会有零漂,因此需要对两者数据进行融合。相当于低通滤波器,滤去加速度计得到的角度的高频分量,同时去除陀螺仪的零漂。
示例代码
double CompFilter( double angleout , double newangle , double newgyro , double dt ,float tau )
{
float K;
//tau = 0.075;
K = tau / ( tau + dt );
angleout = K * (angleout + newgyro * dt) + (1-K) * newangle; //互补滤波加惯性滤波
return angleout;
}
* 输入
* 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)
示例代码
//之前需要定义的变量
float Q_angle = 0.01; //0.001
float Q_gyro = 0.0003; //0.003
float R_angle = 0.01; //0.03
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S; float K_0, K_1;
double KalmanFilter(double angleout , double newAngle, double newRate,double dt)
{
angleout += dt * (newRate - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt; //在这里忽略了(dt)^2这个高阶项
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;
y = newAngle - angleout;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
angleout += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return angleout;
}
- 程序解释
- 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),最近一次的测量值,此处是加速度计直接得到的角度
优缺点
- 优点:逐渐收敛
- 缺点:计算量太大