3.2.4 卡尔曼滤波算法应用举例

例题3.1 设目标在x轴方向上做匀速直线运动，其状态方程为

式中，状态向量，状态转移矩阵F（k）、过程噪声分布矩阵Γ（k）分别为

式中，采样间隔T=1s，过程噪声是零均值的高斯白噪声，且和量测噪声序列相互独立，其方差为E[v2（k）]=q，仿真时取q=0和q=1两种情况，目标真实的初始状态为X（0）=[9 11]′。

量测方程为

式中，量测噪声是零均值的白噪声，具有方差E[W2（k）]=r=4，而量测矩阵H（k）=[1 0]。

要求：

（1）画出目标真实运动轨迹和估计轨迹；

（2）画出目标预测和更新的位置和速度方差。

解：由于系统的状态向量是二维的，所以可采用式（3.59）和式（3.60）的方法进行状态和协方差初始化，量测值Z（k）由式（3.76）获得。图3.5和图3.6分别为过程噪声q=0和q=1情况下的目标真实轨迹和滤波轨迹，其中横坐标为目标的位置，纵坐标为目标的运动速度。图3.7、图3.8和图3.9、图3.10分别为预测位置误差协方差P₁₁（k+1|k）、更新位置误差协方差P₁₁（k+1|k+1）、预测速度误差协方差P₂₂（k+1|k）和更新速度误差协方差P₂₂（k+1|k+1）在过程噪声q=0和q=1情况下的结果图，其中横坐标为跟踪步数，纵坐标分别为位置和速度误差协方差。

图3.5 真实和滤波轨迹（q=0）

图3.6 真实和滤波轨迹（q=1）

图3.7 预测和更新位置误差协方差（q=0）

图3.8 预测和更新速度误差协方差（q=0）

图3.9 预测和更新位置误差协方差（q=1）

图3.10 预测和更新速度误差协方差（q=1）

由图3.7～图3.10可看出，随着估计过程的进行，P（k+1|k+1）是逐渐下降的，这说明估计在起作用，估计的误差在逐渐减少，下降的幅度与过程噪声协方差Q和量测噪声协方差R有关，也与环境的复杂性、滤波算法的好坏有关，而P（k+1|k）却比P（k+1|k+1）大，增大的值与Q有关。