【MATLAB例程】AOA（到达角度）与TOA（到达时间）混合定位方法，在三维空间下的运动轨迹定位和滤波，滤波使用CKF，附代码下载链接

本文所述程序实现了一种结合到达角（AOA）和到达时间（TOA）技术的混合定位方法。它可以自适应基站数量，并在三维空间中进行目标的动态轨迹定位，使用CKF（Cubature Kalman Filter）算法对定位结果进行滤波优化。

程序介绍

以下是对该MATLAB代码的功能介绍和技术解析：

一、代码功能概述

本代码实现了一个基于**到达角（AOA）和到达时间（TOA）混合定位的三维运动目标跟踪系统，结合容积卡尔曼滤波（CKF）**进行轨迹优化，适用于复杂室内或城市环境中的高精度定位需求。主要功能包括：

1. 动态场景建模：生成三维匀速运动轨迹（X/Y轴线性变化，Z轴固定），模拟真实目标运动特性
2. 混合信号模拟：
   • AOA测量：计算方位角（azimuth）和俯仰角（elevation），叠加高斯噪声模拟实际环境干扰
   • TOA测量：通过光速转换距离，并添加时间噪声以反映信号传播误差
3. 自适应基站网络：支持随机生成非均匀分布的基站（锚点），数量可配置（num_station参数）
4. 定位解算与滤波：
   • 基于最小二乘法构建超定方程组，利用单位向量矩阵和伪逆（pinv）求解目标坐标。
   • 采用**容积卡尔曼滤波（CKF）**对定位结果进行优化，抑制噪声并提升轨迹平滑性

二、核心算法与技术亮点

1. 混合定位解算

• AOA-TOA联合模型：
  • 方位角通过atan2计算基站与目标点的平面方向，俯仰角通过三维几何关系确定高度方向。
  • TOA通过信号传播时间转换为距离，结合AOA构建超定方程组，解决传统单一测量方法的维度限制问题
• 最小二乘优化：通过线性方程组的伪逆求解，适应基站数量变化（10个基站示例），解决三维定位中的多解问题。

2. 容积卡尔曼滤波（CKF）

• 非线性处理优势：相较于EKF的一阶泰勒展开近似，CKF通过球面径向容积准则精确计算高斯积分，实现三阶精度逼近，避免线性化误差
• 状态模型：
  • 状态向量为三维位置（[x, y, z]），假设匀速运动模型，速度通过固定增量（[0.2, -0.2, 0]）建模
  • 过程噪声协方差（Q）和观测噪声协方差（R）可调，适应不同噪声环境
• 滤波流程：
  • 预测阶段：生成容积点并通过状态方程传播，计算预测均值和协方差
  • 更新阶段：融合观测值（混合定位结果），利用卡尔曼增益修正状态估计，抑制累积误差

三、实验结果与性能

1. 可视化输出

• 三维轨迹图：展示基站位置（红点）、真实轨迹（蓝线）、观测值（黄点）、CKF滤波结果（蓝点）及未滤波惯导模拟轨迹，直观对比定位效果
• 误差分析图：
  • 三轴误差：X/Y轴滤波后RMSE≤0.15米，Z轴因AOA垂直信息不足误差略高（约0.3米）
  • RMSE对比：CKF曲线显著低于未滤波结果，终点距离误差从0.8米降至0.2米以内

2. 关键参数配置

参数	描述	典型值/范围
AOA_noise	AOA角度噪声标准差	0.1 rad
TOA_noise	TOA时间噪声标准差	1e-9 s
Q	过程噪声协方差矩阵	diag([1,1,1])*0.01
R	观测噪声协方差矩阵	diag([1,1,1])*0.1

四、应用场景与扩展性

• 适用场景：城市峡谷、室内导航、无人机轨迹跟踪等非视距（NLOS）环境
• 扩展方向：
  • 多传感器融合：可集成惯性导航（IMU）或UWB数据，进一步提升鲁棒性
  • 自适应噪声调整：根据环境动态优化Q/R参数，适应时变信道条件
  • 算法改进：替换为平方根CKF（SRCKF）或结合深度学习进行NLOS误差抑制

五、代码结构说明

1. 初始化模块：设置随机种子、生成目标运动轨迹（positions）和随机基站位置（stations_position）
2. 定位解算循环：
   • 计算真实距离与含噪声的AOA/TOA测量值。
   • 构建H矩阵和Y向量，通过最小二乘求解目标位置
3. CKF滤波模块：
   • 容积点采样与状态预测。
   • 观测更新与协方差修正，输出优化后的轨迹
4. 可视化与误差统计：绘制三维轨迹、各轴误差曲线及RMSE对比图

运行结果

定位示意图：

误差曲线：


命令行输出：

MATLAB代码

完整的代码如下：

% AOA与TOA混合定位例程，自适应基站数量，三维，轨迹滤波使用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,:);
    %% 定位
    % 计算目标到各基站的距离
    num_station = size(stations_position, 1);
    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角度和TOA测量的时间上加一些噪声
    AOA_noise = 1e-1; % AOA 角度噪声
    TOA_noise = 1e-9; %TOA时间噪声
    azimuth_angles = azimuth_angles + AOA_noise * randn(num_station, 1);
    elevation_angles = elevation_angles + AOA_noise * randn(num_station, 1);
    t_toa = true_distances/3e8  + TOA_noise * randn(num_station, 1);
    distances = t_toa*3e8;
    % 使用最小二乘法进行定位估计
    % 定义目标位置的初始猜测
    A = zeros(num_station , 3); % 2 行 per anchor, 4 列 (x, y, z, 1)
    b = zeros(num_station , 1);
    
    % 构建方程组

完整代码下载链接：https://download.csdn.net/download/callmeup/90546807

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