• PUDN用户
    了解作者
  • matlab
    开发工具
  • 758KB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 1 积分
    下载积分
  • 152
    下载次数
  • 2010-03-22 13:02
    上传日期
GPS卫星定位中滤波算法的实现,采用UKF对机动目标实现定位,可现实定位误差。可参考
UKF.rar
  • UKF_Model.m
    12.3KB
  • PN23.dat
    779.3KB
  • PN27.dat
    779.3KB
  • PN22.dat
    779.3KB
  • hjqjd.dat
    316.6KB
  • PRhjqjd.dat
    129KB
  • PN13.dat
    779.3KB
  • hjqjdPRdot.dat
    129KB
内容介绍
%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),'
评论
    相关推荐
    • UKF.rar
      GPS定位采用UKF滤波方式。包括调用的数据dat文件(选用卫星数据和机动目标运行轨迹数据)和调用的sigma点生成子程序。运行可观察定位误差(位置、速度和加速度)
    • 高精度捷联惯性导航系统Matlab工具箱.rar
      工具箱主要功能: 1) 姿态向量、四元数、...速度+姿态传递对准 4) 纯惯性导航SINS仿真 航位推算、SINS/DR仿真 SINS/GPS组合仿真 GPS/BD/GLONASS单点伪距定位 SINS/GPS松/紧组合 POS正逆向数据处理与信息融合仿真 5)
    • UKF.rar
      ukf代码 主要是提供北斗卫星是如何实现定位
    • 目标定位.rar
      研究目标跟踪的状态估计方法,最小二乘估计,Kalman滤波,扩展Kalman滤波,无迹Kalman滤波以及粒子滤波等,理论及MATLAB源程序。
    • 06978872.rar
      有关雷达跟踪的IEEE文献,主要讲述EKF和UKF滤波算法,有效地进行了跟踪定位,具有很好的研究价值
    • 卡尔曼多AUV协同定位文献集
      最新查询近二十多篇文献,期刊+学位论文,写代码时帮了大忙。
    • 卡尔曼滤波程序 GPS定位仿真
      针对于 卫星接受数据的 卡尔曼滤波定位, 主要是 UKF sage自适应滤波的MATLAB程序 里面有定位的误差图 数据包 定位仿真结果 等
    • 卡尔曼滤波程序 GPS定位仿真
      针对于 卫星接受数据的 卡尔曼滤波定位, 主要是 UKF sage自适应滤波的MATLAB程序 里面有定位的误差图 数据包 定位仿真结果 等
    • GNSS 与惯性及多传感器组合导航系统原理 (第二版)
      本书的主要目的如下: (1)既从定性的角度、又从...第二版中的新内容,则包括 UKF、粒子滤波、 GNSS 阴影匹配、运动约束、场景定位、合作/协同、非完整惯性测量单元( inertial measurement u时,1M町、系统设计和测试。
    • 人事管理系统(VFP版).rar
      给别人做过的一个毕业设计。用VFP做的。