UKF-GPS-IMU-MATLAB.docVIP

组合导航系统的计算程序代码 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) -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 03122.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 60]; %IMU系统的状态向量X x=[vIMU point_IMU fai tuo jiasu]; %观测量噪声V的斜方差矩阵 R=[]; %GPS系统的量测值Z(vn,ve,vd,m,l,h) [z Rz]=gps_tiqushuju; %Vk的噪声序列方差为：Rk Rz=(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) %加速度计常值漂移wa(e,n,u) wa=[2e-6 2e-6 2e-6]; % 200μg (2e-6 m/s^2) %加速度计常值漂移误差waa(e,n,u) waa=[2e-6 2e-6 2e-6]./4; % 50μg (0.5e-6 m/s^2) %姿态误差角噪声wg wg=wt; %状态向量X的斜方差矩阵 P = eye(length(x))*eps; % note: for stability, P should never be quite zero %速度误差：（0.1m/