三维TDOAAOA定位的扩展卡尔曼滤波仿真主程序..docVIP

%% 三维TDOA/AOA定位的扩展卡尔曼滤波仿真主程序clcclearclose all%% 设置观测站分布N=4;%观测站个数%主站位于坐标原点x1=0;y1=0;z1=0;x2=1000;y2=0;z2=0;x3=0;y3=1000;z3=0;x4=1000;y4=1000;z4=0;XI=[x1,x2,x3,x4];YI=[y1,y2,y3,y4];ZI=[z1,z2,z3,z4];%% 产生观测数据%假设被定位对象匀速地从A点(100,300,300)运动到B点(900,700,0)，单位：米Px=[100:0.8:900];Py=[300:0.4:700];Pz=[300:-0.3:0];%假设采样时间间隔为1sT=1;%根据观测误差水平，产生各个观测站的观测量M=length(Px);%离散点数Alpha=zeros(M,N);Beta=zeros(M,N);Distance=zeros(M,N);Sigma_Alpha_Theta=1;%以角度形式表示的角度噪声标准差Sigma_Alpha=((Sigma_Alpha_Theta)/180)*pi;%转化为弧度表示的角度噪声方差Sigma_Beta_Theta=1.5;%以角度形式表示的角度噪声标准差Sigma_Beta=((Sigma_Beta_Theta)/180)*pi;%转化为弧度表示的角度噪声方差Sigma_Distance=2;%距离测量的标准差for n=1:N Alpha(:,n)=atan2(Py-YI(n),Px-XI(n))+Sigma_Alpha*randn(M,1); Beta(:,n)=atan2(Pz-ZI(n),sqrt((Px-XI(n)).^2+(Py-YI(n)).^2))+Sigma_Beta*randn(M,1); Distance(:,n)=sqrt((Px-XI(n)).^2+(Py-YI(n)).^2+(Pz-ZI(n)).^2)+Sigma_Distance*randn(M,1);endRho=Distance(:,2:end)-Distance(:,1)*ones(1,N-1); %% 使用扩展卡尔曼滤波进行定位SigmaU=0.005;%驱动噪声S0=[120,270,320,0.5,0.1,-0.4];%初始状态向量P0=0.01*ones(6,6);%预测误差矩阵的初始值[MX,MY,MZ]=EKF_TDOA_AOA_3D(Alpha,Beta,Rho,XI,YI,ZI,SigmaU,Sigma_Alpha,Sigma_Beta,Sigma_Distance,T,S0,P0); %% 绘图figureplot3(Px,Py,Pz,b);xlabel(X轴);ylabel(Y轴);zlabel(Z轴);grid onaxis([0 1000 0 1000 0 300]);hold onplot3(MX,MY,MZ,r);title(跟踪轨迹);%figureplot(MX-Px);xlabel(迭代次数);ylabel(误差（米）);title(X轴收敛曲线);grid on%figureplot(MY-Py);xlabel(迭代次数);ylabel(误差（米）);title(Y轴收敛曲线);grid on%figureplot(MZ-Pz);xlabel(迭代次数);ylabel(误差（米）);title(Z轴收敛曲线);grid on%figureERR=sqrt((MX-Px).^2+(MY-Py).^2+(MZ-Pz).^2);plot(ERR);xlabel(迭代次数);ylabel(误差（米）);title(误差收敛曲线);grid on function [X,AllX,Alldxy]=TDOA_AOA_3D(X0,Theta,Phi,Tau,xb,yb,Delta,K)% GreenSim团队原创作品，转载请注明% Email:greensim@163.com% GreenSim团队主页：/greensim% [color=red]欢迎访问GreenSim——算法仿真团队→[url=/greensim]/greensim[/url][/color]N=length(Theta);H=zeros(3*N-1,2*N+2);Rho=zeros(3*N-1,1);AllX=zeros(K,2*N+2);Alldxy=zeros(K,1);dxy=Inf;X=X0;cou