卡尔曼滤波算法

引入

        在如下数轴中，有每秒，单片机收到传感器测量的距离distance。

         一般地，单片机会收到一组上图中红线标记的数据，但是实际值是5m。所以所测数据不可用，此时引入滤波，消除噪声，让数据变得平缓：如下图紫色标记数据。

适用系统

        线性高斯系统：

        ①线性：满足叠加性，齐次性。

        ②高斯：噪声满足正态分布。

宏观意义

        滤波即加权

        理想状态： 信号X 1 + 噪声 X 0

        卡尔曼滤波：估计（）+ 观测（）

状态空间表达式：（方便理解卡尔曼滤波）

状态方程：

        Xk是代表当前状态的当前值。Xk-1是上一个时刻状态的值。A代表了状态之间的某种关系，而Uk代表输入，B代表输入与状态的关系。Wk是代表过程噪声，如摩擦力，阻力等。

观测方程：

        Yk是代表了测量值。Xk就是状态方程的值。C代表了一种关系。Vk是观测噪声，如传感器的误差。

        下图可更清晰地理解，状态方程和观测方程：

高斯噪声

        Wk和Vk 都属于高斯噪声，满足正态分布。所以Wk的均值为0 ，方差为Qk。有Vk的均值为0， 方差为Rk。

        一般地，如果状态测量只有一个反馈值，则只需要用到一维方差；如果有多个测量值，则会用到二维协方差cov(X,Y)。

        在卡尔曼滤波中，Q和R是超参数，需要亲自调试。

卡尔曼直观图解

        左一：上一次状态的最优估计值。

        左二：由左一推算出的先验估计值。

        左三：当前状态的最优估计值。

        左四：当前状态的测量值。

        滤波即权重：最优估计要么偏向估计值，要么偏向测量值。

卡尔曼公式理解

        实现过程：

        使用上一次的最优结果预测当前的值，同时使用观测值修正当前值，得到最优结果。

        简单梳理：

        公式一：先验估计公式，基于之前的最优估计，推算出当阶段的先验估计。

        公式二：先验估计协方差公式。

        公式三：卡尔曼增益。

        公式四：最优估计公式，由预测的先验估计值与测量值，卡尔曼增益参数计算出。

        公式五：最终协方差的更新。

应用举例