【车载】轮速算法-卡尔曼滤波算法

本文主要是介绍【车载】轮速算法-卡尔曼滤波算法，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

1 背景

2 kalman filter 最优化递归估计

3 代码实现：

4 实例

1 背景

Kalman Filter 是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估计动态系统的状态。起源于 Rudolf Emil Kalman 在 1960 年的博士论文和发表的论文《A NewApproach to Linear Eiltering and Prediction Problems》（《线性滤波与预测问题的新方法》）。并且最先在阿波罗登月计划轨迹预测上应用成功，此后 kalman filter 取得重大发展和完善。它的广泛应用已经超过 30 年，包括机器人导航，控制。传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等，近年来更被广泛应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2 kalman filter 最优化递归估计

总结起来就是5个核心公式：

        X(k | k -1) = A× X(k -1| k -1) + B×U(k)

        P(k | k -1) = A×P(k -1| k -1)𝐴𝑇 +Q

        X(k | k) = X(k | k -1) + 𝑘𝑔 (k)×(Z(k) - H × X(k | k -1))

        𝑘𝑔 (k) = P(k | K -1)×𝐻𝑇/(H ×P(k | k -1)×𝐻𝑇 + R)

        P(k | k) = (1-𝑘𝑔(k)×H)×P(k | k -1)

3 代码实现：

float32 g_kal_Q = 0.001;  /*过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏*/
float32 g_kal_R = 0.489;  /*观测噪声协方差,R增大，动态响应变慢，收敛稳定性变好*/void kalman_Filter(float32 *inputData){static float32 x_last = 0;                        //预测现在的状态 X(k|k-1)=A*X(k-1|k-1)+B*U(k)//上一个时刻的状态Xstatic float32 p_last = 0.02;                //上一个时刻的协方差P，这里赋值是初始时刻方差P0float32 kg;      //增益Kfloat32 x_mid;   //状态输入值float32 p_mid;   //P中间值（预测值）        float32 p_now;   //迭代时最优值对应的协方差        x_mid = x_last;                       //x_last=x(k-1|k-1),x_mid=x(k|k-1)p_mid = p_last + g_kal_Q;             //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声kg = p_mid / (p_mid + g_kal_R);*inputData = x_mid + kg*(*inputData - x_mid); //估计出的最优值p_now = (1 - kg)*p_mid;                       //最优值对应的协方差x_last = *inputData;                          //更新系统状态值p_last = p_now;                               //更新 covariance 值
}