《TDOA定位的Chan算法MATLAB源代码》由会员分享,可在线阅读,更多相关《TDOA定位的Chan算法MATLAB源代码(3页珍藏版)》请在金锄头文库上搜索。
1、 TDOA 定位的 Chan 算法 MATLAB 源代码 。 function POS_ref,POS1,POS2,POS3,POS4 = TDOA_chan(R,Pbs,Q) %* % CHAN 算法,假设移动台与各基站位置较近,需进行三四 WLS 计算 % 输入参数: % R(N-11): TDOA 测量值 % Pbs(N2): 基站的坐标,第一列为 X,第二列为 Y;参考基站坐标位于第一行 % 输出参数: % POS_ref(2X1):第一次 WLS 的估计结果,作为参考作最终定位结果的判决 % POS1(2X1) :定位结果 1 % POS2(2X1) :定位结果 2 % POS3(2
2、X1) :定位结果 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;%移
3、动台真实坐标 % %产生 TDOA 测量结果 % Rb = sqrt(Xreal - Xb)2+(Yreal - Yb)2);%移动台到基站的真实距离 % N = normrnd(0,delta,1,M);%产生 TDOA 测量误差 正态分布 均值 0 标准差 delta 返回一 个 N 数组,下标为 1 到 M % Kb = Xb2+Yb2; % 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
4、.5*ones(M)*(delta2); % 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;
5、 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
6、)-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 源代码运行结果展示 0.010.10.10.10.20.20.20.20.30.30.30.30.30.40.40.40.40.40.40.50.50.50.50.50.60.60.60.70.70.70.80.80.90.90.950.950.98x dimension (meters)y dimension (meters)TDOABS1(Main)BS2BS3BS401234x 1040123456x 104