【论文】二维坐标下TDOA之chan算法_chan定位算法

function [POS_ref,POS1,POS2,POS3,POS4] = TDOA_chan(R,Pbs,Q) 
 
delta = 10;          %TDOA测量误差的标准差,用于产生Q矩阵 
M=4;                 %参考基站外的基站数量 
Xb = 20;Yb = 100;    %参考的基站坐标 
X = zeros(M,2); 
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测量误差 
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); % TDOA协方差矩阵， 
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] 
 
%计算第一次WLS估计结果（远距算法） 
Za = inv(Ga.'*inv(Q)*Ga)*Ga.'*inv(Q)*ha; 
Va = R; 
Ba = diag(Va); 
Fa = Ba*Q*Ba; 
Zacov = inv(Ga.'*inv(Fa)*Ga); 
%第一次ＷＬＳ计算（近距算法） 
Za2 = inv(Ga.'*inv(Fa)*Ga)*Ga.'*inv(Fa)*ha; 
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的估计结果,作为参考作最终定位结果的判决 
hb = [(Za2(1)-Pbs(1,1))^2;(Za2(2)-Pbs(1,2))^2;(Za2(3))^2]; 
 
Zb2 = inv(Gb.'*inv(Fb2)*Gb)*Gb.'*inv(Fb2)*hb; ;  
 
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 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

>> TDOA_chan
POS1 =

  155.6420
  184.9247


POS2 =

 -115.6420
   15.0753


POS3 =

 -115.6420
  184.9247


POS4 =

  155.6420
   15.0753
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23