组合导航系统的计算程序代码.DOC

组合导航系统的计算程序代码 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(