【MATLAB例程】二维IMM-UKF定位,基于四个基站的AOA与测距辅助

原创程序,请勿翻卖

文章目录

程序简介

基于MATLAB编写的四基站二维移动目标定位仿真,采用交互式多模型(Interacting Multiple Model,IMM)与无迹卡尔曼滤波(Unscented Kalman Filter,UKF)相结合的方法,融合AOA到达角与距离测量信息,实现机动目标的位置和速度联合估计。

在移动目标跟踪过程中,目标可能经历匀速、加速和减速等不同运动状态。单纯利用当前时刻的角度与距离测量进行位置解算,难以利用相邻时刻之间的运动连续性。为此,程序引入IMM模型交互机制,并利用UKF处理非线性角度与距离量测,提高算法对不同运动状态的适应能力。

程序设置角度与距离直接解算、IMM-UKF动态滤波两种方案,对比其位置和速度估计表现。

算法与仿真设置

程序采用六维运动状态模型,包括目标二维位置、速度和加速度。利用四个固定基站提供的AOA方位角及距离量测,通过IMM-UKF进行递推状态估计。

算法主要包括以下环节:

  1. AOA与距离联合观测: 根据四个基站的方位角和距离构建非线性观测方程,每个时刻获得8维观测信息。
  2. IMM模型交互: 设置两组不同过程噪声水平的运动模型,通过模型转移概率完成状态与协方差混合。
  3. UKF非线性量测更新: 利用Sigma点无迹变换传播状态分布,计算预测量测、协方差及卡尔曼增益,无需显式计算量测雅可比矩阵。
  4. 模型概率更新: 根据量测新息及高斯似然更新各模型概率,并融合不同模型的位置和速度估计。

仿真设置4个固定基站、180个采样点,采样周期为0.2秒,AOA角度噪声标准差为0.01 rad,距离噪声标准差为0.1 m。

目标采用分段加速度轨迹,包含匀速及多个机动阶段,用于验证不同运动状态下的位置和速度估计表现。

结果分析

程序主要输出以下结果:

  1. 二维轨迹定位: 展示基站分布、目标真实轨迹、角度与距离直接解算轨迹及IMM-UKF滤波轨迹。
  2. 两轴坐标跟踪: 分别比较目标x、y坐标的真实值与估计值。
  3. 位置误差分析: 绘制直接解算与IMM-UKF的二维位置距离误差曲线。
  4. 速度估计误差: 对比直接解算差分速度与IMM-UKF估计速度,包括速度向量误差和速度大小误差。
  5. IMM模型概率: 绘制两组运动模型的概率变化曲线,分析模型交互机制对不同运动阶段的响应。
  6. 误差统计: 输出位置与速度的平均误差、均方根误差(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非线性滤波
%% 误差统计

%% 图

完整代码:

如需帮助,或有导航、定位滤波相关的代码定制需求,可从个人主页左侧联系我

相关推荐
feixing_fx37 分钟前
变量与声明:let、const、var 的选择边界
开发语言·前端·javascript·前端框架
个 人 练 习 生1 小时前
C++:从零基础到深入了解vector的使用
开发语言·c++·经验分享·学习·程序人生·stl
H.莓飛2 小时前
【C++】【STL】stack_OJ题
服务器·c语言·开发语言·数据结构·c++·算法·visual studio
hudd6682 小时前
JavaEE 初阶 - 文件操作和IO
java·开发语言·java-ee
starzy19903 小时前
深入理解Scala隐式参数:上下文流转
开发语言·python·scala
feixing_fx4 小时前
JavaScript 是什么:从浏览器脚本到通用编程语言
开发语言·前端·javascript·前端框架·ecmascript
Evand J5 小时前
【MATLAB例程】基于IMM-EKF与AOA/测距融合的二维移动目标跟踪定位
matlab·代码·定位·1024程序员节·toa·aoa
惜.己6 小时前
Java结合idea简单使用Playwright
java·开发语言·自动化
利威尔xu6 小时前
PostgreSQL 索引失效 4 类:函数运算让查询全表扫描拖垮性能
开发语言·数据库·postgresql·性能优化·索引优化