卡尔曼滤波是个什么鬼？！

何为卡尔曼滤波？

卡尔曼滤波是一个最优化自回归数据处理算法（别问我什么是自回归算法，我也不知道，这个时候还是问百度吧＞﹏＜）它可以应用在任何含有不确定信息的动态系统中，对系统下一步的走向做出预测。

卡尔曼滤波的五个基本公式

1. $X\left(k|k-1\right)=A\cdot X\left(k-1|k-1\right)+B\cdot U\left(k\right)$ 状态的一步预测方程
2. $P\left(k|k-1\right)=A\cdot P\left(k-1|k-1\right)\cdot A^{\prime }+Q$ 协方差的一步预测方程
3. $Kg\left(k\right)=\frac{P\left(k|k-1\right)\cdot H^{\prime }}{H\cdot P\left(k|k-1\right)\cdot H^{\prime }+R}$ 滤波增益方程
4. $X\left(k|k\right)=X\left(k|k-1\right)+Kg\left(k\right)\left(Z\left(k\right)-H\cdot X\left(k|k-1\right)\right)$ 滤波估计方程
5. $P\left(k|k\right)=\left(I-Kg\left(k\right)\cdot H\right)\cdot P\left(k|k-1\right)$ 协方差更新方程
   
   在公式中，
   $X\left(k|k-1\right)$-------------利用上一状态预测出的结果；
   $X\left(k-1|k-1\right)$----------上一状态最优结果；
   $U\left(k\right)$------------------当前控制量；
   $P\left(k|k-1\right)$--------------$X\left(k|k-1\right)$的协方差；
   $Q$--------------------------------------系统过程协方差；
   $Kg\left(k\right)$-----------------卡尔曼增益；
   $R$--------------------------------------噪声；
   $X\left(k|k\right)$----------------当前最优估计值；
   $Z\left(k\right)$------------------测量值；
   
   公式1和2是对系统状态的预测；3和4将预测与测量值合并得出最优估计；5更新数据。

5个基本公式中的参数

A：状态转移矩阵
B：输入控制项矩阵
H：测量矩阵
Q：噪声协方差
R：测量噪声协方差
I ：单位矩阵
一般A、B、H、I视为1；Q、R则作为输入参数进行调试，其值人为给定，不可为0，如Q=0.98，R=0.542；对Q、R设定时$\frac{Q}{Q+R}$就是Kg收敛值，Kg越大说明测量值越可靠，最优化接近测量值，反之说明预测值越可靠，最优化接近预测值。同时$\frac{P0}{Q+R}$反映收敛的速度，值越小收敛越快，P0为协方差初值。

离散控制系统的线性微分方程表示

1.$X\left(k\right)=A\cdot X\left(k-1\right)+B\cdot U\left(k\right)+W\left(k\right)$
2.$Z\left(k\right)=H\cdot X\left(k\right)+V\left(k\right)$
公式中，X是系统状态，U是控制量，W、V是干扰，Z是测量值。

协方差更新方程的几种形式

1.$P\left(k|k\right)=\left(I-Kg\left(k\right)\cdot H\right)\cdot P\left(k|k-1\right)$
2.$P\left(k|k\right)=P\left(k|k-1\right)-Kg\left(k\right)\cdot H\cdot P\left(k|k-1\right)$
3.$P\left(k|k\right)=P\left(k|k-1\right)-Kg\left(k\right)\cdot S\left(k\right)\cdot K{g}^T\left(k\right)$
4.$P\left(k|k\right)=\left[I-Kg\left(k\right)\cdot H\right]P\left(k|k-1\right)\cdot {\left[I-Kg\left(k\right)\cdot H\right]}^T-Kg\left(k\right)\cdot R\cdot Kg{\left(k\right)}^T$

公式1和2是最简单的，也是常用的，但它仅在使用最优Kg时成立，否则只可用公式4.

简单的代码

    m_XMid = m_XLast;                                    //将上一次最优值给一个中间变量X(k|k-1)
    m_PMid = m_PLast + m_Q;                              //P(k|k-1)
    m_Kg = m_PMid/(m_PMid + m_R);                        //Kg
    m_XNow = m_XMid + m_Kg * (nMersure - m_XMid);        //X(k|k)
    m_PNow = (1 - m_Kg) * m_PMid;                        //P(k|k)
    m_XLast = m_XNow;
    m_PLast = m_PNow;

这里A、B、H均为1，Q、R自行赋值调试，系统中Q和初值m_XLast不可为0。

其他卡尔曼滤波

AKF—自适应卡尔曼滤波（KF的升级版）
EKF—扩展卡尔曼滤波（适用非线性系统）
KF-----卡尔曼滤波（适用线性系统）

KF优缺点

处理效果好，但计算量大，速度慢，动态性能好。