【MATLAB例程|三维AOA与测距跟踪IMM】n基站移动目标IMM-EKF/UKF/CKF可选择版本，AOA定位为主、测距辅助的非线性滤波，附下载链接

三维移动目标的多基站AOA/距离辅助IMM非线性滤波定位仿真程序。程序以AOA角度观测为主要定位信息，并引入距离量测作为精度增强辅助，在随机布设的n个三维基站条件下，对机动目标的空间位置、速度和运动模型概率进行联合估计。代码内置分段机动轨迹，包括匀速运动段和常加速度机动段，可较好地模拟移动目标在不同运动状态之间切换的跟踪场景。
原创代码，请勿翻卖

程序详解

测距信息作为辅助约束引入，带距离辅助的AOA定位是在原纯AOA定位基础上新提出的精度增强方法，通常可获得更高定位精度

运动轨迹与模型设置

为了让 IMM 模型概率具有明显切换效果，当前轨迹采用“匀速段 + 分段常加速度机动段”：0-6 s、12-18 s、25-31 s 为相对匀速段，6-12 s、18-25 s、31 s 之后为加速/转向机动段。

CV 模型的过程噪声已收紧，CA 模型保留加速度状态并增强对持续加速度的适配能力。

使用方式

在 main_AOA_dist_IMM_EKFUKFCKF_Ver1.m 开头修改：

num_station = 8;
filter_type = 'UKF';    % 可选：'EKF'、'UKF'、'CKF'

输出结果

程序会输出轨迹、位置误差、速度误差、IMM 模型概率和误差统计图。IMM 概率图中用浅色背景标出机动时间段，便于观察 CV/CA 概率随运动状态变化的切换效果。

以UKF为例：
定位结果展示：

误差曲线：

命令行窗口的输出：

录屏演示：

MATLAB源代码

部分代码如下：

% n基站移动目标IMM-EKF/UKF/CKF可选择版本，AOA定位为主、测距辅助的非线性滤波
% 作者微信：matlabfilter（可联系我获取完整例程、个性化定制和讲解）
% 2026-06-08/Ver1：通过filter_type选择EKF、UKF或CKF

clear; clc; close all;

rng(0);

num_station = 8;        % 可自行修改基站数量
filter_type = 'UKF';    % 可选：'EKF'、'UKF'、'CKF'
title_prefix = sprintf('%d个基站移动目标IMM-%s', num_station, filter_type);

%% 参数设置
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的整数，三维定位建议设置为4或更多。');
end

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

%% 构造真实移动目标轨迹
% 构造分段机动移动目标轨迹：匀速段 + 常加速度机动段
% 这样CV模型会在匀速段占优，CA模型会在加速/转向段概率上升。
maneuver_acc = zeros(num_step, 3);
for k = 1:num_step
    t = t_axis(k);
    if t < 6
        maneuver_acc(k, :) = [0.00, 0.00, 0.00];
    elseif t < 12
        maneuver_acc(k, :) = [0.85, -0.55, 0.18];
    elseif t < 18
        maneuver_acc(k, :) = [0.00, 0.00, 0.00];
    elseif t < 25
        maneuver_acc(k, :) = [-0.95, 0.75, -0.16];
    elseif t < 31
        maneuver_acc(k, :) = [0.00, 0.00, 0.00];
    else
        maneuver_acc(k, :) = [0.55, 0.42, 0.10];
    end
end

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

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);

完整代码也可通过下方链接获取：
https://download.youkuaiyun.com/download/callmeup/92972881

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