【MATLAB例程】基于IMM-EKF与AOA/测距融合的二维移动目标跟踪定位

程序基于四基站二维移动目标定位模型,采用交互式多模型与扩展卡尔曼滤波(IMM-EKF)融合AOA角度和距离测量,实现机动目标的位置与速度联合估计。绘制二维轨迹、位置误差、速度误差及IMM模型概率等曲线,对比直接解算与IMM-EKF的定位效果,输出均方根误差(RMSE)、最大误差等指标。基站数量、测量噪声、模型转移概率及滤波参数均可自行调整。

原创代码,请勿翻卖。有中文注释

文章目录

运行结果

轨迹和锚点位置绘图:

XY轴状态曲线:

位置误差曲线:

IMM中各模型的概率曲线:

MATLAB源代码

部分代码展示如下:

matlab 复制代码
% 二维移动目标IMM-EKF定位,AOA定位为主、测距辅助
% 作者微信:matlabfilter(可联系获取完整例程、个性化定制和讲解)
% 2026-10-24/2D:由三维IMM-EKF版本重新整理为二维平面版本

clear; clc; close all;
rng(0);

num_station = 4;
filter_type = 'EKF';
scene_name = '4基站移动目标IMM-EKF二维IMM-EKF';

%% 参数设置
num_step = 180;
dt = 0.2;
station_scale = 35;
AOA_noise = 1e-2;
distance_noise = 0.1;
model_transition = [0.96, 0.04;
                    0.05, 0.95];

if num_station < 2 || mod(num_station, 1) ~= 0
    error('num_station必须为不小于2的整数,二维定位建议设置为2或更多。');
end

t_axis = (0:num_step-1)' * dt;

%% 构造二维移动目标轨迹
maneuver_acc = zeros(num_step, 2);
for k = 1:num_step
    t = t_axis(k);
    if t < 6
        maneuver_acc(k, :) = [0.00, 0.00];
    elseif t < 12
        maneuver_acc(k, :) = [0.85, -0.55];
    elseif t < 18
        maneuver_acc(k, :) = [0.00, 0.00];
    elseif t < 25
        maneuver_acc(k, :) = [-0.95, 0.75];
    elseif t < 31
        maneuver_acc(k, :) = [0.00, 0.00];
    else
        maneuver_acc(k, :) = [0.55, 0.42];
    end
end

position = zeros(num_step, 2);
true_velocity = zeros(num_step, 2);
position(1, :) = [-18.0, -10.0];
true_velocity(1, :) = [2.60, 0.90];

for k = 2:num_step
    a = maneuver_acc(k-1, :);
    true_velocity(k, :) = true_velocity(k-1, :) + a * dt;
    position(k, :) = position(k-1, :) + true_velocity(k-1, :) * dt + 0.5 * a * dt^2;
end
true_speed = vecnorm(true_velocity, 2, 2);

stations_position = station_scale * randn(num_station, 2);

%% 方位角/距离量测生成
measurement_dim = 2 * num_station;
R = kron(eye(num_station), diag([AOA_noise^2, distance_noise^2]));
measurements = zeros(measurement_dim, num_step);
direct_positions = zeros(num_step, 2);

for k = 1:num_step
    measurements(:, k) = makeAoaRangeMeasurement2D( ...
        stations_position, position(k, :), AOA_noise, distance_noise);
    direct_positions(k, :) = directPositionFromMeasurement2D(stations_position, measurements(:, k));
end

%% IMM非线性滤波
[imm_positions, imm_velocity, model_probability] = runImmNonlinear2D( ...
    measurements, direct_positions, stations_position, dt, R, model_transition, filter_type);

direct_velocity = finiteDifferenceVelocity2D(direct_positions, dt);
direct_speed = vecnorm(direct_velocity, 2, 2);
imm_speed = vecnorm(imm_velocity, 2, 2);

%% 误差统计
err_direct = direct_positions - position;
err_imm = imm_positions - position;
dist_err_direct = vecnorm(err_direct, 2, 2);
dist_err_imm = vecnorm(err_imm, 2, 2);

velocity_err_direct = vecnorm(direct_velocity - true_velocity, 2, 2);
velocity_err_imm = vecnorm(imm_velocity - true_velocity, 2, 2);
speed_err_direct = abs(direct_speed - true_speed);
speed_err_imm = abs(imm_speed - true_speed);

axis_rmse = [
    sqrt(mean(err_direct.^2, 1));
    sqrt(mean(err_imm.^2, 1))
];

position_stats = [
    mean(dist_err_direct), sqrt(mean(dist_err_direct.^2)), max(dist_err_direct), std(dist_err_direct, 0);
    mean(dist_err_imm), sqrt(mean(dist_err_imm.^2)), max(dist_err_imm), std(dist_err_imm, 0)
];

velocity_stats = [
    mean(velocity_err_direct), sqrt(mean(velocity_err_direct.^2)), max(velocity_err_direct), std(velocity_err_direct, 0);
    mean(velocity_err_imm), sqrt(mean(velocity_err_imm.^2)), max(velocity_err_imm), std(velocity_err_imm, 0);
    mean(speed_err_direct), sqrt(mean(speed_err_direct.^2)), max(speed_err_direct), std(speed_err_direct, 0);
    mean(speed_err_imm), sqrt(mean(speed_err_imm.^2)), max(speed_err_imm), std(speed_err_imm, 0)
];

%% 颜色设置
anchor_colors = lines(num_station);
color_true = [0.00, 0.00, 0.00];
color_direct = [0.85, 0.33, 0.10];
color_imm = [0.47, 0.67, 0.19];
filter_label = ['IMM-', filter_type];

%% 图
相关推荐
水桶学习吧_1 天前
基于MATLAB的圆锥滚子轴承故障诊断仿真
matlab·故障诊断·圆锥滚子轴承
Evand J2 天前
【MATLAB例程】基于IMM-CKF的AOA+测距融合的二维移动目标跟踪定位
开发语言·matlab·目标跟踪
机器学习之心3 天前
物理信息神经网络与扩展物理信息神经网络(PINN/XPINN)求解功能梯度材料稳态—瞬态热传导,MATLAB代码
matlab·物理信息神经网络
普通人63 天前
PHP多维数组排序完全指南
开发语言·php·1024程序员节
普通人65 天前
WebGL与WebGPU入门指南
开发语言·webgl·1024程序员节
猫猫不是喵喵.6 天前
从0到1创建一个微服务项目
java·数据库·微服务·1024程序员节
Evand J6 天前
【MATLAB例程,PMSM19】永磁同步电机(PMSM)基于多速率无迹卡尔曼滤波(UKF)的转速与转子位置估计。测量缺失、状态预测与恢复性能分析
开发语言·matlab·电机·ekf·卡尔曼·电机滤波
专注仿真7 天前
知识问答模型用到的算法集详解及代码实例
python·算法·代码·大模型训练·控制算法
星夜离尘567 天前
MATLAB 离散数据分箱:半开区间、末端点与计数、加权和、平均
算法·matlab