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

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

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

%? CHAN算法,假設(shè)移動(dòng)臺與各基站位置較近,需進(jìn)行三四WLS計(jì)算

%? 輸入?yún)?shù):

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

%??????????? Pbs(N×2): 基站的坐標(biāo),第一列為X,第二列為Y;參考基站坐標(biāo)位于第一行

%? 輸出參數(shù):?

%??????????? POS_ref(2X1):第一次WLS的估計(jì)結(jié)果,作為參考作最終定位結(jié)果的判決

%??????????? POS1(2X1)?? :定位結(jié)果1

%??????????? POS2(2X1)?? :定位結(jié)果2

%??????????? POS3(2X1)?? :定位結(jié)果3

%??????????? POS4(2X1)?? :定位結(jié)果4

%這段程序用于自己產(chǎn)生算法輸入?yún)?shù),用于算法測試

% delta = 10;????????? %TDOA測量誤差的標(biāo)準(zhǔn)差,用于產(chǎn)生Q矩陣

% M=4;???????????????? %參考基站外的基站數(shù)量

% Xb = 20;Yb = 100;??? %參考的基站坐標(biāo)

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

% a = 2*pi/M;

% for i=1:M??????????? %生成其他基站的坐標(biāo)

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

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

% end

% Xreal = -150;Yreal = 200;%移動(dòng)臺真實(shí)坐標(biāo)

% %產(chǎn)生TDOA測量結(jié)果

% Rb = sqrt((Xreal - Xb)^2+(Yreal - Yb)^2);%移動(dòng)臺到基站的真實(shí)距離

% N = normrnd(0,delta,1,M);%產(chǎn)生TDOA測量誤差 正態(tài)分布 均值0 標(biāo)準(zhǔn)差 delta 返回一

個(gè)N數(shù)組下標(biāo)為1 到 M

% Kb = Xb^2+Yb^2;

% R = zeros(M,1);

% for i=1:M??????????????? %產(chǎn)生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

?%計(jì)算第一次WLS估計(jì)結(jié)果遠(yuǎn)距算法

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

Ba = diag(Va);

Fa = Ba*Q*Ba;

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

%第一次計(jì)算近距算法

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);

%第二次計(jì)算近距算法

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的估計(jì)結(jié)果,作為參考作最終定位結(jié)果的判決

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

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

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

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

總結(jié)

以上是生活随笔為你收集整理的TDOA定位的Chan算法MATLAB源代码的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。