关于线性卡尔曼滤波的参数解释和一点应用见解

        近期笔者感觉单传感器获取的数据噪声太大，受环境影响大，需要用到多传感器融合，这时候卡尔曼滤波就是一个不错的选择，在这里想向大家分享一下本人对最普通版本的线性卡尔曼滤波的一点见解，也是为实验室留一点资料，虽然可能用处并不大哈哈哈哈......

        提前申明一下，笔者对卡尔曼理解并不是很深刻，只能做一些简单的应用，所以本文并不进行公式推导，只讲简单应用，希望能对读者起到一点点帮助吧。

        废话不多说，下面我们开始正文：

        首先引用一下我从别的文章截图过来的卡尔曼个公式。

不知道这些各式各样的“符号”的含义，可能就是大部分人初次难以理解卡尔曼的原因了。

在这里我做一下个人理解的含义，其中：

X(k) 是k时刻的状态量（通常是n*1的矩阵， n表示几个状态变量）；

F(k) 是状态转移矩阵；

F(k) T是状态转移矩阵的转置；

B(k)是控制矩阵；

P(k)是k时刻的协方差（通常是n*n的矩阵）；

Q(k)是过程噪声矩阵（通常是n*n的矩阵）；

K(k)是卡尔曼增益；

H(k)是状态量到观测量的转换矩阵（按照笔者的理解，是为了满足矩阵之间可以运算的前提，一般是[1 0]（比如只有位置测量值）  ，具体看有几个观测量和状态量）；

H(k)T是H(k)的转置矩阵；

R(k)是观测噪声矩阵，如果观测量只有一个那就是一个常数）；

Z(k)是测量值；

I 是单位矩阵（一般是n*n的矩阵）

补充：Z(k) - H(k)*X(k) 是残差，我见有文章写用此值做卡方检验，用于检测传感器数据是否异常，如果读者有此类需求的话可以尝试一下。

        OK 下面做一些简单的解释（当初我学习卡尔曼的时候困扰我的问题），比如什么是状态空间方程，咱怎么知道F矩阵咋写，B矩阵咋表示的······

        在解释之前我想引入一下状态空间方程的概念，状态空间方程就是每个状态变量之间的联系，合在一起就能描述物体在空间中的状态（速度 位置 角度 角速度都算某个状态），至于其是怎么得出来的呢 —— 推导，那怎么推导呢，下面引入匀速运动模型例子。

        比如匀速运动模型，我们初中都学过：（这两个式子就是匀速运动模型的状态空间方程）

        其中，u是加速度， v是速度， p是位置。       

我们尝试把把车体所有的状态量（例如速度 位置 加速度） 通过一个矩阵的形式写出来。

        此时，上述式子就可以写成如下形式：

状态变量X(是一个矩阵，因为包含多个状态量)，写成如下形式：

划重点！！！ 我们再看一下卡尔曼滤波的第一个公式

        注意，此时X(k)我们已经有了，F(k) 和 B(k)也就显而易见咯（提醒一下 u是加速度）。

因此， 我们呢可以把F(k) 和 B(k)都写出来了。

如上的式子我们就可以简单看成如下形式：

各个“符号”含义在上面均已给出，现在我们已经有了F(k) ,  B (k)，P(k)可以根据第二个公式写出来，那所有的公式都能表示出来了。

Q矩阵表示的是过程噪声的协方差，如果状态量是速度-位置， 或者是 角度-角速度的话，状态量之间相互独立，矩阵的从对角线取值都是0，所以就很方便啦。

        如果感觉不知道代码怎么写，那就用笔带入公式，完整推一遍，用多条数学表达式表示完了很容易就把那些推出来的公式转成C语言嘞。

        以上，笔者想表达的见解结束了，如果有误，还请评论区指出，不喜勿喷呜呜呜······

下面贴出卡尔曼融合速度和融合角度的代码：

        1、根据位置和加速度 -- 融合速度：

