卡尔曼（Kalman）滤波（四）--深入浅出Kalman滤波算法

       Kalman滤波是一种递归过程，主要有两个更新过程：时间更新和观测更新，其中时间更新主要包括状态预测和协方差预测，主要是对系统的预测，而观测更新主要包括计算卡尔曼增益、状态更新和协方差更新，因此整个递归过程主要包括五个方面的计算：1）状态预测；2）协方差预测；3）卡尔曼增益；4）状态更新；5）协方差更新；

       用数学公式表示，如下：

状态预测：

        （1）

其中，X(k|k)是k时刻的系统状态，U(k)是k时刻对系统的控制量（如果没有控制量，可以为零），是状态转移矩阵，是系统参数；

主要含义：X(k-1|k-1)是上一状态的最优结果，X(k|k-1)是利用上一状态预测得到的结果，这一步主要更新系统结果。

协方差预测：

               （2）

其中，P(k-1|k-1)是上一状态X(k-1|k-1)对应的协方差，P(k|k-1)是X(k|k-1)对应的协方差，Q是系统过程噪声（假设为高斯白噪声）的协方差矩阵；

主要含义：对系统结果对应的协方差进行更新；

计算卡尔曼增益：

其中，K为卡尔曼增益，H为观测矩阵，R为测量噪声（同样假设为高斯白噪声）对应的协方差矩阵；

主要含义：用于状态更新和协方差更新中，因为前两个预测我们得到了当前状态的预测结果，然后再得到当前状态的观测值，通过预测值和观测值，以及增益，就可以得到当前状态的最优状态估计，也就是状态更新。

状态更新：

其中，Z(k)是k时刻的观测值；

主要含义：利用当前状态的观测值和预测值，及增益，计算k时刻下的最优状态估计值X(k|k)，对应着公式（1）中的X(k-1|k-1);

协方差更新：

其中，I为单位矩阵；

主要含义：前面已经更新了状态，为了保证递推下去，还要跟新状态对应的协方差矩阵，对应着公式（2）中的P(k-1|k-1)；

        最后，注意既然是一个递归过程，就需要给出参数的初始值，递归过程是否很好的收敛取决于初始值的选取，所以要根据实际情况合适选择。