• PUDN用户
    了解作者
  • matlab
    开发工具
  • 2KB
    文件大小
  • rar
    文件格式
  • 1
    收藏次数
  • 1 积分
    下载积分
  • 95
    下载次数
  • 2013-04-22 21:33
    上传日期
ukf代码 主要是提供北斗卫星是如何实现定位
UKF.rar
  • UKF.m
    6.5KB
内容介绍
close all; clear all; clc; J2=1.08263*(10^(-3)); %地球引力场的J2摄动 Re=6378; %地球半径km sd = 0.01; u=3.986*(10^5); %地球引力常数 T=1;%采样周期 R=diag([25000,25000,0.4,0.4]); %观测噪声协方差阵 Q=diag([10e4,10e4,10e4,10e1,10e1,10e1]); % xX=load('F:\UKF低轨卫星定轨算法\数据\low.txt'); % gps=load('F:\UKF低轨卫星定轨算法\数据\E4.txt'); % gps1=load('F:\UKF低轨卫星定轨算法\数据\F3.txt'); X=[ 707.153305 1411.910380 -6746.845842 -2.606802 -6.866442 -1.776126]; xX=load('F:\UKF低轨卫星定轨算法\数据\COMPASS\LOW.txt'); gps=load('F:\UKF低轨卫星定轨算法\数据\COMPASS\IGSO2.txt'); gps1=load('F:\UKF低轨卫星定轨算法\数据\COMPASS\MEO20.txt'); PP=diag([500^2,500^2,500^2,0.2^2,0.2^2,10^(-6)]); %PP=diag([500^4,500^4,500^4,0.2^4,0.2^4,10^(-2)]); XX=[];YY=[];ZZ=[]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% UKF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % X=[ 2384.691487 6260.352526 1287.405051 0.700177 1.345374 -7.523940]; L=6; % 状态方程维数 alpha=1e-2; % 表示向量在状态均值附近的遍布范围,它通常取一个很小的正数1e-4<=alpha<=1; beta=2; % 描述状态变量x的先验分布信息。对于高斯分布=2最优 kappa=3-L; % Seconda Scaling Parameter.常取作0或3-L,对于高斯分布kappa=3-L最优; 取作0或3-L影响不大 lamda=alpha^2*(L+kappa)-L; % 分布参数 Scaling Parameter. rrr=sqrt(L+lamda);%(L*alpha^2)^0.5;; % 为方便所取的中间变量 W0m=lamda/(L+lamda); W0c=W0m+(1-alpha^2+beta); for i=1:2*L Wm(1,i)=1/(2*(L+lamda)); end Wc=Wm; Wic=[W0c Wc]; Wim=[W0m Wm]; XXU=[];YYU=[];ZZU=[]; g0=[];gg0=[]; g01=[];gg01=[]; x=[];y=[];z=[];gx1=[];gy1=[];gz1=[];gx2=[];gy2=[];gz2=[]; n=3000; for i=1:n x=[x xX(i,1)]; y=[y xX(i,2)]; z=[z xX(i,3)]; gx1=[gx1 gps(i,1)]; gy1=[gy1 gps(i,2)]; gz1=[gz1 gps(i,3)]; gx2=[gx2 gps1(i,1)]; gy2=[gy2 gps1(i,2)]; gz2=[gz2 gps1(i,3)]; g0=[g0 sqrt(( gps(i,1)- xX(i,1))^2+(gps(i,2)-xX(i,2))^2+(gps(i,3)-xX(i,3))^2)];% 伪距 gg0=[gg0 ((( gps(i,1)- xX(i,1))*( gps(i,4)- xX(i,4)))+((gps(i,2)-xX(i,2))*(gps(i,5)-xX(i,5)))+((gps(i,3)-xX(i,3))*(gps(i,6)-xX(i,6))))/g0(i)];%伪距率 g01=[g01 sqrt(( gps1(i,1)- xX(i,1))^2+(gps1(i,2)-xX(i,2))^2+(gps1(i,3)-xX(i,3))^2)];% 伪距 gg01=[gg01 ((( gps1(i,1)- xX(i,1))*( gps1(i,4)- xX(i,4)))+((gps1(i,2)-xX(i,2))*(gps1(i,5)-xX(i,5)))+((gps1(i,3)-xX(i,3))*(gps1(i,6)-xX(i,6))))/g01(i)];%伪距率 end for i=0:T:n-1 a=gps(i+1,:); a1=gps1(i+1,:); zk=[g0(1,i+1),g01(1,i+1),gg0(1,i+1),gg01(1,i+1)];%+0.01*randn(1,4);%%观测方程 r=sqrt(X(1)^2+X(2)^2+X(3)^2); r1=r^3; f=1-J2*((Re/r)^2)*(7.5*((X(3)^2)/(r^2))-1.5); f1=7.5*X(3)^2/r^2-1.5; f11=7.5*X(3)^2/r^2-4.5; f2=X(1)/r^3*J2*(Re^2); f3=X(2)/r^3*J2*(Re^2); f4=X(3)/r^3*J2*(Re^2); f41=-u*(((r^2-3*X(1)^2)/(r^5))*f+f2*(2*X(1)/r^4*f1+15*X(1)*(X(3)^2)/(r^6))); f42=-u*((-3*X(1)*X(2)/(r^5))*f+f2*(2*X(2)/r^4*f1+15*X(2)*X(3)^2/r^6)); f43=-u*((-3*X(1)*X(3)/(r^5))*f+f2*(2*X(3)/r^4*f1+15*(X(3)^3-X(3)*r^2)/r^6)); f51=-u*((-3*X(1)*X(2)/(r^5))*f+f3*(2*X(1)/r^4*f1+15*X(1)*X(3)^2/r^6)); f52=-u*(((r^2-3*X(2)^2)/(r^5))*f+f3*(2*X(2)/r^4*f1+15*X(2)*(X(3)^2)/(r^6))); f53=-u*((-3*X(2)*X(3)/(r^5))*f+f3*(2*X(3)/r^4*f1+15*(X(3)^3-X(3)*r^2)/r^6)); f61=-u*((-3*X(1)*X(3)/(r^5))*f+f4*(2*X(1)/r^4*f11+15*X(1)*X(3)^2/r^6)); f62=-u*((-3*X(2)*X(3)/(r^5))*f+f4*(2*X(2)/r^4*f11+15*X(2)*X(3)^2/r^6)); f63=-u*(((r^2-3*X(3)^2)/(r^5))*f+f4*(2*X(3)/r^4*f11+15*(X(3)^3-X(3)*r^2)/r^6)); Fu=[ 0,0,0,1,0,0; 0,0,0,0,1,0; 0,0,0,0,0,1; f41,f42,f43,0,0,0; f51,f52,f53,0,0,0; f61,f62,f63,0,0,0]; [Pr p]=chol(PP); % Pr = P's root pr1 X1=[X' X'*ones(1,L)+rrr*Pr X'*ones(1,L)-rrr*Pr]; % 计算UKF的Sigma 点 for k=1:2*L+1 X=[X1(:,k)]'; r=sqrt(X(1)^2+X(2)^2+X(3)^2); r1=r^3; f=1-J2*((Re/r)^2)*(7.5*((X(3)^2)/(r^2))-1.5); f1=7.5*X(3)^2/r^2-1.5; f11=7.5*X(3)^2/r^2-4.5; f2=X(1)/r^3*J2*(Re^2); f3=X(2)/r^3*J2*(Re^2); f4=X(3)/r^3*J2*(Re^2); f41=-u*(((r^2-3*X(1)^2)/(r^5))*f+f2*(2*X(1)/r^4*f1+15*X(1)*(X(3)^2)/(r^6))); f42=-u*((-3*X(1)*X(2)/(r^5))*f+f2*(2*X(2)/r^4*f1+15*X(2)*X(3)^2/r^6)); f43=-u*((-3*X(1)*X(3)/(r^5))*f+f2*(2*X(3)/r^4*f1+15*(X(3)^3-X(3)*r^2)/r^6)); f51=-u*((-3*X(1)*X(2)/(r^5))*f+f3*(2*X(1)/r^4*f1+15*X(1)*X(3)^2/r^6)); f52=-u*(((r^2-3*X(2)^2)/(r^5))*f+f3*(2*X(2)/r^4*f1+15*X(2)*(X(3)^2)/(r^6))); f53=-u*((-3*X(2)*X(3)/(r^5))*f+f3*(2*X(3)/r^4*f1+15*(X(3)^3-X(3)*r^2)/r^6)); f61=-u*((-3*X(1)*X(3)/(r^5))*f+f4*(2*X(1)/r^4*f11+15*X(1)*X(3)^2/r^6)); f62=-u*((-3*X(2)*X(3)/(r^5))*f+f4*(2*X(2)/r^4*f11+15*X(2)*X(3)^2/r^6)); f63=-u*(((r^2-3*X(3)^2)/(r^5))*f+f4*(2*X(3)/r^4*f11+15*(X(3)^3-X(3)*r^2)/r^6)); Fu=[ 0,0,0,1,0,0; 0,0,0,0,1,0; 0,0,0,0,0,1; f41,f42,f43,0,0,0; f51,f52,f53,0,0,0; f61,f62,f63,0,0,0]; deltax=[X(4);X(5);X(6);-(u*X(1)/r1)*f;-(u*X(2)/r1)*f; -(u*X(3)/r1)*f]*T; Xu(:,k)=X'+deltax+Fu*deltax*0.5; end X=Xu*Wim'; Pre=(Xu-X*ones(1,2*L+1))*diag(Wic)*(Xu-X*ones(1,2*L+1))'+Q; for k=1:2*L+1 U=[Xu(:,k)]'; g=sqrt((a(1)-U(1))^2+(a(2)-U(2))^2+(a(3)-U(3))^2);% 伪距 g1=sqrt((a1(1)-U(1))^2+(a1(2)-U(2))^2+(a1(3)-U(3))^2);% 伪距 gg=(((a(1)-U(1))*(a(4)-U(4)))+((a(2)-U(2))*(a(5)-U(5)))+((a(3)-U(3))*(a(6)-U(6))))/g;%伪距率 gg1=(((a1(1)-U(1))*(a1(4)-U(4)))+((a1(2)-U(2))*(a1(5)-U(5)))+((a1(3)-U(3))*(a1(6)-U(6))))/g1;%伪距率 Zu(1,k)=g; Zu(2,k)=g1; Zu(3,k)=gg; Zu(4,k)=gg1;% 测量推进 end zu=Zu*Wim'; % 传播后测量均值 Pzz1=(Zu-zu*ones(1,2*L+1))*diag(Wic)*(Zu-zu*ones(1,2*L+1))'+R; Pxz1=(Xu-X*ones(1,2*L+1))*diag(Wic)*(Zu-zu*ones(1,2*L+1))'; Ku=Pxz1*inv(Pzz1); % 增益矩阵推进 X=X+Ku*(zk'-zu); % 利用新息预报 PP=Pre-Ku*Pzz1*Ku'; % 协方差推进 XXU=[XXU X(1,:)]; YYU=[YYU X(2,:)]; ZZU=[ZZU X(3,:)]; X=X'; Xx(i+1,:)=X;%记录预测值 end for i=1:n errorW(i,:)=(xX(i,1:3)-Xx(i,1:3)); errorS(i,:)=(xX(i,4:6)-Xx(i,4:6)); end figure(1) plot3(x',y',z','g-') hold on plot3(XXU',YYU',ZZU','r-'); hold on plot3(gx1',gy1',gz1','b'); hold on plot3(gx2',gy2',gz2','k'); grid on figure(2) plot(errorW,'y') figure(3) plot(errorS,'b')
评论
    相关推荐
    • matlabcnhelp.rar
      matlab中文帮助很难找的,快速下载
    • MobilePolice.rar
      移动警察,车牌识别,车牌定位系统源代码,已经运用在移动车载稽查系统中。
    • SVM(matlab).rar
      支持向量机(SVM)实现的分类算法源码[matlab]
    • svm.zip
      用MATLAB编写的svm源程序,可以实现支持向量机,用于特征分类或提取
    • Classification-MatLab-Toolbox.rar
      模式识别matlab工具箱,包括SVM,ICA,PCA,NN等等模式识别算法,很有参考价值
    • VC++人脸定位实例.rar
      一个经典的人脸识别算法实例,提供人脸五官定位具体算法及两种实现流程.
    • QPSK_Simulink.rar
      QPSK的Matlab/Simulink的调制解调仿真系统,给出接收信号眼图及系统仿真误码率,包含载波恢复,匹配滤波,定时恢复等重要模块,帮助理解QPSK的系统
    • LPRBPDemo2009KV.rar
      车牌识别,神经网络算法,识别率高达95%,识别时间低于80ms。
    • MODULATION.RAR
      这个源程序代码包提供了通信系统中BPSK,QPSK,OQPSK,MSK,MSK2,GMSK,QAM,QAM16等调制解调方式 用matlab的实现,以及它们在AWGN和Rayleigh信道下的通信系统实现及误码率性能
    • algorithms.rar
      十大算法论文,包括遗传算法,模拟退火,蒙特卡罗法等等,对于初学者很有帮助!!