项目场景：
导弹跟踪目标的一阶扩展卡尔曼滤波模拟
雷达位于原点，目标在二维平面内匀速运动。
状态向量: [x, y, vx, vy]' (笛卡尔坐标)
测量: 距离 r, 方位角 theta, 径向速度 v_r
代码实例
例如：数据传输过程中数据不时出现丢失的情况，偶尔会丢失一部分数据
APP 中接收数据代码：
Matlab
%% 导弹跟踪目标的一阶扩展卡尔曼滤波模拟
% 雷达位于原点，目标在二维平面内匀速运动。
% 状态向量: [x, y, vx, vy]' (笛卡尔坐标)
% 测量: 距离 r, 方位角 theta, 径向速度 v_r
clear; clc; close all;
addpath("./Kalman工具箱/");
%% 1. 模拟参数设置
dt = 0.1; % 采样时间间隔 (秒)
T = 20; % 总仿真时间 (秒)
t = 0:dt:T; % 时间向量
N = length(t); % 采样点数
% 真实初始状态 (x, y, vx, vy)
x0_true = [1000; 1000; 50; -30]; % 起始位置 (1000,1000) m，速度 (50,-30) m/s
% 过程噪声: 加速度随机扰动 (m/s^2)，对速度项施加
% 连续时间白噪声谱密度 qc = 0.5 (m^2/s^3)，离散化得到 Q
qc = 0.5; % 连续时间过程噪声强度 (加速度)
G = [dt^2/2, 0; 0, dt^2/2; dt, 0; 0, dt]; % 噪声输入矩阵 (位置和速度)
Q = G * (qc * eye(2)) * G'; % 离散过程噪声协方差
% 测量噪声: 距离标准差 sigma_r (m), 角度标准差 sigma_theta (rad), 径向速度标准差 sigma_vr (m/s)
sigma_r = 5; % 距离噪声标准差 5 m
sigma_theta = 0.01; % 角度噪声标准差 0.01 rad (~0.57度)
sigma_vr = 1; % 径向速度噪声标准差 1 m/s
R = diag([sigma_r^2, sigma_theta^2, sigma_vr^2]); % 测量噪声协方差矩阵
%% 2. 生成真实轨迹和带噪测量
% 状态真值
x_true = zeros(4, N);
x_true(:,1) = x0_true;
F_true = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; % 状态转移矩阵
for k = 2:N
w = sqrt(Q) * randn(4,1); % 过程噪声 (符合协方差 Q)
x_true(:,k) = F_true * x_true(:,k-1) + w;
end
% 测量: 根据真值计算并添加噪声
z = zeros(3, N); % 测量值: [r; theta; v_r]
for k = 1:N
xk = x_true(:,k);
r = sqrt(xk(1)^2 + xk(2)^2);
theta = atan2(xk(2), xk(1));
v_r = (xk(1)*xk(3) + xk(2)*xk(4)) / r;
z(:,k) = [r; theta; v_r] + sqrt(R) * randn(3,1); % 加性噪声
end
%% 3. EKF 初始化
x_est = zeros(4, N); % 状态估计
P_est = zeros(4,4,N); % 协方差矩阵序列
% 初始估计: 使用第一次测量来初始化 (假设位置由距离和角度计算，速度先设为0)
r0 = z(1,1); theta0 = z(2,1);
x0_est = [r0 * cos(theta0); r0 * sin(theta0); 0; 0];
P0 = diag([sigma_r^2, sigma_r^2, 10^2, 10^2]); % 初始协方差 (位置不确定度来自距离，速度不确定度较大)
x_est(:,1) = x0_est;
P_est(:,:,1) = P0;
%% 4. 定义系统模型参数（数值形式）
% 状态转移矩阵 A (常数)
A = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1];
% 过程噪声的雅可比 W (加性噪声，因此为单位矩阵)
W = eye(4);
% 测量噪声的雅可比 V (加性噪声，因此为单位矩阵)
V = eye(3);
%% 5. EKF 循环
for k = 2:N
% ---- 预测步骤 ----
% 调用 ekf_predict1，使用数值 A, Q, W
[x_pred, P_pred] = ekf_predict1(x_est(:,k-1), P_est(:,:,k-1), A, Q, [], W);
% ---- 更新步骤 ----
% 手动计算测量预测均值 MU 和测量雅可比 H（数值形式）
MU = measurement(x_pred); % 普通函数，无句柄
H = jacobian_measurement(x_pred); % 普通函数，无句柄
% 调用 ekf_update1，传入数值 MU 和 H
[x_est(:,k), P_est(:,:,k), K, ~, ~, ~] = ekf_update1(x_pred, P_pred, z(:,k), H, R, MU, V);
end
%% 6. 结果绘图
% 真实轨迹、估计轨迹和测量点 (笛卡尔坐标)
figure(1); hold on; grid on;
% 真实轨迹
plot(x_true(1,:), x_true(2,:), 'k-', 'LineWidth', 2, 'DisplayName', '真实轨迹');
% 估计轨迹
plot(x_est(1,:), x_est(2,:), 'r--', 'LineWidth', 1.5, 'DisplayName', 'EKF估计');
% 测量点 (将测量转换为笛卡尔坐标)
meas_x = z(1,:) .* cos(z(2,:));
meas_y = z(1,:) .* sin(z(2,:));
plot(meas_x, meas_y, 'b.', 'MarkerSize', 8, 'DisplayName', '测量点');
xlabel('x (m)'); ylabel('y (m)');
title('目标跟踪轨迹');
legend('Location','best');
axis equal;
% 位置误差 (欧氏距离)
pos_err = sqrt((x_true(1,:)-x_est(1,:)).^2 + (x_true(2,:)-x_est(2,:)).^2);
figure(2); plot(t, pos_err, 'LineWidth', 1.5); grid on;
xlabel('时间 (s)'); ylabel('位置误差 (m)');
title('EKF 位置估计误差');
% 速度误差
vel_err_x = x_true(3,:) - x_est(3,:);
vel_err_y = x_true(4,:) - x_est(4,:);
figure(3); plot(t, vel_err_x, 'r-', t, vel_err_y, 'b--', 'LineWidth', 1.5); grid on;
xlabel('时间 (s)'); ylabel('速度误差 (m/s)');
legend('v_x 误差', 'v_y 误差');
title('EKF 速度估计误差');
%% 7. 测量模型函数（普通子函数）
function MU = measurement(x)
% 计算测量预测值: [距离; 方位角; 径向速度]
x1 = x(1); x2 = x(2); x3 = x(3); x4 = x(4);
r = sqrt(x1^2 + x2^2);
theta = atan2(x2, x1);
v_r = (x1*x3 + x2*x4) / r;
MU = [r; theta; v_r];
end
function H = jacobian_measurement(x)
% 计算测量函数的雅可比矩阵 (3x4)
x1 = x(1); x2 = x(2); x3 = x(3); x4 = x(4);
r = sqrt(x1^2 + x2^2);
if r < 1e-6 % 避免除以零
r = 1e-6;
end
dr_dx = [x1/r, x2/r, 0, 0];
dtheta_dx = [-x2/(r^2), x1/(r^2), 0, 0];
dvr_dx = [x3/r - x1*(x1*x3+x2*x4)/(r^3), ...
x4/r - x2*(x1*x3+x2*x4)/(r^3), ...
x1/r, x2/r];
H = [dr_dx; dtheta_dx; dvr_dx];
end结果展示：
