12.6.3　递推算法

12.6.3　递推算法

假设系统的随机速度扰动和测量噪声相互独立，并且都为零均值、协方差分别为Q（k）和N（k）的高斯随机噪声。对应于模型表达式的卡尔曼滤波器递推过程如下。

（1）一步预测值：

（2）一步预测误差协方差：

（3）最佳增益矩阵：

（4）滤波估计：

（5）滤波误差协方差：

对于式（12-66）、式（12-67）给出的运动方程，启动条件为

式中，为x的测量方差。

当目标位于近处时，r、θ、φ不满足CV模型，同时r、θ、φ的测量精度也不断提高。为了也能适用该递推公式，当目标仰角大于某一特定值，目标接近时，逐步乘以大于1的系数放大模型误差Vx，目标远离时再缩小；同时，随着测量精度的提高和降低，乘以系数，缩小或放大测量方差S，也能达到很好的滤波效果。