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

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

原创代码,请勿翻卖。可联系我获取代码定制、讲解等

文章目录

程序简介

本程序基于MATLAB搭建四基站二维移动目标定位仿真模型,采用交互式多模型(Interacting Multiple Model,IMM)与容积卡尔曼滤波(Cubature Kalman Filter,CKF)相结合的方法,实现基于AOA角度和距离测量的移动目标定位与运动状态估计。

在移动目标定位过程中,目标可能经历匀速、加速和减速等不同运动状态,单一运动模型难以充分适应所有机动变化。为此,程序引入IMM多模型交互机制,结合CKF处理非线性角度与距离量测,实现目标位置及速度的递推估计。

程序设置角度与距离直接解算、IMM-CKF滤波两种方案,分析多模型动态滤波对移动目标定位和速度估计的影响。

算法与仿真设置

程序采用二维平面定位模型,设置4个固定基站,同时获取目标相对于各基站的AOA方位角及距离测量信息。

主要包含两种定位方案:

  1. 角度与距离直接解算: 根据各基站的角度和距离测量分别计算目标位置,再取平均得到目标位置估计,并通过位置差分计算速度。
  2. IMM-CKF动态滤波: 构建包含二维位置、速度和加速度的六维状态模型,通过IMM模型交互、状态预测、CKF非线性量测更新及模型概率融合,实现目标位置与速度联合估计。

IMM设置平稳与机动两类运动模型,主要通过不同的过程噪声协方差体现运动变化程度,并根据量测似然动态调整模型概率。

CKF采用容积点传播方式处理AOA角度及距离构成的非线性量测方程,无需显式计算量测雅可比矩阵。

仿真采用以下参数:

  • 基站数量:4个;
  • 仿真步数:180步;
  • 采样周期:0.2秒;
  • AOA角度噪声标准差:0.01 rad;
  • 距离测量噪声标准差:0.1 m。

目标轨迹包含匀速及多个分段加速度变化过程,用于模拟不同运动状态下的二维机动目标定位场景。

结果分析

程序主要输出以下结果:

  1. 二维轨迹定位: 展示四基站分布、目标真实轨迹、角度与距离直接解算轨迹及IMM-CKF滤波轨迹。
  2. 两轴坐标跟踪: 分别对比目标x、y轴真实位置与两种算法的估计结果。
  3. 位置误差分析: 绘制两种方法的二维位置误差曲线,分析机动过程中的定位精度。
  4. 速度估计误差: 对比直接解算位置差分速度与IMM-CKF估计速度的误差,包括速度向量误差和速度大小误差。
  5. IMM模型概率: 展示两类运动模型的概率变化,观察多模型机制对目标运动状态变化的响应。
  6. 误差统计对比: 输出位置及速度的平均误差、均方根误差(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:二维轨迹


%% 命令行输出
相关推荐
JianZhen✓1 小时前
IM 聊天列表为什么总“消息乱跳“:有序插入、时间轴派生、本地截断、已读回执
开发语言·前端·php
FungLeo1 小时前
FengCMS · PHP+MySQL 升级实录· 代码审计怎么闭环:REVIEW到 CLOSURE 四件套,12 个域90 篇修复文档跟到底
开发语言·mysql·php·代码审计·fengcms
七夜zippoe2 小时前
多轮对话管理:Session/记忆在 Java 后端怎么存(存储设计实战)
java·开发语言·ai·session·记忆
屿华AI2 小时前
AUTOSAR 实战-诊断工具 | 基于 Python 与 USB-CANFD 的 UDS 诊断测试工具设计与实现
开发语言·python·测试工具
SomeOtherTime2 小时前
番外篇(第 12 讲):走向工业界深水区——从物理定律看 Rust 在真实工程中的“降维打击”与“阿喀琉斯之踵”
开发语言·rust
黑客笔记3 小时前
Python 进程池和线程池:用等待任务理解并发
开发语言·python
pps-key15 小时前
网络攻防入门:从“黑客梦”到合法实操(生存篇)
开发语言·web安全·php
汤姆宝库16 小时前
BPE分词器的完整实现:字节对合并与词表构建
开发语言·人工智能·c#
驭渊的小故事18 小时前
订单服务如何调用商品服务?一次 RestTemplate 跨服务调用入门
java·开发语言·php