本文所述的MATLAB代码实现了一个基于两步加权最小二乘法的二维目标定位算法，利用多个锚点（基站）和时间差到达（TDOA）数据来估计未知目标的位置。

运行结果

• 3个锚点时的情况：
  
• 10个锚点时的情况：
  

• 可一键更改锚点数量：
  

• 命令行会输出真实值和估计值的坐标信息：
  

代码

% 两步加权最小二乘，定位二维目标、N个锚点
% 2024-11-07/Ver1
%% 初始化
clc;clear;close all;
rng(1);
% 定义参数和待测点位置
num_stations = 10; % 基站数量（锚点数量）
std_var1 = 1e-9; %TDOA误差
% 固定基站位置
stations_position = 100*randn(num_stations,2);
c = 3e8;
% 生成轨迹数组
position = 50*ones(1,2)+50*randn(1,2);
% TDOA 建模

% 完整代码见：https://gf.bilibili.com/item/detail/1106616012

代码功能概述

该代码模拟了一个二维空间中的目标定位问题，使用随机生成的锚点位置和目标轨迹，并通过最小二乘法进行位置估计。目标的真实位置与估计位置通过图形可视化展示，便于对算法效果的直观理解。

代码结构和详细说明

初始化部分

clc; clear; close all;
rng(1);

• clc：清除命令窗口的内容。
• clear：清除工作空间中的所有变量。
• close all：关闭所有打开的图形窗口。
• rng(1)：设置随机数生成器的种子，以确保结果的可重复性。

参数和待测点位置定义

num_stations = 10; % 基站数量（锚点数量）
std_var1 = 1e-9; % TDOA误差
stations_position = 100 * randn(num_stations, 2); % 随机生成基站位置
c = 3e8; % 声速

• num_stations：定义基站的数量，这里设为10个。
• std_var1：设定TDOA测量的标准差，用于模拟测量误差。
• stations_position：随机生成基站在二维空间中的位置，坐标值为正态分布。
• c：定义声速（单位：米/秒）。

生成目标轨迹

position = 50 * ones(1, 2) + 50 * randn(1, 2);

• 生成一个目标位置，初始位置为(50, 50)附近的一个随机点。

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)); % 计算TDOA并添加噪声
r = delta_t * c; % 计算实际距离

• delta：计算未知目标与各基站之间的相对位置（矢量）。
• r_ideal：根据相对位置计算目标到各个基站的理想距离。
• delta_t：根据理想距离计算到达时间，并添加高斯噪声。
• r：将到达时间转换为实际距离。

距离差计算

Ri = r(2:end, :);
R1 = ones(num_stations - 1, 1) * r(1, :);
R = Ri - R1; % 计算距离差

• 计算从第一个基站到其他基站的距离差，这将用于后续的最小二乘法计算。

迭代初值估算

position_est = mean(stations_position); % 基站位置均值作为初始估计

• 使用所有基站位置的均值作为初始位置估计。

最小二乘迭代

for i1 = 1:100 % 可设置迭代次数
    ...
end

• 迭代100次以优化位置估计，每次迭代计算新的位置估计。

粗迭代部分

h = (R(1))^2 + norm(stations_position(1, :))^2 - norm(stations_position(2, :))^2 + 2 * (R(1)) * norm(position_est - stations_position(1, :));
Ga = 2 * (stations_position(1, :) - stations_position(2, :));
B = 2 * diag([norm(position_est - stations_position(2, :));]);

• 计算初步的残差和雅可比矩阵的构造。

细节迭代部分

for i2 = 3:num_stations
    ...
end

• 在每次迭代中，更新残差h和雅可比矩阵A，并计算最小二乘解。

最小二乘解

Q = std_var1^2 * (0.5 * eye(num_stations - 1) + 0.5 * ones(num_stations - 1));
W = (B * Q * B')^(-1); % 计算加权矩阵
u_star = (Ga' * W * Ga)^(-1) * Ga' * W * h; % 计算最小二乘解

• 构造加权矩阵W，计算最小二乘解u_star，用于更新位置估计。

结果绘图

figure;
hold on
plot(stations_position(:, 1), stations_position(:, 2), 'r*'); % 绘制基站位置
plot(position(1), position(2), 'ok'); % 绘制真实目标位置
plot(position_est(1), position_est(2), '.b'); % 绘制估计目标位置
xlabel('X轴'); ylabel('Y轴');
legend('锚点', '真实值', '估计值');

• 可视化锚点、真实位置和估计位置，便于观察定位效果。

10. 输出结果

fprintf('真实值坐标：%s\n', num2str(position));
fprintf('估计值坐标：%s\n', num2str(position_est));

• 打印真实目标位置和估计位置的坐标。

总结

该$MATLAB$代码使用两步加权最小二乘法通过$TDOA$技术实现了二维目标定位。它通过随机生成的锚点和目标位置进行模拟，展示了如何通过迭代优化算法来提高位置估计的准确性。代码结构清晰，适合用于理解和实现基于$TDOA$的定位算法。

如有需要，可私信或通过下方的卡片联系我