EKF与RTS平滑在雷达目标跟踪中的MATLAB实现

1. 项目概述

在目标跟踪领域，雷达测距和测角是最基础的观测手段之一。这个MATLAB例程展示了如何利用扩展卡尔曼滤波(EKF)和RTS平滑算法，对二维平面上的匀速(CV)运动目标进行状态估计。不同于简单的线性观测模型，这个项目处理的是更具挑战性的非线性距离-方位角观测，更贴近实际工程应用场景。

核心价值：这个例程不仅实现了基础滤波功能，还通过RTS平滑提升了估计精度，并提供了全面的误差分析工具链，非常适合作为学习非线性滤波的实践案例。

2. 核心算法解析

2.1 系统建模

系统采用标准的匀速(CV)运动模型，状态向量定义为：

code复制x = [位置x, 位置y, 速度x, 速度y]^T

状态转移矩阵F为：

matlab复制F = [1 0 dt 0;
     0 1 0  dt;
     0 0 1  0;
     0 0 0  1];

过程噪声采用连续白噪声加速度模型离散化得到：

matlab复制Q = q^2 * [dt^4/4 0       dt^3/2 0;
           0       dt^4/4 0       dt^3/2;
           dt^3/2 0       dt^2   0;
           0       dt^3/2 0       dt^2];

2.2 观测模型

观测模型为典型的极坐标测量：

code复制z = [距离; 方位角]

对应的观测方程为：

matlab复制function z = h(x, sensor_pos)
    dx = x(1) - sensor_pos(1);
    dy = x(2) - sensor_pos(2);
    r = sqrt(dx^2 + dy^2);
    theta = atan2(dy, dx);
    z = [r; theta];
end

观测噪声协方差矩阵：

matlab复制R = diag([sigma_r^2, sigma_b^2]);

2.3 EKF实现

EKF的核心在于对非线性观测方程的线性化处理。计算雅可比矩阵H：

matlab复制function H = jacobianH(x, sensor_po