#define VELKAL_DELTAT  0.005
void Velocity_KalmanFilter(float position_mea, float accel_mea, float* pos_est, float* vel_est)
{
    static float Q_Xmeasure=0.0f, Q_Vmeasure =0.0f;//过程噪声
    static float R_estimate =0.0f;//测量噪声
    static float P[2][2] ={{1,0},{0,1}};//协方差矩阵
    static float K[2]  = {0,0};//卡尔曼增益
    static float position_est =0.0f, velocity_est = 0.0f;

    //先验估计
    position_est = position_est + velocity_est*VELKAL_DELTAT + 0.5*accel_mea*VELKAL_DELTAT*VELKAL_DELTAT;
    velocity_est = velocity_est + accel_mea*VELKAL_DELTAT;
    //先验协方差
    P[0][0] = P[0][0] + P[1][0]*VELKAL_DELTAT + P[0][1]*VELKAL_DELTAT + P[1][1]*VELKAL_DELTAT*VELKAL_DELTAT + Q_Xmeasure;
    P[0][1] = P[0][1] + P[1][1]*VELKAL_DELTAT;
    P[1][0] = P[1][0] + P[1][1]*VELKAL_DELTAT;
    P[1][1] = P[1][1] + Q_Vmeasure;
    //计算卡尔曼增益
    K[0] = P[0][0]/(P[0][0] + R_estimate);
    K[1] = P[1][0]/(P[0][0] + R_estimate);
    //后验估计
    position_est = position_est + K[0]*(position_mea - position_est);
    velocity_est = velocity_est + K[1]*(position_mea - position_est);
    //协方差矩阵更新
    P[0][0] = (1 - K[0])*P[0][0];
    P[0][1] = (1 - K[0])*P[0][1];
    P[1][0] = -K[1]*P[0][0] + P[1][0];
    P[1][1] = -K[1]*P[0][1] + P[1][1];
    //融合后的位置 速度输出
    *pos_est = position_est;
    *vel_est = velocity_est;
}

2、根据加速度 角速度融合角度

#define KAL_DT    0.005
void Kalman_Filter_Angle(float angle_mes, float gyro_mes, float* angle_filter)
{
    static float angle_est =0.0f;//估计值
    static float Q_bias =0.0f;//漂移

    static float K0 =0.0f;//卡尔曼增益
    static float K1 =0.0f;
    static float P[2][2] = {{1,0},{0,1}};//协方差

    //超参数 Q 过程噪声  R 测量噪声
    static float Q_angle =0.001f;//角度噪声协方差
    static float Q_gyro =0.001f;//角速度噪声协方差
    static float R_angle = 0.001f;//角度测量噪声

    //先验估计
    angle_est += (gyro_mes - Q_bias)*KAL_DT;
    Q_bias =Q_bias +0;
    //先验协方差
    P[0][0] = P[0][0] -(P[1][0] +P[0][1])*KAL_DT + P[1][1]*KAL_DT*KAL_DT + Q_angle;
    P[0][1] = P[0][1] - P[1][1]*KAL_DT;
    P[1][0] = P[1][0] - P[1][1]*KAL_DT;
    P[1][1] = P[1][1] +Q_gyro;
    //计算卡尔曼增益
    K0 = P[0][0]/(P[0][0] + R_angle);
    K1 = P[1][0]/(P[0][0] + R_angle);
    //计算最优估计
    angle_est = angle_est+ K0*(angle_mes - angle_est);
    Q_bias = Q_bias + K1*(angle_mes - angle_est);
    //更新协方差
    P[0][0] = (1 - K0)*P[0][0];
    P[0][1] = (1 - K0)*P[0][1];
    P[1][0] = P[1][0] - K1*P[0][0];
    P[1][1] = P[1][1] - K1*P[0][1];
    *angle_filter = angle_est;
}

        顺便提一下，上述的卡尔曼噪声设置的都是常数，这也导致了卡尔曼增益可能在计算了几十次之后就收敛了（数值稳定，几乎不动），所以抗干扰能力很差，如果可以估计出传感器的噪声效果会好很多，而且Q R都属于超参数，和PID参数一样只能根据经验和实际效果来调整，效率也不高，要想达到更好的效果可以用卡尔曼的进阶版，不属于本文的探讨内容，笔者也就不在此过多赘述了，因为笔者也不会哈哈哈哈哈。

        如果本文出现错误还请评论区指正，感谢各位！

        以上，祝好！