【MATLAB例程】三维环境下,动态轨迹的AOA定位与UKF滤波,模拟IMU/AOA的数据融合(AOA的测角基站数量可自适应,目标运动轨迹可自行修改)

本文所述的MATLAB 代码实现了基于到达角(AOA)定位的三维定位算法,使用自适应基站数量进行动态轨迹优化。通过无迹卡尔曼滤波(UKF)技术,代码能够提高定位精度,处理动态目标的轨迹

文章目录

程序讲解

功能模块

  1. 初始化

    • 清空工作环境,设置随机种子。
    • 生成目标点坐标和运动轨迹,定义固定基站的位置。
  2. AOA 定位

    • 计算目标到各基站的距离。
    • 模拟接收到的 AOAs(包括噪声)。
    • 使用最小二乘法进行三维定位估计。
  3. UKF 部分

    • 初始化滤波模型,包括过程噪声和观测噪声协方差矩阵。
    • 进行状态预测和更新,通过迭代优化目标位置的估计。
  4. 绘图

    • 绘制目标的真实轨迹、预测轨迹和观测值的三维图。
    • 生成每个轴的误差图,展示观测值与真实值的差异。
  5. 位置与误差输出

    • 打印终点的真实坐标、估计坐标及其误差。
    • 计算并输出每时刻的均方根误差(RMSE),并绘制 RMSE 比较图。

关键技术

  • AOA 定位:通过测量目标相对于多个基站的角度来进行位置估计。
  • 最小二乘法:用于求解定位估计的线性方程。
  • 无迹卡尔曼滤波(UKF):一种非线性滤波技术,用于提高动态系统状态估计的精度。

结果示例

  1. 轨迹对比图
    • UKF估计值(蓝色点)紧密贴合真实轨迹(蓝色线),显著优于未滤波的观测值(散点)。
    • 锚点(红色星号)随机分布,验证算法的自适应能力。
  2. 误差分析
    • X/Y轴误差受运动模型影响较大,UKF有效抑制累计误差;Z轴误差因静态假设接近零。
    • RMSE曲线显示UKF全程误差波动最小,稳定性优于纯观测

具体如下:

  • 定位示意图:

  • RMSE对比曲线图:

  • 三轴误差图:

  • 命令行窗口截图:

MATLAB源代码

源代码的结构如下:

部分代码如下:

matlab 复制代码
% AOA定位,三维、N个锚点(自适应基站数量),动态轨迹UKF优化
% 作者:matlabfilter
% 2025-03-17/Ver1

%% 初始化
clc;clear;close all;
rng(0);
% 生成目标点坐标
position = [-1,-1,1];
% 生成目标的运动
positions = repmat(position,21,1)+[0:0.2:4;0:-0.2:-4;zeros(1,21)]';

% 固定基站位置
num_station =10; %基站数量
stations_position=2*randn(num_station,3); %定义基站的坐标,这里是随机坐标

for i1 = 1:size(positions,1)
    position = positions(i1,:);
%% AOA定位
% 计算目标到各基站的距离
true_distances = vecnorm(stations_position - position, 2, 2);

% 模拟接收到的AOA(此时是角度理想值,后面要加噪声)
azimuth_angles = atan2(position(2) - stations_position(:, 2), position(1) - stations_position(:, 1));
elevation_angles = atan2(position(3) - stations_position(:, 3), ...
    sqrt((position(1) - stations_position(:, 1)).^2 + (position(2) - stations_position(:, 2)).^2));

% 假设测量的AOA角度和距离上加一些噪声
AOA_noise = 1e-2; % AOA 角度噪声
azimuth_angles = azimuth_angles + AOA_noise * randn(num_station, 1);
elevation_angles = elevation_angles + AOA_noise * randn(num_station, 1);
% 使用最小二乘法进行定位估计 三维直接求解(无需距离)
H = zeros(2*20,3);
Y = zeros(2*20,1);
for i2 = 1:num_station
    H(2*i2-1:2*i2,:) = [
        sin(azimuth_angles(i2)),-cos(azimuth_angles(i2)),0;
        cos(azimuth_angles(i2))*sin(elevation_angles(i2)),sin(azimuth_angles(i2))*sin(

完整代码:https://download.csdn.net/download/callmeup/90538619

如需帮助,或有导航、定位滤波相关的代码定制需求,请点击下方卡片联系作者

相关推荐
小秋学嵌入式-不读研版2 分钟前
C56-字符串拷贝函数strcpy与strnpy
c语言·开发语言·笔记
hui函数12 分钟前
python全栈(基础篇)——day04:后端内容(字符编码+list与tuple+条件判断+实战演示+每日一题)
开发语言·数据结构·python·全栈
Never_Satisfied16 分钟前
在JavaScript / HTML中,转移字符导致js生成的html出错
开发语言·javascript·html
羚羊角uou22 分钟前
【Linux】POSIX信号量、环形队列、基于环形队列实现生产者消费者模型
java·开发语言
数据知道2 小时前
Go语言:用Go操作SQLite详解
开发语言·后端·golang·sqlite·go语言
晨非辰3 小时前
《剑指Offer:单链表操作入门——从“头删”开始破解面试》
c语言·开发语言·数据结构·c++·笔记·算法·面试
sheji34166 小时前
【开题答辩全过程】以 python杭州亚运会数据分析与可视化开题为例,包含答辩的问题和答案
开发语言·python·数据分析
啊我不会诶6 小时前
24ICPC成都站补题
数据结构·算法
2401_841495649 小时前
【计算机视觉】基于数学形态学的保留边缘图像去噪
人工智能·python·算法·计算机视觉·图像去噪·数学形态学·边缘保留
十八岁讨厌编程9 小时前
【算法训练营Day30】动态规划part6
算法·动态规划