第三章 卡尔曼滤波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)对应的误差协方差矩阵更新,采用以下公式:
更新后的状态向量估计,建立于更多信息的基础上,更新后状态的不确定度要比更新之前小。
这里介绍的算法为卡尔曼滤波的开环形式,所有的状态估计都保留在卡尔曼滤波算法内。闭环形式里,状态估计被反馈到系统里用以校正系统参数。