
程序基于四基站二维移动目标定位模型,采用交互式多模型与容积卡尔曼滤波(IMM-CKF)融合AOA角度和距离测量,实现机动目标的位置与速度联合估计。绘制二维跟踪轨迹、位置误差、速度误差及IMM模型概率等曲线,对比直接解算与IMM-CKF的定位效果,输出均方根误差(RMSE)、最大误差等指标。基站数量、测量噪声、模型转移概率及滤波参数均可自行调整。
原创代码,请勿翻卖。可联系我获取代码定制、讲解等
文章目录
程序简介
本程序基于MATLAB搭建四基站二维移动目标定位仿真模型,采用交互式多模型(Interacting Multiple Model,IMM)与容积卡尔曼滤波(Cubature Kalman Filter,CKF)相结合的方法,实现基于AOA角度和距离测量的移动目标定位与运动状态估计。
在移动目标定位过程中,目标可能经历匀速、加速和减速等不同运动状态,单一运动模型难以充分适应所有机动变化。为此,程序引入IMM多模型交互机制,结合CKF处理非线性角度与距离量测,实现目标位置及速度的递推估计。
程序设置角度与距离直接解算、IMM-CKF滤波两种方案,分析多模型动态滤波对移动目标定位和速度估计的影响。
算法与仿真设置
程序采用二维平面定位模型,设置4个固定基站,同时获取目标相对于各基站的AOA方位角及距离测量信息。
主要包含两种定位方案:
- 角度与距离直接解算: 根据各基站的角度和距离测量分别计算目标位置,再取平均得到目标位置估计,并通过位置差分计算速度。
- IMM-CKF动态滤波: 构建包含二维位置、速度和加速度的六维状态模型,通过IMM模型交互、状态预测、CKF非线性量测更新及模型概率融合,实现目标位置与速度联合估计。
IMM设置平稳与机动两类运动模型,主要通过不同的过程噪声协方差体现运动变化程度,并根据量测似然动态调整模型概率。
CKF采用容积点传播方式处理AOA角度及距离构成的非线性量测方程,无需显式计算量测雅可比矩阵。
仿真采用以下参数:
- 基站数量:4个;
- 仿真步数:180步;
- 采样周期:0.2秒;
- AOA角度噪声标准差:0.01 rad;
- 距离测量噪声标准差:0.1 m。
目标轨迹包含匀速及多个分段加速度变化过程,用于模拟不同运动状态下的二维机动目标定位场景。
结果分析
程序主要输出以下结果:
- 二维轨迹定位: 展示四基站分布、目标真实轨迹、角度与距离直接解算轨迹及IMM-CKF滤波轨迹。
- 两轴坐标跟踪: 分别对比目标x、y轴真实位置与两种算法的估计结果。
- 位置误差分析: 绘制两种方法的二维位置误差曲线,分析机动过程中的定位精度。
- 速度估计误差: 对比直接解算位置差分速度与IMM-CKF估计速度的误差,包括速度向量误差和速度大小误差。
- IMM模型概率: 展示两类运动模型的概率变化,观察多模型机制对目标运动状态变化的响应。
- 误差统计对比: 输出位置及速度的平均误差、均方根误差(RMSE)、最大误差和标准差等评价指标。
运行结果展示
运动轨迹:

XY轴的位移曲线:

速度误差与位移误差曲线:


各模型概率曲线:

命令行输出:

MATLAB源代码
部分代码展示:
matlab
% 二维移动目标IMM-CKF定位,AOA定位为主、测距辅助
% 作者:matlabfilter
% 2026-10-23/2D:由三维IMM-CKF版本重新整理为二维平面版本
clear; clc; close all;
rng(0);
num_station = 4;
filter_type = 'CKF';
scene_name = '4基站移动目标IMM-CKF二维IMM-CKF';
%% 参数设置
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:二维轨迹
%% 命令行输出