当前位置：服务支持 > 软件文章 > EKF扩展卡尔曼滤波雷达红外数据融合MATLAB实现

EKF扩展卡尔曼滤波雷达红外数据融合MATLAB实现

阅读数 2

基于扩展卡尔曼滤波(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/contentbib/59706.html