TDOA定位的Chan算法MATLAB源代码

Posted 2021-09-03 fpga&matlab

TDOA定位的Chan算法MATLAB源代码 。

function [POS_ref,POS1,POS2,POS3,POS4] = TDOA_chan(R,Pbs,Q) 

%*********************************************************

%  CHAN算法,假设移动台与各基站位置较近,需进行三四WLS计算

%  输入参数:

%            R(N-1×1): TDOA测量值

%            Pbs(N×2): 基站的坐标,第一列为X,第二列为Y;参考基站坐标位于第一行

%  输出参数: 

%            POS_ref(2X1):第一次WLS的估计结果,作为参考作最终定位结果的判决

%            POS1(2X1)   :定位结果1

%            POS2(2X1)   :定位结果2

%            POS3(2X1)   :定位结果3

%            POS4(2X1)   :定位结果4

%这段程序用于自己产生算法输入参数,用于算法测试

% delta = 10;          %TDOA测量误差的标准差,用于产生Q矩阵

% M=4;                 %参考基站外的基站数量

% Xb = 20;Yb = 100;    %参考的基站坐标

% X = zeros(M,2);%M行2列0

% a = 2*pi/M;

% for i=1:M            %生成其他基站的坐标

%     X(i,1) = 400*cos(a*(i-1));

%     X(i,2) = 400*sin(a*(i-1));

% end

% Xreal = -150;Yreal = 200;%移动台真实坐标

% %产生TDOA测量结果

% Rb = sqrt((Xreal - Xb)^2+(Yreal - Yb)^2);%移动台到基站的真实距离

% N = normrnd(0,delta,1,M);%产生TDOA测量误差 正态分布 均值0 标准差 delta 返回一

个N数组下标为1 到 M

% Kb = Xb^2+Yb^2;

% R = zeros(M,1);

% for i=1:M                %产生TDOA测量值

%     R(i) = -Rb+sqrt((Xreal - X(i,1))^2+(Yreal - X(i,2))^2)+N(i);

% end 

% Q = (0.5*eye(M)+0.5*ones(M))*(delta^2);

% Pbs = [Xb Yb;X];%矩阵

% N = 5;

% [POS_ref POS1 POS2 POS3 POS4] = TDOA_chan(R,Pbs,Q)

N = size(Pbs,1);%维度

K = zeros(1,N);

K = Pbs(:,1).^2 + Pbs(:,2).^2;

ha = 0.5*(R.^2-K(2:N)+K(1))

Ga = -[Pbs(2:N,1)-Pbs(1,1) Pbs(2:N,2)-Pbs(1,2) R] www.docin.com

 %计算第一次WLS估计结果远距算法

Za = inv(Ga.'*inv(Q)*Ga)*Ga.'*inv(Q)*ha;

Ba = diag(Va);

Fa = Ba*Q*Ba;

Zacov = inv(Ga.'*inv(Fa)*Ga);

%第一次计算近距算法

Va2 = sqrt((Za2(1)-Pbs(2:N,1)).^2+(Za2(2)-Pbs(2:N,2)).^2);

Ba2 = diag(Va2);

Fa2 = Ba2*Q*Ba2;

Zacov2 = inv(Ga.'*inv(Fa2)*Ga);

%第二次计算近距算法

Gb = [1 0;0 1;1 1];

Bb2 = [Za2(1)-Pbs(1,1) 0 0;0 Za2(2)-Pbs(1,2) 0;0 0

sqrt((Za2(1)-Pbs(1,1))^2+(Za2(2)-Pbs(1,2))^2)];

Fb2 = 4*Bb2*Zacov2*Bb2;

POS_ref = Za2(1:2,1);        %第一次WLS的估计结果,作为参考作最终定位结果的判决

POS1 = sqrt(Zb2)+Pbs(1,:).'; %定位结果1

POS2 = -sqrt(Zb2)+Pbs(1,:).';%定位结果2

POS3 = [-sqrt(Zb2(1));sqrt(Zb2(2))]+Pbs(1,:).';%定位结果3

POS4 = [sqrt(Zb2(1));-sqrt(Zb2(2))]+Pbs(1,:).';%定位结果4