UKF.rar_ trajectory ukf_GPS UKF_gps dat_卫星定位误差_机动方式

%UKF load PN13.dat;ps1=PN13(5001:7000,2:7); load PN22.dat;ps2=PN22(5001:7000,2:7); load PN23.dat;ps3=PN23(5001:7000,2:7); load PN27.dat;ps4=PN27(5001:7000,2:7); load PRhjqjd.dat; load hjqjdPRdot.dat; z1=PRhjqjd(1:2000,:);z2=hjqjdPRdot(1:2000,:); Z=[z1,z2]; load hjqjd.dat; X=hjqjd(1:2000,:); N=length(Z);%N=2000； %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% a=10; %机动加速度变化率的相关时间常数的倒数； af=1/1500;%钟差变化率的相关时间常数； %amx=[0,0,0];ami=[0,0,0]; %匀速 %amx=[2.5,2.5,0];ami=[-2.5,-2.5,0]; %匀加速 %amx=[110,110,80];ami=[-10,-10,-10]; %强机动 amx=[20,20,10];ami=[-20,-20,-10]; %强机动 zigt=9; %系统噪声钟差噪声,可引起6m误差。 zr32=zeros(3,2);zr3=zeros(3); %量测噪声，伪距误差，伪距变化率误差设为0. zr23=zeros(2,3); T=0.1; W1=(1-exp(-af*T))/af; W2=exp(-af*T); Ft=[1,W1;0,W2];%状态转移矩阵11行11列 Fx=[1,T,T^2/2;0,1,T;0,0,1]; F=[Fx,zr3,zr3,zr32;zr3,Fx,zr3,zr32;zr3,zr3,Fx,zr32;zr23,zr23,zr23,Ft]; qkx1=[T^5/20,T^4/8,T^3/6;T^4/8,T^3/3,T^2/2;T^3/6,T^2/2,T]; %qkt1=[T*sb+sf*T^3/3,sf*T^2/2;sf*T^2/2,sf*T]; qkt1=[T^3/3,T^2/2;T^2/2,T]; %R=diag([1e2,1e2,1e2,1e2,50,50,50,50]); %匀速 % R=diag([1e2,1e2,1e2,1e2,50,50,50,50]); %匀加速 R=diag([1e3,1e3,1e3,1e3,50,50,50,50]); %强机动 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %xEst=[-2.5414e+6,10,0,4.7811e+6,10,0,3.3621e+6,0,0,80,5]';%匀速 %PEst=diag([1e3,15,0.1,3e3,20,0.1,1e3,0.5,0.08,1e3,25]); %匀速 %xEst=[-2.5414e+6,0,0,4.7811e+6,0,0,3.3621e+6,0,0,80,5]'; %匀加速 %PEst=diag([1e3,80,50,1e3,100,50,1e3,0.1,0.1,1e3,25]); %匀加速 xEst=[-2.5414e+6,0,0,4.7811e+6,0,0,3.3621e+6,0,0,80,5]'; %强机动 PEst=diag([1e5,1e3,1e4,1e5,1e3,1e4,1e5,1e3,2e3,1e3,20]); %强机动 x_UKF=[]; deta=[]; PP_UKF=[]; tm=[]; time=[]; num=100; SUM=zeros(2000,11); SUMdeta=zeros(2000,9); SUMppx=zeros(2000,11); q=0; t_k=0; alpha=0.001;beta=2;kappa=0; %滤波递推. for j=1:num tic; while q<N q=q+1; q if(xEst(3)<=0) % 计算x方向过程噪声方差 zigx=((4-pi)*(ami(1)+xEst(3))^2)/pi; else zigx=((4-pi)*(amx(1)-xEst(3))^2)/pi; end if(xEst(6)<=0) % 计算y方向过程噪声方差 zigy=((4-pi)*(ami(2)+xEst(6))^2)/pi; else zigy=((4-pi)*(amx(2)-xEst(6))^2)/pi; end if(xEst(9)<0) % 计算y方向过程噪声方差 zigz=((4-pi)*(ami(3)+xEst(9))^2)/pi; else zigz=((4-pi)*(amx(3)-xEst(9))^2)/pi; end qkx=2*a*zigx*qkx1; qky=2*a*zigy*qkx1; qkz=2*a*zigz*qkx1; qkt=2*af*zigt*qkt1; Qk=[qkx,zr3,zr3,zr32;zr3,qky,zr3,zr32;zr3,zr3,qkz,zr32;zr23,zr23,zr23,qkt]; sX=[ps1(q,:);ps2(q,:);ps3(q,:);ps4(q,:)]; Zk=Z(q,:)'+5*randn(8,1); [xSigmaPts, wSigmaPtm,wSigmaPtc, nsp] =SigmaPoints(xEst,PEst, alpha, beta, kappa); wSigmaPts_xmat = repmat(wSigmaPtm(:,1:nsp),11,1); wSigmaPts_zmat = repmat(wSigmaPtm(:,1:nsp),8,1); wSigmaPts_cxmat = repmat(wSigmaPtc(:,1:nsp),11,1); wSigmaPts_czmat = repmat(wSigmaPtc(:,1:nsp),8,1); for i=1:nsp xPredSigmaPts(:,i) =F*xSigmaPts(:,i); zPredSigmaPts(:,i)= bshfun(xPredSigmaPts(:,i),sX); end xPred = sum(wSigmaPts_xmat .* (xPredSigmaPts(:,1:nsp)),2); zPred = sum(wSigmaPts_zmat .* (zPredSigmaPts(:,1:nsp)),2); exSigmaPt = xPredSigmaPts-repmat(xPred,1,nsp); ezSigmaPt = zPredSigmaPts-repmat(zPred,1,nsp); PPred = (wSigmaPts_cxmat .* exSigmaPt) * exSigmaPt'+Qk; S = (wSigmaPts_czmat .* ezSigmaPt) * ezSigmaPt'+R; PxzPred = exSigmaPt * (wSigmaPts_czmat .* ezSigmaPt)'; K = PxzPred / S; inovation = Zk - zPred; xEst = xPred + K*inovation; PEst = PPred - K*S*K'; t_k=t_k+T; t=t_k; x_UKF=[x_UKF;t,xEst']; detaX=abs(xEst(1:9)'-X(q,2:10)); deta=[deta;t,detaX]; PP_UKF=[PP_UKF;t,sqrt((diag(PEst))')]; %%%%%%%%%%% update Pvx and Pvy %%%%%%%%%%%%%%%%%%%%%%%%%%%%% end time =[time;toc]; SUM=SUM+x_UKF(:,2:12); SUMdeta=SUMdeta+deta(:,2:10); SUMppx=SUMppx+PP_UKF(:,2:12); end tm_UKF=sum(time)/num tm=tm_UKF/2000 SUM=SUM/num; SUMdeta=SUMdeta/num; SUMppx=SUMppx/num; ox=sum(SUMdeta)/2000 ppx=sum(SUMppx)/2000 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %-----------------------------------各个时刻递推完毕----------------------------- %- fh_laberr=figure('Name','coordinate error of UKF ','NumberTitle','off'); subplot(3,1,1) plot(0.1:0.1:200,SUMdeta(:,1),'b'); xlabel('历元/s');ylabel('x方向位置误差（m）');grid on;hold on; %ylim([-100,100]); subplot(3,1,2) plot(0.1:0.1:200,SUMdeta(:,4),'b'); xlabel('历元/s');ylabel('y方向位置误差（m）');grid on;hold on; %ylim([-100,100]); subplot(3,1,3) plot(0.1:0.1:200,SUMdeta(:,7),'b'); xlabel('历元/s');ylabel('z方向位置误差（m）');grid on;hold on; %ylim([-100,100]); fh_velerr=figure('Name','velocity error of UKF','NumberTitle','off'); subplot(3,1,1) plot(0.1:0.1:200,SUMdeta(:,2),'b'); xlabel('历元/s');ylabel('x方向速度误差（m/s）');grid on;hold on; %ylim([-100,100]); subplot(3,1,2) plot(0.1:0.1:200,SUMdeta(:,5),'b'); xlabel('历元/s');ylabel('y方向速度误差（m/s）');grid on;hold on; %ylim([-100,100]); subplot(3,1,3) plot(0.1:0.1:200,SUMdeta(:,8),'b'); xlabel('历元/s');ylabel('z方向速度误差（m/s）');grid on;hold on; %ylim([-100,100]); fh_accerr=figure('Name','accelerate error of UKF','NumberTitle','off'); subplot(3,1,1) plot(0.1:0.1:200,SUMdeta(:,3),'b'); xlabel('历元/s');ylabel('x方向加速度误差（m/s/s）');grid on;hold on; subplot(3,1,2) plot(0.1:0.1:200,SUMdeta(:,6),'b'); xlabel('历元/s');ylabel('y方向加速度误差（m/s/s）');grid on;hold on; subplot(3,1,3) plot(0.1:0.1:200,SUMdeta(:,9),'b'); xlabel('历元/s');ylabel('z方向加速度误差（m/s/s）');grid on;hold on; fh_PPlab=figure('Name','label MSE of UKF','NumberTitle','off'); subplot(3,1,1) plot(0.1:0.1:200,SUMppx(:,1),'b'); xlabel('历元/s');ylabel('x方向位置均方误差（m）');grid on;hold on; %ylim([-100,100]); subplot(3,1,2) plot(0.1:0.1:200,SUMppx(:,4),'b'); xlabel('历元/s');ylabel('y方向位置均方误差（m）');grid on;hold on; %ylim([-100,100]); subplot(3,1,3) plot(0.1:0.1:200,SUMppx(:,7),'b'); xlabel('历元/s');ylabel('z方向位置均方误差（m）');grid on;hold on; fh_PPvel=figure('Name','velocity MSE of UKF','NumberTitle','off'); subplot(3,1,1) plot(0.1:0.1:200,SUMppx(:,2),'b'); xlabel('历元/s');ylabel('x方向速度均方误差（m/s）');grid on;hold on; %ylim([-100,100]); subplot(3,1,2) plot(0.1:0.1:200,SUMppx(:,5),'b'); xlabel('历元/s');ylabel('y方向速度均方误差（m/s）');grid on;hold on; %ylim([-100,100]); subplot(3,1,3) plot(0.1:0.1:200,SUMppx(:,8),'b'); xlabel('历元/s');ylabel('z方向速度均方误差（m/s）');grid on;hold on; fh_PPacc=figure('Name','accelerate MSE of UKF','NumberTitle','off'); subplot(3,1,1) plot(0.1:0.1:200,SUMppx(:,3),'b'); xlabel('历元/s');ylabel('x方向加速度均方误差（m/s/s）');grid on;hold on; %ylim([-100,100]); subplot(3,1,2) plot(0.1:0.1:200,SUMppx(:,6),'b'); xlabel('历元/s');ylabel('y方向加速度均方误差（m/s/s）');grid on;hold on; %ylim([-100,100]); subplot(3,1,3) plot(0.1:0.1:200,SUMppx(:,9),'