算法推导——卡尔曼滤波

什么是卡尔曼滤波？

  你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。
  在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其他历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。

卡尔曼滤波五大公式

定义变量
状态预测值：${\hat{x}}_k^{-}$，维度$n\times 1$
状态估计值：${\hat{x}}_k$
实际观测值：$y_k$，维度$m\times 1$
输入量：$u_k$，维度$l\times 1$
状态转移矩阵：$F$，维度$n\times n$
测量矩阵：$H$，维度$m\times n$
输入量到状态的转换矩阵：$B$，维度$n\times l$
状态噪声：$Q$
观测噪声：$R$
状态预测值的状态协方差矩阵：${\hat{P}}_k^{-}$，表示状态预测值与状态真实值之间的误差协方差矩阵；
状态估计值的状态协方差矩阵：${\hat{P}}_k$，表示状态估计值与状态真实值之间的误差协方差矩阵；
卡尔曼滤波增益：$K_k$

时间更新：
$${\hat{x}}_k^{-}=F{\hat{x}}_{k-1}+B_k{u}_{k-1}（式1）$$$${\hat{P}}_k^{-}=F{\hat{P}}_{k-1}{F}^T+Q（式2）$$
状态更新：
$$K_k={\hat{P}}_k^{-}{H}^T(H{\hat{P}}_k^{-}{H}^T+R{)}^{-1}（式3）$$$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(y_k-H{\hat{x}}_k^{-})（式4）$$$${\hat{P}}_k=(I-K_{k}H){\hat{P}}_k^{-}（式5）$$
预测过程是利用系统模型(状态方程)预测状态的先验概率密度，也就是通过已有的先验知识对未来的状态进行猜测，即$p(x(k)|x(k-1))$。更新过程则利用最新的测量值对先验概率密度进行修正，得到后验概率密度，也就是对之前的猜测进行修正。

卡尔曼滤波推导过程

1、在k时刻，卡尔曼滤波的目的是得到估计值：${\hat{x}}_k$，通过（式4：通过观测值与状态预测值的误差，修正状态预测值，得到状态估计值，此为后验），已知：观测矩阵$H$以及观测量$y_k$，需要确定k时刻的状态预测值${\hat{x}}_k^{-}$和k时刻卡尔曼增益$K_k$。
2、