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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。