到达角（AOA）的三维定位算法，并结合容积卡尔曼滤波（CKF）对动态目标的运动轨迹进行优化，附代码下载链接-优快云博客

本文链接：https://blog.youkuaiyun.com/callmeup/article/details/146709857

在这里插入图片描述

本代码实现了一种基于到达角（AOA）的三维定位算法，并结合容积卡尔曼滤波（CKF） 对动态目标的运动轨迹进行优化。代码通过模拟基站信号的角度测量及噪声干扰，展示了从原始观测值到滤波后轨迹的完整流程，并对比了滤波前后的定位误差性能。

文章目录

程序讲解
运行结果
MATLAB源代码

程序讲解

概述

本程序实现了一种基于到达角（AOA）技术的三维定位算法，支持自适应数量的锚点（基站），并使用CKF（CKF）对动态轨迹进行优化。该方法旨在提高定位精度并减少噪声影响。

主要功能

初始化参数：设置基站数量、噪声参数并生成基站位置和目标运动轨迹。
AOA定位：计算目标到各锚点的距离，并使用最小二乘法进行定位估计。
CKF滤波：利用CKF对定位结果进行优化，减少测量噪声的影响。
结果可视化：绘制目标的运动轨迹、估计值、误差曲线和RMSE对比图。

代码结构

1. 初始化与数据生成

随机生成目标位置：通过设定初始坐标并生成运动轨迹，模拟目标在三维空间中的移动。
随机生成基站位置：使用正态分布生成多个基站的随机坐标。

2. AOA定位

计算距离：使用向量范数计算目标与各基站之间的真实距离。
模拟AOA角度：通过三角函数计算目标相对于各基站的方位角和俯仰角，并添加高斯噪声。
最小二乘法求解：构造线性方程并通过伪逆法求解目标位置的估计值。

3. CKF滤波部分

初始化滤波模型：设置过程噪声和观测噪声的协方差矩阵，初始化状态估计。
状态预测与更新：使用CKF算法对状态进行预测和校正，处理非线性测量。
迭代计算：通过采样生成容积点，并更新状态均值和协方差矩阵。

4. 结果可视化

绘制三维轨迹图：展示锚点、真实轨迹、观测值和CKF估计值。
误差分析：计算并绘制各轴上的位移误差和RMSE对比，直观展示滤波效果。

结论

该程序有效结合了AOA技术和CKF滤波，能够在动态环境中进行高精度的三维定位。通过模拟和可视化，展示了不同算法在定位精度上的差异，为实际应用提供了有力的支持。

运行结果

定位示意图：
在这里插入图片描述
误差曲线：

命令行的输出结果：

MATLAB源代码

程序结构：

在这里插入图片描述

部分代码：

% AOA定位，三维、N个锚点（自适应基站数量），动态轨迹CKF优化
% 作者：matlabfilter
% 2025-03-29/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);