![]() |
【原创】TDOA/AOA/AOD混合定位最小二乘定位算法MATLAB源码
TDOA/AOA/AOD混合定位的非线性比较强,这里给出其最小二乘的泰勒级数展开最小二乘算法。本源码由GreenSim团队原创,转载请注明,有意购买源码或代写相关程序,请与GreenSim团队联系。
function [X,AllX,Alldxy]=Taylor_TDOA_AOA_AOD(X0,Theta,Phi,Tau,xb,yb,Delta,K) %% TDOA/AOA/AOD定位的泰勒级数展开最小二乘算法 % 参考文献 % Ji Li, Ligen Wang, Jean-Jules Brault, Jean Conan % Mobile Location in MIMO Communication Systems by Using Learning Machine % 要求多径(散射体)个数:N>=3 % GreenSim团队原创作品,转载请注明 % [color=red]欢迎访问GreenSim——算法仿真团队→[url=http://blog.sina.com.cn/greensim]http://blog.sina.com.cn/greensim[/url][/color] %% 输入参数列表 % X0 迭代初始值(米),(2N+2)×1列向量,排列规则为[x1,y1,x2,y2,…,xN,yN,xm,ym] % Theta 移动台到达角观测值AOA(弧度),N×1列向量 % Phi 基站离开角观测值AOD(弧度),N×1列向量 % Tau 传播时延差的观测值TDOA,折算成距离(米),(N-1)×1列向量 % (xb,yb) 基站坐标(米) % Delta 迭代停止的门限(米) % K 最大迭代次数 %% 输出参数列表 % X 输出结果,含散射体和移动台的定位结果 %% N=length(Theta);%多径(散射体)个数 H=zeros(3*N-1,2*N+2);%H矩阵初始化 Rho=zeros(3*N-1,1);%计算观测值初始化 AllX=zeros(K,2*N+2); Alldxy=zeros(K,1); dxy=Inf;%收敛误差初始化 X=X0;%迭代初始点 counter=1;%迭代计数器 while dxy>Delta xm=X(2*N+1); ym=X(2*N+2); x1=X(1); y1=X(2); for i=1:N xi=X(2*i-1); yi=X(2*i); %以下是防止分母为零而采取的措施 if xi==xm xi=xm+0.000001; end if yi==ym yi=ym+0.000001; end if xi==xb xi=xb+0.000001; end if yi==yb yi=yb+0.000001; end %以下是H矩阵和Rho向量的前N行 H(i,2*i-1)=-(yi-ym)/(xi-xm)^2/(1+(yi-ym)^2/(xi-xm)^2); H(i,2*i)=1/(xi-xm)/(1+(yi-ym)^2/(xi-xm)^2); H(i,2*N+1)=(yi-ym)/(xi-xm)^2/(1+(yi-ym)^2/(xi-xm)^2); H(i,2*N+2)=-1/(xi-xm)/(1+(yi-ym)^2/(xi-xm)^2); Rho(i,1)=atan((yi-ym)/(xi-xm)); %以下是H矩阵的中间N行 H(N+i,2*i-1)=-(yi-yb)/(xi-xb)^2/(1+(yi-yb)^2/(xi-xb)^2); H(N+i,2*i)=1/(xi-xb)/(1+(yi-yb)^2/(xi-xb)^2); Rho(N+i,1)=atan((yi-yb)/(xi-xb)); %以下是H矩阵的后N-1行 if i>=2 H(2*N+i-1,1)=-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(2*x1-2*xm)-1/2/(x1^2-2*x1*xb+xb^2+y1^2-2*y1*yb+yb^2)^(1/2)*(2*x1-2*xb); H(2*N+i-1,2)=-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(2*y1-2*ym)-1/2/(x1^2-2*x1*xb+xb^2+y1^2-2*y1*yb+yb^2)^(1/2)*(2*y1-2*yb); H(2*N+i-1,2*i-1)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(2*xi-2*xm)+1/2/(xi^2-2*xi*xb+xb^2+yi^2-2*yi*yb+yb^2)^(1/2)*(2*xi-2*xb); H(2*N+i-1,2*i)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(2*yi-2*ym)+1/2/(xi^2-2*xi*xb+xb^2+yi^2-2*yi*yb+yb^2)^(1/2)*(2*yi-2*yb); H(2*N+i-1,2*N+1)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(-2*xi+2*xm)-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(-2*x1+2*xm); H(2*N+i-1,2*N+2)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(-2*yi+2*ym)-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(-2*y1+2*ym); Rho(2*N+i-1,1)=sqrt((xi-xm)^2+(yi-ym)^2)+sqrt((xi-xb)^2+(yi-yb)^2)-sqrt((x1-xm)^2+(y1-ym)^2)-sqrt((x1-xb)^2+(y1-yb)^2); end end %构造Y向量 Y=[Theta;Phi;Tau]-Rho; %收敛误差 dxy=sqrt(sum(DX.^2)); %更新X X=X+DX; AllX(counter,:)=X'; Alldxy(counter,1)=dxy; %显示(xm,ym) %disp(counter); %disp(X(2*N+1:2*N+2)); counter=counter+1; if counter>K return end end AllX=AllX(1:counter-1,:); Alldxy=Alldxy(1:counter-1,1); |
回复: 【原创】TDOA/AOA/AOD混合定位最小二乘定位算法MATLAB源码
绝对的广告,真是鄙视!
|
所有时间均为北京时间。现在的时间是 12:58。 |
Powered by vBulletin
版权所有 ©2000 - 2025,Jelsoft Enterprises Ltd.