《UKF应用于GPS-IMU组合导航系统的MATLAB代码》由会员分享，可在线阅读，更多相关《UKF应用于GPS-IMU组合导航系统的MATLAB代码（19页珍藏版）》请在金锄头文库上搜索。
1、组合导航系统的计算程序代码functionyy=ukf_IMUgps()%functionukf_IMUgps()%UKF在IMU/GPS组合导航系统中应用%以IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量；%以GPS中的位置、速度为观测量。%7,July2008.clc%InitialisestateglobalweRNRMgfldetawgTtwtdwwvuWRblTawad=0;%验证循环次数%地球自转角速度we(rad/s):we=7.292115e-5;g=9.81;%地球重力加速度(m/sA2)a=6.378137e+6;%地球长半轴e2=0.0066944;%地球第一偏心
2、率的平方%姿态角初始值(r,p,y)zitai=(pi/180).*02.0282196.9087;%姿态误差角fai=(pi/180).*1/361/365/36;%(100,100,500)r=zitai(1)+fai(1);p=zitai(2)+fai(2);y=zitai(3)+fai(3);%当地坐标系(I)相对于载体坐标系(b)的转换矩阵：Rbl(在e,n,u坐标系下)Rbl=cos(r)*cos(y)-sin(r)*sin(y)*sin(p)-sin(y)*cos(p)cos(y)*sin(r)+sin(y)*sin(p)*cos(r)cos(r)*sin(y)+sin(r)*c
3、os(y)*sin(p)cos(y)*cos(p)sin(y)*sin(r)-cos(y)*sin(p)*cos(r)-cos(p)*sin(r)sin(p)cos(p)*cos(r);%由转换矩阵Rbl计算四元数的初始值Q0=q1,q2,q3,q4QQ=0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)0.25*(Rbl(3,2)-Rbl(2,3)/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)0.25*(Rbl(1,3)-Rbl(3,1)/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)0.25*(Rbl(2
4、,1)-Rbl(1,2)/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3);%IMU系统给出的初始值：速度(ve,vn,vu),位置(l,m,h)=(经度，纬度，高度)，姿态误差角fai(e,n,u)vIMU=-21.775011-71.6310273.10094;point_IMU=0.1470229860.818373653122.826;T=1；%UKF滤波的采样周期(s)%陀螺常值漂移初始值tuo(e,n,u)(单位：。/s)tuo=000;%陀螺一阶相关时间Tt(s)Tt=606060;%加速度计常值漂移初始值jiasu(e,n,u)(单位：m/sA2)j
5、iasu=000;%加速度计一阶反相关时间Ta(s)Ta=606060;%IMU系统的状态向量Xx=vIMUpoint_IMUfaituojiasu;%观测量噪声V的斜方差矩阵R=;%GPS系统的量测值Z(vn,ve,vd,m,l,h)zRz=gps_tiqushuju;%Vk的噪声序列方差为：RkRz=(Rz./T);%陀螺常值漂移wt(e,n,u)wt=(pi/180).*1/(3600)1/(3600)1/(3600);%1(。/h)%陀螺常值漂移误差wtt(e,n,u)wtt=(pi/180).*0.08/(3600)0.08/(3600)0.08/(3600);%0.08(。/h)%
6、加速度计常值漂移wa(e,n,u)wa=2e-62e-62e-6;%200闵(2e-6m/sA2)%加速度计常值漂移误差waa(e,n,u)waa=2e-62e-62e-6./4;%50闻(0.5e-6m/sA2)%姿态误差角噪声wgwg=wt;%状态向量X的斜方差矩阵P=eye(length(x)*eps;%note:forstability,Pshouldneverbequitezero%速度误差：(0.1m/s)位置误差：水平(1m),高度(5m)u=0.10.10.10.0004744290.0004744292wgwttwaa;%IMU系统在载体坐标系下的比力值输出值fbfb=;%I
7、MU系统中陀螺输出量wib=;%为载体坐标系(b)相对于惯性坐标系(i)的角速度向量fw=IMU_tiqushuju;%IMU系统输出的比力值和角速度%通过GPS观测值计算得到的姿态角原始数据zitiajiao-gps.xls);%循环开始：for1:noutx=;outp=;detajiao=zeros(3,1);NN=1000;fori=1:NNoutx=outx;x;outp=outp;diag(P);%大地子午圈曲率半径：RMRM=a*(1-e2)/(1-e2*(sin(x(5)A2)A(2/3);%地球卯酉圈的曲率半径：RNRN=a/sqrt(1-e2*(sin(x(5)A2);%当
8、地坐标系下的比力值fl%IMU系统在载体坐标系下的比力值输出值fb转换到当地坐标系(1)下flfb=f(i,:);%fl=Rbl*(fb+x(13)x(14)x(15);%初始对准，比力值加上加速度计的测量偏差fl=Rbl*fb;%计算IMU的速度、位置输出减去GPS相应的输出值：deta(ve,vn,1,h)zd=z(i,:);deta=x(1256,:)-zd(1256,:);%观测值zz,及观测噪声Rzz=z(i+1,:);v=Rz(i+1,:);%zitai_v=0.0010.02660.9664;%GPS观测值姿态角的误差zitai_v=0.000017.0983e-0040.000
9、6;%GPS观测值姿态角的误差v=v;zitai_v;R=diag(v.A2);%通过GPS观测值，计算得到roll=0,pitch=atan(ve/vn),yaw=asin(h12/s12),将姿态误差角加入观测量中进行滤波计算zz=zz;detajiao;%GPS计算得到的姿态角z_zitai=zitaijiao_gps(i+1,:);%IMU系统的力学编排方程和姿态误差方程离散化之后的截断误差：ve=x(1);vn=x(2);vu=x(3);l=x(4);m=x(5);h=x(6);faie=x(7);fain=x(8);faiu=x(9);tuo1=x(10);tuo2=x(11);t
10、uo3=x(12);fl1=fl(1);fl2=fl(2);fl3=fl(3);deta1=deta(1);deta2=deta(2);deta3=deta(3);deta4=deta(4);wg1=wg(1);wg2=wg(2);wg3=wg(3);jiasu1=x(13);jiasu2=x(14);jiasu3=x(15);Ta1=Ta(1);Ta2=Ta(2);Ta3=Ta(3);wa1=wa(1);wt2=wa(2);wt3=wa(3);Q=diag(u).A2);%predictx,P=predict(x,P,u,Q,T);%updatex,P=update(x,P,zz,R);%x
11、x=x;x=xx(1:15,1);u=xx(16:30,1);%u(1)=u(1)+x(13);%u(2)=u(2)+x(14);%u(3)=u(3)+x(15);%u(7)=x(10);%u(8)=x(11);%u(9)=x(12);PP=P;P=PP(1:15,1:15);%利用四元数Q计算转换矩阵Rbl%首先计算在当地坐标系(l)下的角速度向量wbl%地固坐标系(e)相对于当地坐标系(1)的转换矩阵：Rel=Rle%Rel=-sin(x(4)cos(x(4)0%-sin(x(5)*cos(x(4)-sin(x(5)*sin(x(4)cos(x(5)%cos(x(5)*cos(x(4)co
12、s(x(5)*sin(x(4)sin(x(5);wie=00we;%wie为地球自转角速度向量%omiga12=;fork=0:1wib=w(i+k,:)+T.*x(10)x(11)x(12);%初始对准，角速度加上陀螺漂移wie1=0we*cos(x(5)we*sin(x(5);%wie1=Re1*wie;we11=-x(2)/(RM+x(6)x(1)/(RN+x(6)x(1)*tan(x(5)/(RN+x(6);wi11=wie1+we11;w1b=wib-Rb1*wi11;%四元数的更新%计算反对称矩阵omigaomiga=0w1b(3)-w1b(2)w1b(1)-wlb(3)0wlb(
13、1)wlb(2)wlb(2)-wlb(1)0wlb(3)wlb(1)wlb(2)wlb(3)0;omiga12=omiga12,omiga;endomiga1=omiga12(1:4,1:4);omiga2=omiga12(1:4,5:8);%采用四阶龙格库塔法数值积分解Q(K+1)QQ=QQ+(0.5*T.*(omiga1+omiga2)+8A(-1)*TA2.*(omiga1A2+omiga1*omiga2+omiga2A2)+48人(-1)*TA3.*(omiga1A2*omiga2+omiga1*omiga2A2)+384A(-1)*TA4.*(omiga1A2*omiga2A2)*Q
14、Q;%由Q(k+1)可得Rbl(k+1)Rbl=QQ(1)A2+QQ(2)A2-QQ(3)A2-QQ(4)A22*(QQ(2)*QQ(4)+QQ(1)*QQ(3)2*(QQ(2)*QQ(3)+QQ(1)*QQ(4)2*(QQ(3)*QQ(4)-QQ(1)*QQ(2)2*(QQ(2)*QQ(4)-QQ(1)*QQ(3)QQ(1)A2-QQ(2)A2-QQ(3)A2+QQ(4)A2;2*(QQ(2)*QQ(3)-QQ(1)*QQ(4)QQ(1)A2-QQ(2)A2+QQ(3)A2-QQ(4)A22*(QQ(2)*QQ(1)+QQ(4)*QQ(3)%由Rbl（k+1）可得姿态角，（翻滚角r,俯仰角p,航向角y）k+1%实际导航系（p系）相对于理想导航系（n系）存在误差角fai（e,n,u）,Cpn为校正矩阵%当方位角为大失准角时Cpn=cos(x(9)-sin(x(9)x(8)*cos(x(9)+x(7)*sin(x(9)sin(x(9)cos(x(9)x(8)*sin(x(9)-x(7)*cos(x(9)-x(8)x(7)1;%当三个方向的失准角为小角度时Cpnn=1-x(9)x(8)x(9)1-x(7)-x(8)x(7)1;ifabs(x(9)*180*60/pi10
