【卡尔曼滤波程序理解】

前言

主要讲解当初做飞卡时，直立所用的卡尔曼滤波，本文章只涉及少量理论，主要是公式推导和程序讲解，建议大家事先了解卡尔曼滤波的效果及公式意义。

一. 卡尔曼滤波主要公式

首先是状态方程和观测方程：

  x(k) = A · x(k-1) + B · u(k) + w(k)            
  z(k) = H · x(k) + y(k)
    
    x(k) —— k时刻系统的状态
    u(k) —— 控制量
    w(k) —— 符合高斯分布的过程噪声，其协方差在下文中为Q
    z(k) —— k时刻系统的观测值
    y(k) —— 符合高斯分布的测量噪声，其协方差在下文中为R
    A、B、H —— 系统参数，多输入多输出时为矩阵，单输入单输出时就是几个常数
 

接下来的两个步骤：预测和修正。预测是根据前一时刻迭代的结果，即x(k-1|k-1)和P(k-1|k-1)，来预测这一时刻的系统状态和误差协方差，得到x(k|k-1)和P(k|k-1)：

 x(k|k-1) = A · x(k-1|k-1) + B · u(k)
 P(k|k-1) = A · P(k-1|k-1) + Q 

  然后计算卡尔曼增益K(k)，和这一次的实际测量结果z(k)一起，用于修正系统状态x(k|k-1)及误差协方差P(k|k-1)，得到最新的x(k|k)和P(k|k)：

  K(k) = P(k|k-1) ·  ·
  x(k|k) = x(k|k-1) + K(k) · (z(k) - H · x(k|k-1))
  P(k|k) = (I - K(k) · H) · P(k|k-1) 

I为对角阵，H在下文中有介绍

二. 程序源码

//卡尔曼滤波参数与函数
float dt=0.001;                //注意：dt的取值为kalman滤波器采样时间
float angle, angle_dot;        //角度和角速度
float P[2][2] = {{ 1, 0 },     //卡尔曼增益
                 { 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1; 
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//卡尔曼滤波
float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
{
        angle+=(gyro_m-q_bias) * dt;
        angle_err = angle_m - angle;
        Pdot[0]=Q_angle - P[0][1] - P[1][0];
        Pdot[1]=- P[1][1];
        Pdot[2]=- P[1][1];
        Pdot[3]=Q_gyro;
        P[0][0] += Pdot[0] * dt;
        P[0][1] += Pdot[1] * dt;
        P[1][0] += Pdot[2] * dt;
        P[1][1] += Pdot[3] * dt;
        PCt_0 = C_0 * P[0][0];
        PCt_1 = C_0 * P[1][0];
        E = R_angle + C_0 * PCt_0;
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
        t_0 = PCt_0;
        t_1 = C_0 * P[0][1];
        P[0][0] -= K_0 * t_0;
        P[0][1] -= K_0 * t_1;
        P[1][0] -= K_1 * t_0;
        P[1][1] -= K_1 * t_1;
        angle += K_0 * angle_err; //最优角度
        q_bias += K_1 * angle_err;
        angle_dot = gyro_m-q_bias;//最优角速度
 
    
}

三.程序实现

 1.建立角度观测模型

angle += (gyro_m-q_bias) * dt

换个写法

angle = angle + (gyro_m-q_bias) * dt

 很容易理解，角度估计值=上次角度估计值+（当前角速度-角速度漂移）*dt

对于漂移来说我们认为每次漂移都一样，有

q_bias = q_bias

2.状态预测

我们将角度观测模型angle = angle + (gyro_m-q_bias) * dt 转换成矩阵形式。

令    ,有

状态预测方程为 x(k|k-1) = A · x(k-1|k-1) + B · u(k)，对比上式可以得到：

,,我们将A称为状态转移矩阵，B称为控制矩阵。

3.协方差矩阵预测

 协方差矩阵预测方程为： P(k|k-1) = A · P(k-1|k-1) · + Q

A矩阵我们已经知道了，他的转置也可以求得，在程序中P(k|k-1)我们表示为。

接下来是过程噪声协方差矩阵Q，我们假设角度和角速度的方差相互独立，所以副对角线上的元素为0。

 下面我们推导下协方差矩阵预测方程 P(k|k-1) = A · P(k-1|k-1) · + Q

计算下矩阵，这里我就不列出过程了，感兴趣的小伙伴可以自己推导下，直接给结果

对应程序

Pdot[0] =  Q_angle - P[0][1] - P[1][0];
Pdot[1] =- P[1][1];
Pdot[2] =- P[1][1];
Pdot[3] =  Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

 4.计算卡尔曼增益

公式为   K(k) = P(k|k-1) ·  ·

开始推导

                                       

这里还是直接给出结果

                                       

对应程序

        PCt_0 = C_0 * P[0][0];
        PCt_1 = C_0 * P[1][0];
        E = R_angle + C_0 * PCt_0;
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;

程序中C_0 = 1，K_0,K_1为卡尔曼增益，其实就是，大家可以带进去自己验证一下。

5.状态更新

状态更新方程为：x(k|k) = x(k|k-1) + K(k) · (z(k) - H · x(k|k-1))，其中 y = (z(k) - H · x(k|k-1)),叫做残差。就是实际观测值和预测值的差，而预测值前乘了一个观测矩阵H。H代表真实状态和预测状态的一个变换关系。在我用的这个实际系统中，观测量只有一个轴的角度，是个一维的量。而我们的状态矩阵是二维的，所以计算时必须将Xk,k-1转换成观测量下的一维。所以。

先计算残差，       y = Zk - ，等于观测值-估计值

再计算Xk ,           

.对应程序

        angle_err = angle_m - angle;
        angle  += K_0 * angle_err; //最优角度
        q_bias += K_1 * angle_err;

6.协方差矩阵更新

协方差更新方程为： P(k|k) = (I - K(k) · H) · P(k|k-1) 

这里还是矩阵带入，大家感兴趣的话自己算一下，我直接给出结果

对应程序

    t_0 = PCt_0;
    t_1 = C_0 * P[0][1];

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

以上程序只不过拆分成几步运算了，大家可以带进去验算一下，也可以直接整合成一个式子在程序中体现出来。

总结

 参考

卡尔曼滤波-实战IMU姿态估计 - 知乎 (zhihu.com)