% TDOA定位的Chan算法Matlab源码
function [POS_ref,POS1,POS2,POS3,POS4] = TDOA_chan(R,Pbs,Q)
%% TDOA定位定位Chan算法
% GreenSim团队原创作品，转载请注明
% Email:greensim@163.com
% GreenSim团队主页：http://blog.sina.com.cn/greensim
% [color=red]欢迎访问GreenSim——算法仿真团队→[url=http://blog.sina.com.cn/greensim]http://blog.sina.com.cn/greensim[/url][/color]
%*********************************************************
% 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);
% 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);
% Pbs = [Xb Yb;X];
% N = 5;
% [POS_ref POS1 POS2 POS3 POS4] = TDOA_chan(R,Pbs,Q)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Pbs=[42.39 -188.40 2.46
231.90 327.73 2.35
260.47 -250.76 2.94
-168.35 92.99 5.07
-109.27 163.75 5.44
199.06 -228.37 2.45
96.72 19.62 5.45
-234.99 211.08 3.32
-129.02 4.30 4.44
313.16 -135.34 3.39
43.42 4.57 5.01
47.31 80.79 3.44
276.06 132.71 5.06
-151.95 234.95 2.99
12.42 -218.40 4.11
-317.33 -272.83 5.29
247.26 -260.09 3.65
126.23 282.61 2.03
8.21 260.83 3.98
-169.91 196.13 3.74
-229.62 -250.58 4.32
-238.21 229.71 4.98
-255.44 -64.35 4.20
263.28 -317.34 2.17
-236.32 -75.30 5.19
-309.09 30.06 3.96
-260.95 323.02 2.60
325.33 -106.91 2.96
-330.78 -206.90 4.14
-122.33 -255.17 5.42
];
R=[0.00000194824290 0.00000257859249 0.00000291602036 0.00000052983782 0.00000086448079 0.00000262550616 0.00000175929170 0.00000059680598 0.00000080797823 0.00000289670968 0.00000153967394 0.00000150794352 0.00000255175414 0.00000089340137 0.00000194355534 0.00000165286087 0.00000288818722 0.00000206101746 0.00000153852643 0.00000070972574 0.00000156441842 0.00000067093507 0.00000071140360 0.00000309499756 0.00000078170013 0.00000029323031 0.00000106612411 0.00000290800820 0.00000136298326 0.00000172971710 ];
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]
评论0