加载中…
个人资料
GreenSim团队
GreenSim团队
  • 博客等级:
  • 博客积分:0
  • 博客访问:448,622
  • 关注人气:680
  • 获赠金笔:0支
  • 赠出金笔:0支
  • 荣誉徽章:
正文 字体大小:

TDOA定位的Chan算法Matlab源码

(2009-07-26 12:47:49)
标签:

杂谈

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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;
Ba = diag(Va);
Fa = Ba*Q*Ba;
Zacov = inv(Ga.'*inv(Fa)*Ga);
%第一次WLS计算(近距算法)
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);
%第二次WLS计算(近距算法)
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
          

0

阅读 收藏 喜欢 打印举报/Report
  

新浪BLOG意见反馈留言板 欢迎批评指正

新浪简介 | About Sina | 广告服务 | 联系我们 | 招聘信息 | 网站律师 | SINA English | 产品答疑

新浪公司 版权所有