基于扩展卡尔曼滤波(EKF)的雷达与红外数据融合的MATLAB实现。将融合雷达的距离/方位角测量和红外的方位角/俯仰角测量。
% EKF雷达与红外数据融合
% 目标跟踪：雷达提供距离和方位角，红外提供方位角和俯仰角
clear; clc; close all;
%% 参数设置
dt = 0.1; % 采样时间(s)
T = 30; % 总仿真时间(s)
N = T/dt; % 总步数
% 过程噪声协方差
q = 0.1;
Q = diag([q, q, q, q, q, q]);
% 雷达测量噪声协方差
sigma_r = 5; % 距离噪声(m)
sigma_az_r = 0.5*pi/180; % 方位角噪声(rad)
R_radar = diag([sigma_r^2, sigma_az_r^2]);
% 红外测量噪声协方差
sigma_az_ir = 0.3*pi/180; % 方位角噪声(rad)
sigma_el = 0.4*pi/180; % 俯仰角噪声(rad)
R_ir = diag([sigma_az_ir^2, sigma_el^2]);
%% 真实轨迹生成 (三维匀速运动)
X_true = zeros(6, N);
% 初始状态 [x, y, z, vx, vy, vz]
X_true(:,1) = [1000, 500, 300, -50, 20, -5]';
for k = 2:N
% 状态转移矩阵
F = [1, 0, 0, dt, 0, 0;
0, 1, 0, 0, dt, 0;
0, 0, 1, 0, 0, dt;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
X_true(:,k) = F * X_true(:,k-1) + sqrt(Q) * randn(6,1);
end
%% 生成观测数据
Z_radar = zeros(2, N); % 雷达观测: [距离, 方位角]
Z_ir = zeros(2, N); % 红外观测: [方位角, 俯仰角]
for k = 1:N
x = X_true(1,k); y = X_true(2,k); z = X_true(3,k);
% 雷达观测
r = sqrt(x^2 + y^2 + z^2); % 距离
az_r = atan2(y, x); % 方位角
Z_radar(:,k) = [r; az_r] + sqrt(R_radar) * randn(2,1);
% 红外观测
az_ir = atan2(y, x); % 方位角
el = atan2(z, sqrt(x^2 + y^2)); % 俯仰角
Z_ir(:,k) = [az_ir; el] + sqrt(R_ir) * randn(2,1);
end
%% EKF初始化
X_est = zeros(6, N);
P_est = zeros(6, 6, N);
% 初始状态估计 (使用第一次雷达观测进行初始化)
r0 = Z_radar(1,1);
az0 = Z_radar(2,1);
X_est(:,1) = [r0*cos(az0); r0*sin(az0); 0; 0; 0; 0];
P_est(:,:,1) = diag([100, 100, 100, 10, 10, 10]);
%% EKF主循环
for k = 2:N
% 预测步骤
F = [1, 0, 0, dt, 0, 0;
0, 1, 0, 0, dt, 0;
0, 0, 1, 0, 0, dt;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
X_pred = F * X_est(:,k-1);
P_pred = F * P_est(:,:,k-1) * F' + Q;
% 更新步骤1: 雷达数据更新
if mod(k,2) == 0 % 雷达更新频率
[X_pred, P_pred] = radar_update(X_pred, P_pred, Z_radar(:,k), R_radar);
end
% 更新步骤2: 红外数据更新
if mod(k,3) == 0 % 红外更新频率
[X_pred, P_pred] = ir_update(X_pred, P_pred, Z_ir(:,k), R_ir);
end
X_est(:,k) = X_pred;
P_est(:,:,k) = P_pred;
end
%% 雷达更新函数
function [X_updated, P_updated] = radar_update(X_pred, P_pred, Z_radar, R_radar)
x = X_pred(1); y = X_pred(2); z = X_pred(3);
% 预测观测
r_pred = sqrt(x^2 + y^2 + z^2);
az_pred = atan2(y, x);
Z_pred = [r_pred; az_pred];
% 观测矩阵H
H = zeros(2,6);
H(1,1) = x/r_pred; H(1,2) = y/r_pred; H(1,3) = z/r_pred;
H(2,1) = -y/(x^2+y^2); H(2,2) = x/(x^2+y^2);
% EKF更新
y_residual = Z_radar - Z_pred;
% 方位角残差归一化到[-pi, pi]
y_residual(2) = mod(y_residual(2) + pi, 2*pi) - pi;
S = H * P_pred * H' + R_radar;
K = P_pred * H' / S;
X_updated = X_pred + K * y_residual;
P_updated = (eye(6) - K * H) * P_pred;
end
%% 红外更新函数
function [X_updated, P_updated] = ir_update(X_pred, P_pred, Z_ir, R_ir)
x = X_pred(1); y = X_pred(2); z = X_pred(3);
% 预测观测
az_pred = atan2(y, x);
el_pred = atan2(z, sqrt(x^2 + y^2));
Z_pred = [az_pred; el_pred];
% 观测矩阵H
H = zeros(2,6);
r_xy = sqrt(x^2 + y^2);
r = sqrt(x^2 + y^2 + z^2);
H(1,1) = -y/(x^2+y^2); H(1,2) = x/(x^2+y^2);
H(2,1) = -x*z/(r_xy*r^2); H(2,2) = -y*z/(r_xy*r^2);
H(2,3) = r_xy/r^2;
% EKF更新
y_residual = Z_ir - Z_pred;
% 角度残差归一化
y_residual(1) = mod(y_residual(1) + pi, 2*pi) - pi;
y_residual(2) = mod(y_residual(2) + pi, 2*pi) - pi;
S = H * P_pred * H' + R_ir;
K = P_pred * H' / S;
X_updated = X_pred + K * y_residual;
P_updated = (eye(6) - K * H) * P_pred;
end
%% 结果分析
% 位置误差
pos_error = sqrt((X_true(1,:) - X_est(1,:)).^2 + ...
(X_true(2,:) - X_est(2,:)).^2 + ...
(X_true(3,:) - X_est(3,:)).^2);
% 速度误差
vel_error = sqrt((X_true(4,:) - X_est(4,:)).^2 + ...
(X_true(5,:) - X_est(5,:)).^2 + ...
(X_true(6,:) - X_est(6,:)).^2);
%% 绘图
figure('Position', [100, 100, 1200, 800]);
% 三维轨迹
subplot(2,3,1);
plot3(X_true(1,:), X_true(2,:), X_true(3,:), 'b-', 'LineWidth', 2); hold on;
plot3(X_est(1,:), X_est(2,:), X_est(3,:), 'r--', 'LineWidth', 1.5);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('三维轨迹');
legend('真实轨迹', 'EKF估计');
grid on;
% XY平面轨迹
subplot(2,3,2);
plot(X_true(1,:), X_true(2,:), 'b-', 'LineWidth', 2); hold on;
plot(X_est(1,:), X_est(2,:), 'r--', 'LineWidth', 1.5);
xlabel('X (m)'); ylabel('Y (m)');
title('XY平面轨迹');
legend('真实轨迹', 'EKF估计');
grid on;
% 位置误差
subplot(2,3,3);
plot((1:N)*dt, pos_error, 'LineWidth', 2);
xlabel('时间 (s)'); ylabel('位置误差 (m)');
title('位置估计误差');
grid on;
% 速度误差
subplot(2,3,4);
plot((1:N)*dt, vel_error, 'LineWidth', 2);
xlabel('时间 (s)'); ylabel('速度误差 (m/s)');
title('速度估计误差');
grid on;
% X坐标对比
subplot(2,3,5);
plot((1:N)*dt, X_true(1,:), 'b-', 'LineWidth', 2); hold on;
plot((1:N)*dt, X_est(1,:), 'r--', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('X坐标 (m)');
title('X坐标估计');
legend('真实值', '估计值');
grid on;
% Y坐标对比
subplot(2,3,6);
plot((1:N)*dt, X_true(2,:), 'b-', 'LineWidth', 2); hold on;
plot((1:N)*dt, X_est(2,:), 'r--', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('Y坐标 (m)');
title('Y坐标估计');
legend('真实值', '估计值');
grid on;
%% 性能统计
fprintf('=== EKF雷达红外数据融合性能统计 ===\n');
fprintf('平均位置误差: %.2f m\n', mean(pos_error(100:end)));
fprintf('平均速度误差: %.2f m/s\n', mean(vel_error(100:end)));
fprintf('最终位置误差: %.2f m\n', pos_error(end));
fprintf('最终速度误差: %.2f m/s\n', vel_error(end));
这个代码的主要特点：
说明
-
传感器模型：
- 雷达：提供距离和方位角测量
- 红外：提供方位角和俯仰角测量
-
EKF实现：
- 预测步骤：使用匀速运动模型
- 更新步骤：分别处理雷达和红外数据
- 考虑角度测量的周期性
-
数据融合策略：
- 异步数据融合（不同更新频率）
- 顺序更新处理多传感器数据
改进点
% 可以进一步改进的方面：
% 1. 自适应噪声调整
% if std(pos_error(end-9:end)) > threshold
% Q = adjust_process_noise(Q);
% end
% 2. 传感器失效检测
% if innovation_norm > threshold
% % 使用单一传感器或降低权重
% end
% 3. 非线性更强的运动模型
% F = get_jacobian(X_est(:,k-1), dt); % 对于机动目标
参考代码 基于EKF的雷达与红外数据融合 www.youwenfan.com/contentcsh/59706.html
建议
- 参数调优：根据实际传感器特性调整噪声参数
- 初始化：改进初始状态估计方法
- 实时性：对于实时应用，优化矩阵运算效率
