
原创程序,请勿翻卖
文章目录
程序简介
基于MATLAB编写的四基站二维移动目标定位仿真,采用交互式多模型(Interacting Multiple Model,IMM)与无迹卡尔曼滤波(Unscented Kalman Filter,UKF)相结合的方法,融合AOA到达角与距离测量信息,实现机动目标的位置和速度联合估计。
在移动目标跟踪过程中,目标可能经历匀速、加速和减速等不同运动状态。单纯利用当前时刻的角度与距离测量进行位置解算,难以利用相邻时刻之间的运动连续性。为此,程序引入IMM模型交互机制,并利用UKF处理非线性角度与距离量测,提高算法对不同运动状态的适应能力。
程序设置角度与距离直接解算、IMM-UKF动态滤波两种方案,对比其位置和速度估计表现。
算法与仿真设置
程序采用六维运动状态模型,包括目标二维位置、速度和加速度。利用四个固定基站提供的AOA方位角及距离量测,通过IMM-UKF进行递推状态估计。
算法主要包括以下环节:
- AOA与距离联合观测: 根据四个基站的方位角和距离构建非线性观测方程,每个时刻获得8维观测信息。
- IMM模型交互: 设置两组不同过程噪声水平的运动模型,通过模型转移概率完成状态与协方差混合。
- UKF非线性量测更新: 利用Sigma点无迹变换传播状态分布,计算预测量测、协方差及卡尔曼增益,无需显式计算量测雅可比矩阵。
- 模型概率更新: 根据量测新息及高斯似然更新各模型概率,并融合不同模型的位置和速度估计。
仿真设置4个固定基站、180个采样点,采样周期为0.2秒,AOA角度噪声标准差为0.01 rad,距离噪声标准差为0.1 m。
目标采用分段加速度轨迹,包含匀速及多个机动阶段,用于验证不同运动状态下的位置和速度估计表现。
结果分析
程序主要输出以下结果:
- 二维轨迹定位: 展示基站分布、目标真实轨迹、角度与距离直接解算轨迹及IMM-UKF滤波轨迹。
- 两轴坐标跟踪: 分别比较目标x、y坐标的真实值与估计值。
- 位置误差分析: 绘制直接解算与IMM-UKF的二维位置距离误差曲线。
- 速度估计误差: 对比直接解算差分速度与IMM-UKF估计速度,包括速度向量误差和速度大小误差。
- IMM模型概率: 绘制两组运动模型的概率变化曲线,分析模型交互机制对不同运动阶段的响应。
- 误差统计: 输出位置与速度的平均误差、均方根误差(RMSE)、最大误差及标准差。
运行结果
轨迹图:

误差曲线:


命令行输出:

MATLAB源代码
部分代码如下:
matlab
% 二维移动目标IMM-UKF定位,AOA定位为主、测距辅助
% 作者 V:matlabfilter(可获取完整例程、个性化定制和讲解)
% 2026-10-27/2D:由三维IMM-UKF版本重新整理为二维平面版本
clear; clc; close all;
rng(0);
num_station = 4;
filter_type = 'UKF';
scene_name = '4基站移动目标IMM-UKF二维IMM-UKF';
%% 参数设置
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非线性滤波
%% 误差统计
%% 图
完整代码:
如需帮助,或有导航、定位滤波相关的代码定制需求,可从个人主页左侧联系我