【matlab例程】TDOA定位与粒子滤波（PF）的轨迹滤波代码，三维空间，TDOA的锚点数量可自适应，使用chan-Taylor算法，模拟TDOA/IMU融合，附下载链接

该代码完整实现了从TDOA定位到轨迹滤波的全流程，通过加权最小二乘解决非线性问题，再通过粒子滤波抑制噪声。代码结构清晰，适合作为定位算法的入门学习案例。实际应用中需根据场景调整参数和模型

程序讲解

代码概述

目标：本程序旨在利用到达时间差（TDOA）和两步加权最小二乘法进行三维定位，并结合粒子滤波（PF）对运动轨迹进行优化和滤波。

主要步骤：

1. 初始化参数：设置基站数量、噪声参数，并生成基站位置和目标运动轨迹。
2. TDOA建模与定位：通过加权最小二乘法迭代求解目标位置。
3. 粒子滤波（PF）：对定位结果进行滤波，以减少噪声影响。
4. 结果可视化：绘制目标轨迹、误差曲线以及RMSE对比图。

初始化与数据生成

参数定义

num_stations = 20;        % 基站数量
std_var1 = 1e-10;        % TDOA测量噪声标准差
c = 3e8;                 % 光速
stations_position = 3*randn(num_stations,3); % 随机生成基站位置

• 基站位置：通过randn生成符合正态分布的随机坐标，以模拟真实环境中的基站分布。
• 运动轨迹：使用positions生成目标的三维运动路径，设定其在X轴正向、Y轴负向匀速运动，并保持Z轴不变。

TDOA建模

delta = ones(num_stations,1)*position - stations_position;
r_ideal = sqrt(sum(delta.^2,2));       % 理想距离
r = (r_ideal/c + noise) * c;          % 添加噪声后的距离
R = r(2:end) - r(1);                  % TDOA转换为距离差

• 距离计算：计算目标到每个基站的距离，并添加高斯噪声以模拟测量误差。
• 距离差构造：以第一个基站为参考，计算其他基站与该参考基站之间的距离差。

两步加权最小二乘定位

粗迭代求解初值

position_est(i3,:) = mean(stations_position); % 初始值设为基站位置均值
for i1 = 1:5 % 迭代次数
    % 构造矩阵Ga和h
    Ga = 2 * (基站1位置 - 其他基站位置);
    h = 包含R^2和基站位置范数的线性项;
    % 加权最小二乘解
    W = (B*Q*B')^(-1); % 权重矩阵
    u_star = (Ga' * W * Ga) \ (Ga' * W * h);

• 目标：通过线性化模型获得位置的粗略估计。
• 权重矩阵W：根据测量噪声协方差矩阵Q构造，反映不同测量的可信度。

细迭代优化

% 构造矩阵A和h2
A = 包含非线性项的雅可比矩阵;
h2 = 更新后的观测模型;
delta_u_star = (A' * W * A) \ (A' * W * h2);
u_hat = u_star - delta_u_star; % 最终估计位置

• 非线性修正：通过二次迭代进一步细化估计，修正粗估计中的近似误差。

粒子滤波（PF）

初始化粒子群

N = 100;                 % 粒子数量
P = X(:,1) + randn(3,N); % 初始粒子位置
w = 计算初始权重;         % 基于与观测值的距离

• 粒子分布：在初始位置附近随机生成N个粒子，模拟状态的不确定性。

预测与更新

% 预测步骤（状态方程）
P(:,i) = 前一步位置 + 运动模型 + 过程噪声;
% 观测更新
dist = norm(粒子位置 - 观测位置);
w = 根据高斯分布更新权重;

• 状态方程：假设目标以匀速运动，叠加过程噪声（Q为协方差）。
• 权重计算：粒子权重反映其与当前观测的匹配程度。

重采样

% 系统重采样（避免粒子退化）
w_noml = 归一化权重;
index = 按权重概率选择粒子;
Pnext = P(:,index); % 新粒子集合

• 重采样目的：保留高权重的粒子，避免粒子贫化现象。

结果可视化

三维轨迹图

plot3(基站位置, 'r*', 'DisplayName', '锚点');
plot3(真实轨迹, '-b', 'DisplayName', '真实值');
plot3(滤波前观测, '.', 'DisplayName', '观测值');
plot3(PF结果, '.', 'DisplayName', 'PF估计值');

• 效果：对比真实轨迹、观测值与滤波结果，直观展示粒子滤波的平滑效果。

误差分析

% 计算各轴绝对误差
subplot(3,1,1); plot(X轴误差);
subplot(3,1,2); plot(Y轴误差);
subplot(3,1,3); plot(Z轴误差);
% RMSE对比
plot(RMSE_ins, RMSE_out, RMSE_PF);

• 分析：粒子滤波显著降低了位置估计的误差，尤其在噪声较大的情况下效果明显。

关键改进点与注意事项

1. TDOA模型局限性：

   • 假设测量误差为高斯分布，但实际情况可能存在非高斯噪声。
   • 迭代次数（当前设置为5次）会影响精度，需根据实际情况调整。

2. 粒子滤波优化：

   • 粒子数量N越多，定位精度通常越高，但计算量也随之增加。
   • 过程噪声Q和观测噪声R应根据具体系统进行调整。

3. 运动模型假设：

   • 当前代码假设目标匀速运动（简化状态方程），对于复杂运动情况应考虑使用更复杂的模型（如卡尔曼滤波）。

运行结果

定位示意图：

误差曲线图：

命令行输出结果：

MATLAB代码

% TDOA对轨迹的定位，使用两步加权最小二乘方法，定位三维目标、N个锚点，PF对轨迹进行滤波
% 2025-03-19/Ver1
%% 初始化
clc;clear;close all;
rng(0);
% 定义参数和待测点位置
num_stations = 20; % 基站数量（锚点数量）
std_var1 = 1e-10; %TDOA误差
% 固定基站位置
stations_position = 3*randn(num_stations,3);
c = 3e8;
% 生成轨迹数组
point1 = 2*ones(1,3);
positions = point1 + repmat(point1,21,1)+[0:0.2:4;0:-0.2:-4;zeros(1,21)]'; %生成目标的运动

for i3 = 1:size(positions,1)
    position = positions(i3,:);
    % TDOA 建模
    delta = ones(num_stations,1)*position - stations_position; %未知点与各基站之间的相对位置（矢量）
    r_ideal = (sum(delta.^2,2)).^(1/2); %计算移动台到各个基站的实际距离（标量）
    delta_t = r_ideal/c+std_var1*randn(size(r_ideal));
    r = delta_t*c;
    Ri = r(2:end,:);
    R1 = ones(num_stations-1,1)*r(1,:);
    R = Ri-R1; %表示从[2,i]开始MS与基站i和基站1的距离差
    %% 迭代初值估算
    % 基站均值作为初值
    position_est(i3,:) = mean(stations_position);
    %% 最小二乘迭代

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

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