第三章 卡尔曼滤波3.2 算法和模型-2卡尔曼滤波算法

离散时间卡尔曼滤波算法包含以下步骤：

 不必严格遵守这个顺序，

前四个步骤组成了卡尔曼滤波的系统传播流程，也被称为是系统更新、系统外推、预测、时间更新或者时间传播流程。

状态转移矩阵定义了状态向量随时间的变化规律，在卡尔曼滤波系统模型中，状态是系统动力学过程的函数。

状态转移矩阵必然是卡尔曼滤波迭代时间间隔的函数，当这些参数随时间发生变化时，就必须在每次迭代中重新计算状态转移矩阵。

步骤（2）计算系统噪声协方差矩阵（system noise covariance matrix）Qk-1,也被称为过程噪声协方差矩阵。

系统噪声常常是迭代时间间隔的函数，针对不同的应用，可将其建模为时变值或常值（对给定的时间间隔）。

步骤（3）状态向量估计值随时间的传播，采用下式：

 步骤（4）对应的误差协方差矩阵的传播，标准形式为

 

 步骤（5）计算观测矩阵（measurement matrix）Hk，该矩阵定义了观测向量随状态的变化规律，其中行对应观测值，列对应状态。

在大多数应用中，观测矩阵是变化的，所以在每次卡尔曼滤波迭代中都必须重新计算。在导航中，Hk常常是载体运动学或GNSS卫星几何构型的函数。

步骤（6）计算观测噪声协方差矩阵Rk。在不同应用中，可假设为常量，也可建模为动力学和或信噪比测量的函数。

步骤（7）计算卡尔曼增益矩阵（卡尔曼gain matrix）Kk。该矩阵决定了观测信息在状态更新时的权值。行对应状态，列对应观测值。卡尔曼增益取决于实际测量向量zk的误差协方差矩阵，和状态估计预测的观测向量的误差协方差矩阵。

误差协方差矩阵的对角元素为不确定度的平方。

由式 (3.8) -式 (3.10) 知,真实观测向量的误差协方差为

 从式（3.5）可得到由状态向量预测的观测向量的误差协方差为

 卡尔曼增益矩阵为

 注意，由于式（3.20）中第一项Hk矩阵在方差比的分子项中被去掉，卡尔曼增益矩阵以及对观测信息进行的加权，都从观测空间转移到了状态空间。矩阵Pk-非对角元素中的相关信息，把观测向量和哪些不能直接通过Hk矩阵与观测相关的状态耦合到了一起。

步骤（8）观测向量zk的构建。

步骤（9）用观测向量更新状态向量，公式为

 步骤（10）对应的误差协方差矩阵更新，采用以下公式：

 更新后的状态向量估计，建立于更多信息的基础上，更新后状态的不确定度要比更新之前小。

 这里介绍的算法为卡尔曼滤波的开环形式，所有的状态估计都保留在卡尔曼滤波算法内。闭环形式里，状态估计被反馈到系统里用以校正系统参数。