在雷达目标跟踪、导航系统、机器人定位等领域，卡尔曼滤波器是一种实现最优状态估计的核心算法。其核心思想是在存在测量噪声和过程不确定性的情况下，通过预测与更新两个阶段的循环迭代，不断优化对系统状态的估计精度。本文以一维雷达跟踪飞机为例，深入剖析卡尔曼滤波器的数学原理与工程实现。
问题建模与系统状态定义
考虑一个简单但典型的场景：雷达对沿直线飞行的飞机进行距离测量。雷达通过发射脉冲信号并接收目标反射的回波，可以计算出飞机的距离；同时利用多普勒效应测量飞机的径向速度。在这个一维跟踪问题中，系统状态由两个变量组成：飞机的距离 r 和速度 v。我们用状态向量 x 来表示：
x = [r; v]
其中，分号表示列向量分隔。距离 r 的单位为米，速度 v 的单位为米每秒。在卡尔曼滤波器的数学表述中，向量用粗体小写字母表示，矩阵用粗体大写字母表示。
假设在初始时刻 t₀，雷达进行一次高精度测量，得到距离为 10000 米、速度为 200 米每秒。这次测量值构成测量向量 z₀ = [10000; 200]。由于测量必然受到噪声影响，每次测量都是一个随机变量，其真值我们永远无法确切得知。测量的可靠性用测量不确定度来描述，在卡尔曼滤波器中用协方差矩阵 R 表示。假设距离测量的标准差为 4 米，速度测量的标准差为 0.5 米每秒，则测量协方差矩阵为：
R₀ = [[16, 0]; [0, 0.25]]
主对角线上的元素是各测量分量的方差（即标准差的平方），非主对角线为零表示距离和速度的测量误差相互独立。
初始化与状态估计
卡尔曼滤波器的第一次迭代（迭代 0）从初始化开始。在没有先验信息的情况下，我们直接使用第一次测量作为系统的初始状态估计：
x̂₀₀ = z₀ = [10000; 200]
这里 x̂ 表示状态估计值，下标的双重含义是：第一个下标表示估计所对应的时刻，第二个下标表示进行估计时所使用的最新信息时刻。因此 x̂₀₀ 表示在时刻 t₀ 基于时刻 t₀ 的信息所作的估计，即当前估计。
同时，我们用测量协方差矩阵作为初始的状态估计协方差矩阵：
P₀₀ = [[16, 0]; [0, 0.25]]
协方差矩阵 P 描述了状态估计的不确定程度，其主对角线元素分别是各状态分量的估计方差。
状态预测：预测方程
完成初始化后，卡尔曼滤波器进入预测阶段。假设雷达的采样间隔为 Δt = 5 秒，即每 5 秒更新一次目标信息。预测阶段的任务是基于当前的状态估计，推算下一时刻的系统状态。
我们假设飞机在短时间内保持匀速运动，这就是动态模型（或称为状态转移模型）。在匀速假设下，状态转移关系为：
v₁ = v₀ r₁ = r₀ + v₀ × Δt
用矩阵形式表示为：
x̂₁₀ = F × x̂₀₀
其中，F 是状态转移矩阵：
F = [[1, Δt]; [0, 1]] = [[1, 5]; [0, 1]]
代入数值计算：
x̂₁₀ = [[1, 5]; [0, 1]] × [10000; 200] = [11000; 200]
这表示预测在 5 秒后，飞机的距离将为 11000 米，速度保持 200 米每秒。
协方差预测：不确定性传播
预测不仅需要估计状态值，还需量化预测的不确定程度。状态估计的协方差矩阵也需要按照相同的动态模型进行预测。协方差预测方程为：
P₁₀ = F × P₀₀ × Fᵀ + Q
其中，Q 是过程噪声协方差矩阵，用于建模动态模型的不确定性。在实际系统中，飞机可能因气流、风速等因素产生随机加速度，这会导致实际运动与匀速假设产生偏差。假设随机加速度的标准差为 σₐ = 0.2 m/s²，则过程噪声矩阵为：
Q = [[Δt⁴/4, Δt³/2]; [Δt³/2, Δt²]] × σₐ²
代入 Δt = 5 秒和 σₐ² = 0.04，计算得到：
Q = [[6.25, 2.5]; [2.5, 1]]
完整计算协方差预测：
F × P₀₀ × Fᵀ = [[22.25, 1.25]; [1.25, 0.25]]
P₁₀ = [[22.25, 1.25]; [1.25, 0.25]] + [[6.25, 2.5]; [2.5, 1]] = [[28.5, 3.75]; [3.75, 1.25]]
注意到距离的方差从 16 增加到 28.5，这是因为时间推移导致速度不确定性逐渐累积到位置估计中；而速度方差从 0.25 变为 1.25，则是过程噪声直接作用的结果。
测量更新：卡尔曼增益计算
预测完成后，雷达在 t₁ 时刻获得第二次测量。由于此次测量遇到强噪声干扰，测量精度下降：距离标准差变为 6 米，速度标准差变为 1.5 米每秒。因此测量协方差矩阵变为：
R₁ = [[36, 0]; [0, 2.25]]
测量值为 z₁ = [11020; 202]。
更新阶段的核心问题是：如何融合预测值与测量值以获得更优的估计？卡尔曼滤波器的解决思路是对两者进行加权平均，权重由卡尔曼增益 K 决定。卡尔曼增益的计算公式为：
K₁ = P₁₀ × Hᵀ × (H × P₁₀ × Hᵀ + R₁)⁻¹
在本例中，观测矩阵 H 是单位矩阵（因为测量值与状态值直接对应），因此：
K₁ = P₁₀ × (P₁₀ + R₁)⁻¹
计算可得：
P₁₀ + R₁ = [[64.5, 3.75]; [3.75, 3.5]]
求逆后与 P₁₀ 相乘，得到卡尔曼增益矩阵：
K₁ = [[0.4048, 0.6377]; [0.0399, 0.3144]]
状态更新与协方差更新
有了卡尔曼增益，即可进行状态更新。状态更新方程为：
x̂₁₁ = x̂₁₀ + K₁ × (z₁ - H × x̂₁₀)
计算创新量（残差）：
z₁ - H × x̂₁₀ = [11020; 202] - [11000; 200] = [20; 2]
应用卡尔曼增益进行修正：
K₁ × [20; 2] = [9.37; 1.43]
最终得到更新后的状态估计：
x̂₁₁ = [11000; 200] + [9.37; 1.43] = [11009.37; 201.43]
更新后的协方差矩阵使用约瑟夫形式（Joseph form）计算：
P₁₁ = (I - K₁ × H) × P₁₀ × (I - K₁ × H)ᵀ + K₁ × R₁ × K₁ᵀ
使用简化形式计算的结果为：
P₁₁ = [[14.57, 1.43]; [1.43, 0.71]]
分析结果可以发现，更新后的估计方差（14.57 和 0.71）既小于预测方差（28.5 和 1.25），也小于测量方差（36 和 2.25）。这正是卡尔曼滤波器 “最优” 的体现：通过融合预测与测量，即使测量本身精度不高，也能获得比单独使用任一来源更精确的估计。
工程实践参数建议
在实际雷达系统中应用卡尔曼滤波器，有几个关键工程参数值得关注。状态转移矩阵 F 应根据目标运动特性选择，匀速模型使用 F = [[1, Δt]; [0, 1]]，若考虑加速度则需扩展为包含加速度分量的形式。过程噪声协方差矩阵 Q 的取值直接影响滤波器对目标机动的响应速度，Q 值越大意味着对动态模型不确定性的容忍度越高，但会导致预测方差增长更快。测量协方差矩阵 R 通常基于传感器的技术指标估算，在雷达系统中可依据信噪比（SNR）动态调整。实际工程中建议对 R 进行实时更新，以反映测量条件的变化。
卡尔曼滤波器通过 “预测 — 更新” 的循环迭代，持续利用新的测量信息修正状态估计，实现对目标运动轨迹的最优跟踪。掌握其数学原理对于雷达信号处理、传感器融合等工程实践具有重要价值。
资料来源：本文数学推导与数值示例主要参考 Kalman Filter 官方教程网站（kalmanfilter.net）的雷达跟踪示例。