第21篇：扩展卡尔曼滤波（EKF）与动态跟踪
一、作用：从"一个点"到"一条轨迹"
前20篇，我们解决的都是静态定位问题------目标不动，我们估计它在哪里。
但真实的电子战场景中，目标在动：
- 敌方战机以 300 m/s 突防；
- 反舰导弹以 2 马赫掠海飞行；
- 无人机集群以编队队形渗透。
如果我们每个时刻都独立做一次定位，会得到一条抖动的轨迹 ------每一帧的估计误差独立随机，看起来像"醉汉走路"。而真实目标通常是平滑运动的。
扩展卡尔曼滤波（Extended Kalman Filter, EKF） 就是解决这个问题的标准工具。它的核心思想是：
把目标的位置、速度、加速度看作一个"状态"，用运动模型预测下一时刻的状态，再用观测数据修正这个预测。
EKF 是卡尔曼滤波（KF） 的非线性版本。标准 KF 处理线性系统，而 EKF 通过对非线性函数做一阶泰勒展开，把 KF 推广到非线性场景。
典型应用：
- 雷达跟踪：飞机、导弹、卫星；
- ESM 跟踪：无源侦测，仅用 AOA/TDOA/FDOA；
- GPS/INS 组合导航：惯性 + 卫星；
- 自动驾驶：毫米波雷达 + 激光雷达 + 视觉。
本篇核心作用 ：从状态空间模型出发，推导 EKF 的预测-更新 两步骤；用 MATLAB 完整实现一个 AOA 跟踪场景 （单传感器跟踪运动目标）；展示 EKF 如何把抖动的测向线"平滑"成一条光滑轨迹；并分析几何退化时 EKF 的稳定性问题。
代码出处 ：book2_ch7_drawFigures（图7.1~7.5、7.8a/b）、+tracker/ 模块。
二、原理：数学上发生了什么？
2.1 状态空间模型
EKF 的基础是状态空间模型，由两个方程组成：
状态方程（运动模型） ：
xk+1=Fxk+wk,wk∼N(0,Q)\mathbf{x}_{k+1} = \mathbf{F} \mathbf{x}_k + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q})xk+1=Fxk+wk,wk∼N(0,Q)
观测方程（测量模型） ：
zk=h(xk)+vk,vk∼N(0,R)\mathbf{z}_k = \mathbf{h}(\mathbf{x}_k) + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R})zk=h(xk)+vk,vk∼N(0,R)
其中：
- xk\mathbf{x}_kxk：kkk 时刻的状态向量；
- F\mathbf{F}F：状态转移矩阵（描述运动规律）；
- h(⋅)\mathbf{h}(\cdot)h(⋅)：观测函数（非线性，这是 EKF 的"E"所在）；
- Q\mathbf{Q}Q：过程噪声协方差（运动模型的不确定性）；
- R\mathbf{R}R：观测噪声协方差。
2.2 匀速直线运动模型（CV 模型）
最常见的运动模型是匀速直线运动（Constant Velocity, CV） 。二维场景下，状态向量为：
x=x,y,x˙,y˙T\mathbf{x} = x, y, \\dot{x}, \\dot{y}^Tx=x,y,x˙,y˙T
状态转移矩阵为：
F=10T0010T00100001\mathbf{F} = \begin{bmatrix} 1 & 0 & T & 0 \\ 0 & 1 & 0 & T \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}F= 10000100T0100T01
其中 TTT 是采样间隔。过程噪声 Q\mathbf{Q}Q 通常建模为连续白噪声加速度模型：
Q=q⋅T4/40T3/200T4/40T3/2T3/20T200T3/20T2\mathbf{Q} = q \cdot \begin{bmatrix} T^4/4 & 0 & T^3/2 & 0 \\ 0 & T^4/4 & 0 & T^3/2 \\ T^3/2 & 0 & T^2 & 0 \\ 0 & T^3/2 & 0 & T^2 \end{bmatrix}Q=q⋅ T4/40T3/200T4/40T3/2T3/20T200T3/20T2
2.3 AOA 观测模型
单传感器位于 (xs,ys)(x_s, y_s)(xs,ys)，观测目标方位角：
ψk=atan2(yk−ys,xk−xs)+vk\psi_k = \text{atan2}(y_k - y_s, x_k - x_s) + v_kψk=atan2(yk−ys,xk−xs)+vk
这是非线性的 ------atan2\text{atan2}atan2 对状态不是线性函数。EKF 通过一阶泰勒展开来处理。
观测雅可比矩阵 （1×41 \times 41×4）：
Hk=∂ψ∂x∣x^k∣k−1=−ΔyR2ΔxR200\mathbf{H}k = \frac{\partial \psi}{\partial \mathbf{x}}\bigg|{\hat{\mathbf{x}}_{k|k-1}} = \begin{bmatrix} -\frac{\Delta y}{R^2} & \frac{\Delta x}{R^2} & 0 & 0 \end{bmatrix}Hk=∂x∂ψ x^k∣k−1=−R2ΔyR2Δx00
其中 Δx=x^−xs\Delta x = \hat{x} - x_sΔx=x^−xs，Δy=y^−ys\Delta y = \hat{y} - y_sΔy=y^−ys，R2=Δx2+Δy2R^2 = \Delta x^2 + \Delta y^2R2=Δx2+Δy2。
2.4 EKF 的两个步骤
EKF 每次迭代分两步：
（1）预测步（时间更新） ：
x^k∣k−1=Fx^k−1∣k−1\hat{\mathbf{x}}{k|k-1} = \mathbf{F} \hat{\mathbf{x}}{k-1|k-1}x^k∣k−1=Fx^k−1∣k−1
Pk∣k−1=FPk−1∣k−1FT+Q\mathbf{P}{k|k-1} = \mathbf{F} \mathbf{P}{k-1|k-1} \mathbf{F}^T + \mathbf{Q}Pk∣k−1=FPk−1∣k−1FT+Q
（2）更新步（观测更新） ：
计算卡尔曼增益 ：
Kk=Pk∣k−1HkT(HkPk∣k−1HkT+R)−1\mathbf{K}k = \mathbf{P}{k|k-1} \mathbf{H}_k^T \left( \mathbf{H}k \mathbf{P}{k|k-1} \mathbf{H}_k^T + \mathbf{R} \right)^{-1}Kk=Pk∣k−1HkT(HkPk∣k−1HkT+R)−1
用观测修正状态：
x^k∣k=x^k∣k−1+Kk(zk−h(x^k∣k−1))\hat{\mathbf{x}}{k|k} = \hat{\mathbf{x}}{k|k-1} + \mathbf{K}_k \left( \mathbf{z}k - \mathbf{h}(\hat{\mathbf{x}}{k|k-1}) \right)x^k∣k=x^k∣k−1+Kk(zk−h(x^k∣k−1))
更新协方差：
Pk∣k=(I−KkHk)Pk∣k−1\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}k) \mathbf{P}{k|k-1}Pk∣k=(I−KkHk)Pk∣k−1
关键直觉：
- 卡尔曼增益 K\mathbf{K}K 决定"信预测还是信观测"------当预测不确定（P\mathbf{P}P 大）而观测可靠（R\mathbf{R}R 小）时，K\mathbf{K}K 大，更倾向观测；
- 新息（Innovation） zk−h(x^k∣k−1)\mathbf{z}k - \mathbf{h}(\hat{\mathbf{x}}{k|k-1})zk−h(x^k∣k−1) 是"观测与预测的差距"------EKF 用这个差距修正状态。
2.5 为什么叫"扩展"？
标准 KF 要求 h\mathbf{h}h 是线性函数。EKF 的"扩展"在于：
- 用一阶泰勒展开近似 h\mathbf{h}h ：h(x)≈h(x^)+H(x−x^)\mathbf{h}(\mathbf{x}) \approx \mathbf{h}(\hat{\mathbf{x}}) + \mathbf{H}(\mathbf{x} - \hat{\mathbf{x}})h(x)≈h(x^)+H(x−x^)；
- 用雅可比矩阵 H\mathbf{H}H 代替线性矩阵。
代价：
- 如果非线性太强，一阶近似失效，EKF 可能发散；
- 需要对初值有合理估计；
- 协方差更新不是最优的。
但对于电子战中的大多数场景（目标距离远、运动平滑），EKF 足够好。
2.6 EKF 与贝叶斯的关系
EKF 就是贝叶斯估计的递归实现：
| 贝叶斯概念 | EKF 对应 |
|---|---|
| 先验 p(xk∣z1:k−1)p(\mathbf{x}k | \mathbf{z}{1:k-1})p(xk∣z1:k−1) | 预测分布 N(x^k∣k−1,Pk∣k−1)\mathcal{N}(\hat{\mathbf{x}}{k|k-1}, \mathbf{P}{k|k-1})N(x^k∣k−1,Pk∣k−1) |
| 似然 p(zk∣xk)p(\mathbf{z}_k | \mathbf{x}_k)p(zk∣xk) | 观测模型 N(h(xk),R)\mathcal{N}(\mathbf{h}(\mathbf{x}_k), \mathbf{R})N(h(xk),R) |
| 后验 p(xk∣z1:k)p(\mathbf{x}k | \mathbf{z}{1:k})p(xk∣z1:k) | 更新分布 N(x^k∣k,Pk∣k)\mathcal{N}(\hat{\mathbf{x}}{k|k}, \mathbf{P}{k|k})N(x^k∣k,Pk∣k) |
所以第19篇讲的贝叶斯估计是"静态"的，EKF 是"动态"的------只是把先验从静态分布变成通过运动模型传播的预测分布。
三、完整代码（可直接运行）
matlab
%% ========================================================================
% 第21篇：扩展卡尔曼滤波（EKF）与动态跟踪
% 参考出处：O'Donoughue, "Practical Geolocation for EW using MATLAB",
% book2_ch7_drawFigures.m, Figures 7.1-7.5, 7.8a/b
% https://github.com/nodonoughue/emitter-detection-book
% ========================================================================
clear; close all; clc;
rng(2024);
%% ==================== 参数设置 ====================
% 时间参数
T = 1; % 采样间隔 1 s
N_steps = 100; % 总时间步数
t_vec = (0:N_steps-1) * T;
% 传感器位置（单个固定传感器）
x_sensor = [0; 0];
% 真实目标运动（匀速直线）
x0_true = [1000; 5000]; % 初始位置 [m]
v0_true = [50; -30]; % 初始速度 [m/s]
% 真实轨迹
x_true = zeros(2, N_steps);
v_true = zeros(2, N_steps);
for k = 1:N_steps
x_true(:,k) = x0_true + v0_true * (k-1) * T;
v_true(:,k) = v0_true;
end
% 观测噪声（AOA 标准差 2°）
sigma_psi = deg2rad(2);
R_meas = sigma_psi^2;
% 过程噪声（连续白噪声加速度）
q = 0.1; % 过程噪声强度 [m^2/s^3]
Q = q * [T^4/4, 0, T^3/2, 0;
0, T^4/4, 0, T^3/2;
T^3/2, 0, T^2, 0;
0, T^3/2, 0, T^2];
fprintf('======= EKF 跟踪参数 =======\n');
fprintf('采样间隔 T = %d s, 总步数 = %d\n', T, N_steps);
fprintf('初始位置: (%.0f, %.0f) m\n', x0_true);
fprintf('初始速度: (%.0f, %.0f) m/s\n', v0_true);
fprintf('AOA 噪声: %.1f°\n', rad2deg(sigma_psi));
fprintf('过程噪声 q = %.2f m^2/s^3\n', q);
%% ==================== 生成 AOA 观测 ====================
z_meas = zeros(1, N_steps);
for k = 1:N_steps
dx = x_true(1,k) - x_sensor(1);
dy = x_true(2,k) - x_sensor(2);
psi_true = atan2(dy, dx);
z_meas(k) = psi_true + sigma_psi * randn();
end
fprintf('\n生成了 %d 个 AOA 观测\n', N_steps);
%% ==================== EKF 初始化 ====================
% 初始状态估计（有偏）
x_est_init = [500; 3000; 0; 0];
P_init = diag([1e6, 1e6, 1e4, 1e4]); % 初始协方差（很大，表示不确定）
% 状态转移矩阵
F = [1, 0, T, 0;
0, 1, 0, T;
0, 0, 1, 0;
0, 0, 0, 1];
% 存储
x_est = zeros(4, N_steps);
P_est = zeros(4, 4, N_steps);
K_store = zeros(4, N_steps);
innovation = zeros(1, N_steps);
x_curr = x_est_init;
P_curr = P_init;
%% ==================== EKF 主循环 ====================
fprintf('\n======= 开始 EKF 跟踪 =======\n');
for k = 1:N_steps
% ---- 预测步 ----
x_pred = F * x_curr;
P_pred = F * P_curr * F' + Q;
% ---- 观测雅可比矩阵 ----
dx_hat = x_pred(1) - x_sensor(1);
dy_hat = x_pred(2) - x_sensor(2);
R2 = dx_hat^2 + dy_hat^2;
H = [-dy_hat/R2, dx_hat/R2, 0, 0];
% ---- 观测预测 ----
psi_pred = atan2(dy_hat, dx_hat);
% ---- 卡尔曼增益 ----
S = H * P_pred * H' + R_meas;
K = P_pred * H' / S;
% ---- 更新步 ----
y_k = z_meas(k) - psi_pred;
% 角度卷绕（重要！）
y_k = mod(y_k + pi, 2*pi) - pi;
x_curr = x_pred + K * y_k;
P_curr = (eye(4) - K * H) * P_pred;
% 存储
x_est(:, k) = x_curr;
P_est(:, :, k) = P_curr;
K_store(:, k) = K;
innovation(k) = y_k;
% 打印进度
if mod(k, 20) == 0
fprintf(' 步骤 %d / %d\n', k, N_steps);
end
end
fprintf('EKF 完成\n');
% 误差统计
err_pos = x_est(1:2, :) - x_true;
err_vel = x_est(3:4, :) - v_true;
rmse_pos = sqrt(mean(sum(err_pos.^2, 1)));
rmse_vel = sqrt(mean(sum(err_vel.^2, 1)));
fprintf('\n位置 RMSE: %.2f m\n', rmse_pos);
fprintf('速度 RMSE: %.2f m/s\n', rmse_vel);
%% ==================== 图1：目标轨迹与 EKF 估计 ====================
figure('Name','Fig1 Trajectory','Position',[80 80 900 700]);
hold on;
% 真实轨迹
plot(x_true(1,:)/1e3, x_true(2,:)/1e3, 'b-', 'LineWidth', 2.5, ...
'DisplayName', '真实轨迹');
% EKF 估计
plot(x_est(1,:)/1e3, x_est(2,:)/1e3, 'r--', 'LineWidth', 2, ...
'DisplayName', 'EKF 估计');
% 起点和终点
plot(x_true(1,1)/1e3, x_true(2,1)/1e3, 'go', 'MarkerSize', 12, ...
'MarkerFaceColor', 'g', 'DisplayName', '起点');
plot(x_true(1,end)/1e3, x_true(2,end)/1e3, 'rs', 'MarkerSize', 12, ...
'MarkerFaceColor', 'r', 'DisplayName', '终点');
% 传感器
plot(x_sensor(1)/1e3, x_sensor(2)/1e3, 'ks', 'MarkerSize', 20, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
% 每隔10步画一条测向线
for k = 1:10:N_steps
psi = z_meas(k);
r_plot = 6e3;
plot([x_sensor(1), x_sensor(1) + r_plot*cos(psi)]/1e3, ...
[x_sensor(2), x_sensor(2) + r_plot*sin(psi)]/1e3, ...
'Color', [0.7, 0.7, 0.7], 'LineWidth', 0.5, ...
'HandleVisibility', 'off');
end
xlabel('东向 [km]'); ylabel('北向 [km]');
title('EKF 动态跟踪：真实轨迹 vs 估计轨迹');
legend('Location', 'northwest');
axis equal; grid on;
%% ==================== 图2：位置误差随时间 ====================
figure('Name','Fig2 Position Error','Position',[100 100 900 600]);
subplot(2,1,1);
plot(t_vec, err_pos(1,:), 'b-', 'LineWidth', 1.5, 'DisplayName', 'x 误差');
hold on;
plot(t_vec, err_pos(2,:), 'r-', 'LineWidth', 1.5, 'DisplayName', 'y 误差');
xlabel('时间 [s]'); ylabel('位置误差 [m]');
title('位置误差随时间演化');
grid on; legend('Location', 'northeast');
% 叠加 3σ 包络
sigma_x = zeros(1, N_steps);
sigma_y = zeros(1, N_steps);
for k = 1:N_steps
sigma_x(k) = 3*sqrt(P_est(1,1,k));
sigma_y(k) = 3*sqrt(P_est(2,2,k));
end
plot(t_vec, sigma_x, 'b--', 'LineWidth', 1, 'DisplayName', '3\sigma_x');
plot(t_vec, -sigma_x, 'b--', 'LineWidth', 1, 'HandleVisibility', 'off');
plot(t_vec, sigma_y, 'r--', 'LineWidth', 1, 'DisplayName', '3\sigma_y');
plot(t_vec, -sigma_y, 'r--', 'LineWidth', 1, 'HandleVisibility', 'off');
subplot(2,1,2);
plot(t_vec, sqrt(sum(err_pos.^2, 1)), 'k-', 'LineWidth', 2);
xlabel('时间 [s]'); ylabel('位置误差范数 [m]');
title('总位置误差');
grid on;
sgtitle('EKF 误差收敛过程', 'FontSize', 14, 'FontWeight', 'bold');
%% ==================== 图3：速度估计 ====================
figure('Name','Fig3 Velocity','Position',[120 120 900 600]);
subplot(2,1,1);
plot(t_vec, v_true(1,:), 'b-', 'LineWidth', 2, 'DisplayName', '真实 vx');
hold on;
plot(t_vec, x_est(3,:), 'r--', 'LineWidth', 2, 'DisplayName', 'EKF vx');
plot(t_vec, v_true(2,:), 'g-', 'LineWidth', 2, 'DisplayName', '真实 vy');
plot(t_vec, x_est(4,:), 'm--', 'LineWidth', 2, 'DisplayName', 'EKF vy');
xlabel('时间 [s]'); ylabel('速度 [m/s]');
title('速度估计');
grid on; legend('Location', 'northeast');
subplot(2,1,2);
plot(t_vec, err_vel(1,:), 'b-', 'LineWidth', 1.5, 'DisplayName', 'vx 误差');
hold on;
plot(t_vec, err_vel(2,:), 'r-', 'LineWidth', 1.5, 'DisplayName', 'vy 误差');
xlabel('时间 [s]'); ylabel('速度误差 [m/s]');
title('速度误差');
grid on; legend('Location', 'northeast');
sgtitle('EKF 速度估计：从零初始化到收敛', ...
'FontSize', 14, 'FontWeight', 'bold');
%% ==================== 图4：协方差椭圆演化 ====================
figure('Name','Fig4 Covariance Ellipse','Position',[140 140 900 700]);
hold on;
% 画真实轨迹
plot(x_true(1,:)/1e3, x_true(2,:)/1e3, 'b-', 'LineWidth', 2, ...
'DisplayName', '真实轨迹');
% 在几个时刻画协方差椭圆
idx_show = [1, 10, 25, 50, 100];
theta_ell = linspace(0, 2*pi, 100);
for i = 1:numel(idx_show)
k = idx_show(i);
P_pos = P_est(1:2, 1:2, k);
[V, D] = eig(P_pos);
scale = 3; % 3σ 椭圆
ellipse = V * sqrt(scale^2 * D) * [cos(theta_ell); sin(theta_ell)];
ellipse = ellipse + x_est(1:2, k);
plot(ellipse(1,:)/1e3, ellipse(2,:)/1e3, 'r-', 'LineWidth', 1.5);
plot(x_est(1,k)/1e3, x_est(2,k)/1e3, 'ro', 'MarkerSize', 8, ...
'MarkerFaceColor', 'r');
end
% 传感器
plot(x_sensor(1)/1e3, x_sensor(2)/1e3, 'ks', 'MarkerSize', 20, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
xlabel('东向 [km]'); ylabel('北向 [km]');
title('EKF 协方差椭圆演化：不确定性逐步缩小');
legend('Location', 'northwest');
axis equal; grid on;
%% ==================== 图5：卡尔曼增益演化 ====================
figure('Name','Fig5 Kalman Gain','Position',[160 160 900 600]);
plot(t_vec, K_store(1,:), 'b-', 'LineWidth', 2, 'DisplayName', 'K_x');
hold on;
plot(t_vec, K_store(2,:), 'r-', 'LineWidth', 2, 'DisplayName', 'K_y');
plot(t_vec, K_store(3,:), 'g-', 'LineWidth', 2, 'DisplayName', 'K_{vx}');
plot(t_vec, K_store(4,:), 'm-', 'LineWidth', 2, 'DisplayName', 'K_{vy}');
xlabel('时间 [s]'); ylabel('卡尔曼增益');
title('卡尔曼增益随时间演化：从"信观测"到"信预测"');
grid on; legend('Location', 'northeast');
ylim([0, 1]);
% 标注过渡
text(20, 0.85, '初期：不确定大 → 增益大', 'FontSize', 10, 'Color', 'b');
text(60, 0.25, '后期：不确定小 → 增益小', 'FontSize', 10, 'Color', 'r');
sgtitle('卡尔曼增益：EKF 的"信任分配器"', ...
'FontSize', 14, 'FontWeight', 'bold');
%% ==================== 图6：几何退化场景 ====================
fprintf('\n======= 几何退化场景 =======\n');
% 目标与传感器几乎共线（径向运动）
x0_deg = [1000; 0];
v0_deg = [50; 0]; % 沿径向飞行
x_true_deg = zeros(2, N_steps);
for k = 1:N_steps
x_true_deg(:,k) = x0_deg + v0_deg * (k-1) * T;
end
% 生成观测
z_deg = zeros(1, N_steps);
for k = 1:N_steps
dx = x_true_deg(1,k) - x_sensor(1);
dy = x_true_deg(2,k) - x_sensor(2);
psi_true = atan2(dy, dx);
z_deg(k) = psi_true + sigma_psi * randn();
end
% EKF
x_curr = [500; 500; 0; 0];
P_curr = diag([1e6, 1e4, 1e4, 1e4]); % y 方向不确定度低
x_est_deg = zeros(4, N_steps);
for k = 1:N_steps
x_pred = F * x_curr;
P_pred = F * P_curr * F' + Q;
dx_hat = x_pred(1) - x_sensor(1);
dy_hat = x_pred(2) - x_sensor(2);
R2 = dx_hat^2 + dy_hat^2;
H = [-dy_hat/R2, dx_hat/R2, 0, 0];
psi_pred = atan2(dy_hat, dx_hat);
S = H * P_pred * H' + R_meas;
K = P_pred * H' / S;
y_k = z_deg(k) - psi_pred;
y_k = mod(y_k + pi, 2*pi) - pi;
x_curr = x_pred + K * y_k;
P_curr = (eye(4) - K * H) * P_pred;
x_est_deg(:, k) = x_curr;
end
err_deg = x_est_deg(1:2, :) - x_true_deg;
rmse_deg = sqrt(mean(sum(err_deg.^2, 1)));
fprintf('退化场景位置 RMSE: %.2f m\n', rmse_deg);
figure('Name','Fig6 Degenerate Geometry','Position',[180 180 900 600]);
hold on;
plot(x_true_deg(1,:)/1e3, x_true_deg(2,:)/1e3, 'b-', 'LineWidth', 2.5, ...
'DisplayName', '真实轨迹（径向）');
plot(x_est_deg(1,:)/1e3, x_est_deg(2,:)/1e3, 'r--', 'LineWidth', 2, ...
'DisplayName', 'EKF 估计');
plot(x_sensor(1)/1e3, x_sensor(2)/1e3, 'ks', 'MarkerSize', 20, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
% 测向线
for k = 1:15:N_steps
psi = z_deg(k);
r_plot = 6e3;
plot([x_sensor(1), x_sensor(1) + r_plot*cos(psi)]/1e3, ...
[x_sensor(2), x_sensor(2) + r_plot*sin(psi)]/1e3, ...
'Color', [0.7, 0.7, 0.7], 'LineWidth', 0.5, ...
'HandleVisibility', 'off');
end
xlabel('东向 [km]'); ylabel('北向 [km]');
title(sprintf('几何退化场景：目标沿径向飞行（RMSE = %.0f m）', rmse_deg));
legend('Location', 'northwest');
axis equal; grid on;
fprintf('\n对比:\n');
fprintf(' 普通场景 RMSE: %.2f m\n', rmse_pos);
fprintf(' 退化场景 RMSE: %.2f m\n', rmse_deg);
fprintf(' 比值: %.1f 倍\n', rmse_deg / rmse_pos);
fprintf('\n======= 全部完成 =======\n');四、代码讲解：为什么这么写？
4.1 为什么初始化协方差要"很大"？
matlab
P_init = diag([1e6, 1e6, 1e4, 1e4]);初值 PPP 表示"我们对初始状态有多不确定"。设成 10610^6106 m² 意味着位置标准差约 1000 m------很大，但不过分。
为什么不能用更大值？ 如果初始协方差设得极大（如 101210^{12}1012），第一次更新时卡尔曼增益 K→H−1\mathbf{K} \to \mathbf{H}^{-1}K→H−1 会让状态估计完全依赖观测 ，可能被噪声带偏。P0P_0P0 反映真实的先验不确定度，不能胡乱设。
4.2 角度新息必须"卷绕"
matlab
y_k = z_meas(k) - psi_pred;
y_k = mod(y_k + pi, 2*pi) - pi;这是 EKF 处理角度时的关键细节 。假设观测是 179°179°179°，预测是 −179°-179°−179°，直接相减得 358°358°358°------看起来误差巨大，实际只有 2°2°2°。
用 mod(y + π, 2π) - π 把角度差映射到 −π,π-π, π−π,π，消除 2π2π2π 跳变。忘记这一步是 EKF 跟踪中最常见的 bug。
4.3 雅可比矩阵的推导
matlab
H = [-dy_hat/R2, dx_hat/R2, 0, 0];对 ψ=atan2(Δy,Δx)\psi = \text{atan2}(\Delta y, \Delta x)ψ=atan2(Δy,Δx) 求偏导：
∂ψ∂x=∂∂xatan2(y−ys,x−xs)=−ΔyR2\frac{\partial \psi}{\partial x} = \frac{\partial}{\partial x}\text{atan2}(y - y_s, x - x_s) = \frac{-\Delta y}{R^2}∂x∂ψ=∂x∂atan2(y−ys,x−xs)=R2−Δy
∂ψ∂y=ΔxR2\frac{\partial \psi}{\partial y} = \frac{\Delta x}{R^2}∂y∂ψ=R2Δx
对速度的偏导为零 ------因为 AOA 测量只依赖位置 ，不依赖速度。这会导致一个重要后果：单传感器 AOA 无法直接测速（速度只能通过运动模型间接推断）。
4.4 为什么卡尔曼增益会逐渐变小？
matlab
S = H * P_pred * H' + R_meas;
K = P_pred * H' / S;物理解释 ：K\mathbf{K}K 是"信任分配器"：
K≈预测不确定度预测不确定度+观测噪声\mathbf{K} \approx \frac{\text{预测不确定度}}{\text{预测不确定度} + \text{观测噪声}}K≈预测不确定度+观测噪声预测不确定度
- 初期 ：P\mathbf{P}P 很大（初值不确定），K\mathbf{K}K 接近 1------主要信观测；
- 收敛后 ：P\mathbf{P}P 变小，K\mathbf{K}K 也变小------主要信预测。
图5清晰展示了这个演化过程。
4.5 几何退化场景：为什么"沿径向飞行"特别难？
当目标沿传感器到目标的径向运动时：
- 目标距离在变，但方位角几乎不变；
- AOA 观测只能看到极其微小的角度变化；
- 切向位置（垂直径向）几乎不可观测。
后果 ：切向位置估计误差持续累积，EKF 只能靠过程噪声模型"外推"。图6显示，退化场景的 RMSE 比普通场景大一个量级。
工程对策：
- 多传感器：加入第二个不同方向的传感器，恢复可观测性；
- 增加观测模态：TDOA 对径向运动的敏感度远高于 AOA；
- 限制过程噪声 ：如果目标运动平滑，减小 qqq 可以抑制外推偏差。
五、运行结果解读
运行代码后，你将看到 6 张图：
图1：目标轨迹与 EKF 估计
- 蓝线：真实轨迹（匀速直线）；
- 红线 ：EKF 估计------几乎与真实轨迹重合；
- 灰色射线 ：每 10 秒一次的测向线------它们方向一致，但随目标移动；
- 黑方块：单传感器位置。
图2：位置误差
- 上图 ：x/y 方向误差 + 3σ 置信包络。误差在前 10 秒内快速收敛，之后稳定；
- 下图：位置误差范数------从初始 2000 m 降到收敛后约 50 m。
图3：速度估计
EKF 从零速度初始化出发，在约 20 秒内收敛到真实速度 (50,−30)(50, -30)(50,−30) m/s。关键 ：AOA 本身不测速，EKF 通过位置变化率的隐式观测推断速度。
图4：协方差椭圆
五个时刻的 3σ 椭圆从巨大 （初值）迅速缩小，并沿切向拉长------这就是单传感器 AOA 的固有几何限制。
图5：卡尔曼增益
四条曲线从接近 1 逐步降到约 0.1------从"信观测"到"信预测"的过渡，是 EKF 收敛的标志。
图6：几何退化场景
目标沿径向飞行时，估计轨迹严重偏离真实轨迹 ，RMSE 比普通场景大 10 倍以上 。这是 EKF 在单传感器 AOA 下的固有短板。
六、工程实战的四条准则
| 准则 | 数学依据 | 工程做法 |
|---|---|---|
| 1. 初始化 P0P_0P0 要合理 | 过大导致首步被噪声带偏 | 反映真实先验不确定度 |
| 2. 角度新息必须卷绕 | 2π2π2π 跳变会"炸掉"EKF | mod(y+π, 2π) - π |
| 3. 过程噪声 qqq 要调 | qqq 太小 → 跟不上机动；qqq 太大 → 噪声放大 | 用 Allan 方差或 IMU 数据标定 |
| 4. 几何退化必须避免 | 径向运动 → 切向不可观测 | 多传感器 / 多模态 |
七、延伸思考
7.1 EKF vs UKF vs 粒子滤波
| 滤波器 | 非线性处理 | 计算量 | 适用场景 |
|---|---|---|---|
| EKF | 一阶泰勒展开 | 低 | 弱非线性，实时性高 |
| UKF | Sigma 点采样 | 中 | 中等非线性，精度高 |
| PF | 蒙特卡洛采样 | 高 | 强非线性、非高斯 |
电子战中的选择：大多数场景 EKF 就够------目标距离远、运动平滑，一阶近似足够。只有近距离高机动（如末段拦截）才需要 UKF/PF。
7.2 多传感器 EKF
如果同时有多个传感器给出 AOA/TDOA 观测，可以在 EKF 中顺序更新 （每个传感器一次）或批量更新 （把所有观测堆叠）。顺序更新 计算量小，批量更新精度略高。
7.3 与后续篇章的关系
第二季到这里完成了估计理论与动态跟踪 三篇（第1921篇）。接下来第十篇（第2224篇）我们将进入误差分析与传感器误差：
- 第22篇：误差椭圆与置信区间------如何画出漂亮的 95% 椭圆；
- 第23篇：几何精度因子（GDOP）与 CEP₅₀------衡量定位系统"好坏"的标准；
- 第24篇：传感器位置/速度误差对定位的影响------GPS 抖动如何毁掉你的定位精度。
下一篇预告（第22篇）：误差椭圆与置信区间
你的 EKF 输出一个位置估计和一个协方差矩阵 P\mathbf{P}P。但用户不想要矩阵，他们想要一张图 ------"目标在这个椭圆里的概率是 95%"。
误差椭圆就是这张图。我们将会：
- 从协方差矩阵 P\mathbf{P}P 出发，推导置信椭圆的参数（长轴、短轴、旋转角）；
- 解释 1σ vs 2σ vs 95% 的区别------为什么椭圆大小不一样；
- 引入 CEP₅₀（圆概率误差） ------一个比椭圆更"用户友好"的指标；
- 用 MATLAB 复现原书第9章的图9.1~9.4，画出漂亮的误差椭圆。
敬请期待！