四轴mpu6050姿态角卡尔曼滤波代码分析

卡尔曼滤波理解及代码分析

鉴于网上的代码以及分析的各种错误，所以写一个正确的详细的分析。

过程方程以及量测方程

X(K)=AX(K-1)+BU(K-1)+W(K-1)

Z(K)=HX(K)+V(K)

说明，下面带T的表示转置。

• 卡尔曼滤波的黄金五条公式
  
  1. X(k|k-1)=AX(k-1|k-1)+BU(k)……….先验估计
  2. P(k|k-1)=A P(k-1| k-1) AT+Q……….误差协方差
  3. Kg(k)= P(k|k-1) HT / (H P(k|k-1) HT + R)……….计算卡尔曼增益
  4. X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1))……….修正估计
  5. P(k|k)=（ I-Kg(k) H） P(k|k-1)……….更新误差协方差

下面的程序主要针对MPU6050的姿态角的滤波。

float  Q_angle=0.001;    //陀螺仪噪声的协方差
float  Q_gyro=0.003;    //陀螺仪漂移噪声的协方差
float  R_angle=0.5;      // 加速度计的协方差
float  dt=0.005;      
char   C_0 = 1;
float  Q_bias=0, Angle_err=0;  //Q_bias为陀螺仪漂移
float  PCt_0=0, PCt_1=0, E=0;
float  K_0=0, K_1=0, t_0=0, t_1=0;
float  Pdot[4] ={
  0,0,0,0};
float  PP[2][2] = { { 1, 0 },{ 0, 1 } };

首先建立的是过程方程，这里的状态变量是angle以及Q_bias,角度以及陀螺仪的漂移。

那么已经建立了这里的预测方程，没有加上噪声。

void Kalman_Filter(float Gyro,float Accel)    
{    //Gyro陀螺仪的测量值，Accel加速度计的角度计算值

     Angle+=(Gyro - Q_bias) * dt;  
    //角度测量模型方程
    //就漂移来说认为每次都是相同的Q_bias=Q_bias；
    //由此得到矩阵

上面的代码就对应着预测方程。对应着卡尔曼滤波的五个公式的第一条：X(k|k-1)=AX(k-1|k-1)+BU(k)

这里再分析第二条公式，P(k|k-1)=A P(k-1| k-1) AT+Q。可以在之前看出，A=[1,-dt;0,1]。而Q的定义如下：

因为角度噪声和陀螺仪的角速度的漂移噪声相互独立，所以为一个对角矩阵。然后，Q_angle,Q_gyro再程序开头已经给出。所以设P=[a,b;c,d]

的出一个更新的式子，

[acbd]=[10−