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



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



过程方程以及量测方程

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

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

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

卡尔曼滤波的黄金五条公式 
X(k|k-1)=AX(k-1|k-1)+BU(k)……….先验估计
P(k|k-1)=A P(k-1| k-1) AT+Q……….误差协方差
Kg(k)= P(k|k-1) HT / (H P(k|k-1) HT + R)……….计算卡尔曼增益
X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1))……….修正估计
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−dt1][acbd][1−dt01]+[Qangle00Qgyro]


最后的到的更新的方法 
[acbd]=[a−c∗dt−b∗dt+d∗dt∗dtc−d∗dtb−d∗dtd]+[Qangle00Qgyro]


所以看代码，可以看出写的极为的不合理，但是都是这样写的，先看一看。



    Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; 
    Pdot[1] = -PP[1][1];
    Pdot[2] = -PP[1][1];
    Pdot[3] = Q_gyro;

    PP[0][0] += Pdot[0] * dt;   
    PP[0][1] += Pdot[1] * dt;   
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;

对照上面的公式，还是可以看出来，PP就是[a,b;c,d]的，但是注意，Pdot只是矩阵运算的中间值，但是不知为什么要叫成Pdot，误人子弟。而且最大的错误在于这样写，Q乘以了一个dt，但是最后并不会怎么影响，因为Q也是初始给的一个值而已，但是这样写还是有问题的，还是按照推导来写比较好。

再是第三个公式来计算卡尔曼增益，Kg(k)= P(k|k-1) HT / (H P(k|k-1) HT + R)，所以这里要做的就是再建立一个量测方程，这里测量的值是加速度计算出来的角度值，所以 
Accelangle=[10][angleQbias]+R


所以H=[1 0],卡尔曼增益就是一个二维向量[k0,k1]T。

直接带入计算第三个公式。



    PCt_0 = C_0 * PP[0][0];//矩阵乘法的中间变量
    PCt_1 = C_0 * PP[1][0];//C_0=1
    //分母
    E = R_angle + C_0 * PCt_0;
    //增益值
    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;    

基本还算比较清楚，但是命名的话真的不忍心吐槽。

再看第四个公式，X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1)).



    Angle_err = Accel - Angle;   //Accel是加速度计的值，算出来的角度的测量值。

    Angle    += K_0 * Angle_err;    //对状态的卡尔曼估计。
    Q_bias    += K_1 * Angle_err;    

    Gyro_x   = Gyro - Q_bias;     //计算得角速度值，这里由于每次对Q_bias更新，就更准确，比初始矫正后不管肯定要好很多。

第五个公式对PP进行更新，P(k|k)=（ I-Kg(k) H） P(k|k-1)；



    t_0 = PCt_0; //矩阵计算中间变量
    t_1 = C_0 * PP[0][1];

    PP[0][0] -= K_0 * t_0;        
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;
}

已经不忍心吐槽他的命名了。到这里基本分析完毕，至于卡尔曼滤波的证明推导，可以参考其他，这里只是分析代码，但是网上基本都是这样写的，有分析的，但是要么就是直接甩代码，要么就是分析的很多错误，太乱，决定写一个完整的正确的分析mpu6050卡尔曼滤波的程序。

n = length(x);%输入信号长度

%设置参数

y(1,1)=x(1,1); %初始值

p(1)=0; %误差协方差初始值

R=2^-4;%高斯分布的测量噪声

Q=2^-6;%高斯分布的过程噪声

for i=2:n

    k(i,1)=p(i-1)/(p(i-1)+R);

    y(i,1)=y(i-1)+k(i)*(x(i)-y(i-1));

    p(i,1)=(1-k(i,1))*p(i-1)+Q;

end

%y为输出信号，滤波效果



 

工程介绍
工程使用stm32f407开发，imu单元为ADIS16465-1 BMLZ,通过spi通信读取imu的输出寄存器，对读取到的三轴加速度值和三轴陀螺仪值利用四元数进行姿态解算，最后得到pitch和roll。由于只通过六轴传感器无法得到yaw，工程中只是对陀螺仪输出的角速度进行了简单的积分，误差较大。
学习资料
四元数原理参考网上：https://blog.csdn.net/guanjianhe/article/details/95608801，把博主的这篇文章从上到下推导一遍基本就明白了。
工程中还有卡尔曼滤波的代码，但是在姿态解算中并未用到，有需要的可以自己学习一下。
加速度计、陀螺仪原理以及卡尔曼滤波原理参考：https://www.arduino.cn/thread-18392-1-1.html
代码下载
网盘下载链接：https://pan.baidu.com/s/14ekDutUUJU_7skPqi0qe-Q

提取码：h2dd
github下载
有关四元数、ADIS16465、陀螺仪加速度计、卡尔曼滤波原理的文档，我整合了一些，放在另一个repository：ADIS16465-version1
博文链接
代码移植的时候可能会有bug，只能满足基本需要，仅供参考，敬请原谅。
结果
ADIS16465图片



IMU静止时输出



绕x轴旋转输出



绕y轴旋转输出



绕z轴旋转输出



转载请说明出处哦~希望对你有帮助！

struct _1_ekf_filter
{
    float LastP;
    float   Now_P;
    float out;
    float Kg;
    float Q;
    float R;    
};

void kalman_1(struct _1_ekf_filter *ekf,float input)  
{
    ekf->Now_P = ekf->LastP + ekf->Q;
    ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
    ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
    ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
}


//使用范例
static struct _1_ekf_filter ekf = {0.02,0,0,0,0.001,0.543};//初始化参数
float pMpu;
kalman_1(&ekf,(float)pMpu);  
pMpu = (int16_t)ekf.out;