无源定位入门（一）TDOA（4）TDOA定位模块代码

最新推荐文章于 2024-10-12 12:58:15 发布

syy_1797

最新推荐文章于 2024-10-12 12:58:15 发布

阅读量9.5k

点赞数 10

本文链接：https://blog.csdn.net/syy_1797/article/details/88727206

版权

本文介绍了无源定位中的TDOA（时间差分）定位原理，并展示了基于Chan算法的TDOA定位模块代码。该代码实现了二维目标定位，包括远场模型和近场模型的两次加权最小二乘法（WLS）计算，以求得四个定位结果。

摘要由CSDN通过智能技术生成

TDOA定位模块代码

function [POS1,POS2,POS3,POS4] = TDOA_Location(r,bs,Q,c)
%% TDOA定位定位Chan算法
%*********************************************************
% CHAN算法,假设移动台与各基站位置较近,需进行三四WLS计算

%仅针对二维目标定位场景
% 输入参数:
% r(N-1×1): TDOA距离差测量值，N为基站数
% Pbs(N×2): 基站的坐标,第一列为X,第二列为Y;参考基站坐标位于第一行
% 输出参数:
% POS1(2X1):定位结果1
% POS2(2X1):定位结果2
% POS3(2X1):定位结果3
% POS4(2X1):定位结果4
%c=3e8; %光速
N = size(bs,1);
K = bs(:,1).^2 + bs(:,2).^2; %K1和Ki,i=2,...,M
ha = 0.5*(r.^2-K(2:N)+K(1)); %h矩阵
Ga = -[bs(2:N,1)-bs(1,1) bs(2:N,2)-bs(1,2) r]; %Ga矩阵

%% 第一次WLS结果（远场模型算法，粗糙模型）
Za1 = inv(Ga.'*inv(Q)*Ga)*Ga.'*inv(Q)*ha;
WLS1_far=Za1(1:2); %第一步WLS远场模型结果

%% 第一次WL结果（近场模型算法，精确模型）
W1=Za1(1)*ones(N