UKF应用于GPS-IMU组合导航系统的MATLAB代码.pdf

精品文档 组合导航系统的计算程序代码 function yy=ukf_IMUgps() %function ukf_IMUgps() % UKF在 IMU/GPS组合导航系统中应用 % % 以 IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量； % 以GPS中的位置、速度为观测量。 % % 7,July 2008. clc % Initialise state global we RN RM g fl deta wg Tt wt d ww v u W Rbl Ta wa d=0; % 验证循环次数 %地球自转角速度 we(rad/s): we=7.292115e-5; g=9.81; % 地球重力加速度（ m/s^2 ） a=6.378137e+6; % 地球长半轴 e2=0.0066944; % 地球第一偏心率的平方 %姿态角初始值（ r,p,y ） zitai=(pi/180).*[0 2.0282 196.9087]; %姿态误差角 fai=(pi/180).*[1/36 1/36 5/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 坐标系下） 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)*cos(y)*sin(p) cos(y)*cos(p) sin(y)*sin(r)-cos(y)*sin(p)*cos(r) 。 1 欢迎下载 精品文档 -cos(p)*sin(r) sin(p) cos(p)*cos(r)]; %由转换矩阵 Rbl 计算四元数的初始值 Q0=[q1,q2,q3,q4] QQ=[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,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.631027 3.10094]; point_IMU=[0.147022986 0 3122.826]; T=1; %UKF 滤波的采样周期（ s ） %陀螺常值漂移初始值 tuo(e,n,u)( 单位：。 /s ） tuo=[0 0 0]; %陀螺一阶相关时间 Tt(s) Tt=[60 60 60]; %加速度计常值漂移初始值 jiasu(e,n,u)( 单位： m/s^2 ） jiasu=[0 0 0]; %加速度计一阶反相关时间 Ta(s) Ta=[60 60