资源描述：
《ukf应用于gps-imu组合导航系统的matlab代码》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。
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;%地球重力加
2、速度（m/s^2）a=6.378137e+6;%地球长半轴e2=0.0066944;%地球第一偏心率的平方%姿态角初始值（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);%当地坐标系（l）相对于载体坐标系（b）的转换矩阵：Rbl(在e,n,u坐标系下）Rb
3、l=[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)*cos(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,q4]'QQ=[0.5*sqrt(1+Rb
4、l(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,1)-Rbl(1,2))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))];%IMU系统给出的初始值：速度（ve,vn,vu），位置（l,
5、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/s^2）jiasu=[000];%加速度计一阶反相关时间Ta(s)
6、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
7、.08/(3600)0.08/(3600)];%0.08(。/h)%加速度计常值漂移wa(e,n,u)wa=[2e-62e-62e-6];%200μg(2e-6m/s^2)%加速度计常值漂移误差waa(e,n,u)waa=[2e-62e-62e-6]./4;%50μg(0.5e-6m/s^2)%姿态误差角噪声wgwg=wt;%状态向量X的斜方差矩阵P=eye(length(x))*eps;%note:forstability,Pshouldneverbequitezero%速度误差：（0.1m/s)位
8、置误差：水平（1m),高度（5m)u=[0.10.10.10.0004744290.0004744292wgwttwaa]';%IMU系统在载体坐标系下的比力值输出值fbfb=[];%IMU系统中陀螺输出量wib=[];%为载体坐标系（b）相对于惯性坐标系（i）的角速度向量[fw]=IMU_tiqushuju;%IMU系统输出的比力值和角速度%%%%%%%%%通过GPS观测值计算得到的姿态角zitaijiao_gps=xlsread('D:myfile
