UKF应用于GPSIMU组合导航系统的MATLAB代码.docx

1、UKF应用于GPSIMU组合导航系统的MATLAB代码组合导航系统的计算程序代码 function yy=ukf_IMUgps()%function ukf_IMUgps()% UKF在IMU/GPS组合导航系统中应用% 以IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量；% 以GPS中的位置、速度为观测量。% 7,July 2008.clc% Initialise stateglobal we RN RM g fl deta wg Tt wt d ww v u W Rbl Ta wad=0; %验证循环次数%地球自转角速度we(rad/s):we=7.292115e-5;g=9.81;

2、 %地球重力加速度（m/s2）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)

3、*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,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

4、.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.81837365 3122.826;T=1; %UKF滤波的采样周期（s）%陀螺常值漂移初始值tuo(e,n,

5、u)(单位：。/s）tuo=0 0 0;%陀螺一阶相关时间Tt(s)Tt=60 60 60;%加速度计常值漂移初始值jiasu(e,n,u)(单位：m/s2）jiasu=0 0 0;%加速度计一阶反相关时间Ta(s)Ta=60 60 60;%IMU系统的状态向量Xx=vIMU point_IMU fai tuo jiasu;%观测量噪声V的斜方差矩阵R=;%GPS系统的量测值Z(vn,ve,vd,m,l,h)z Rz=gps_tiqushuju;%Vk的噪声序列方差为：RkRz=(Rz./T);%陀螺常值漂移wt(e,n,u)wt=(pi/180).*1/(3600) 1/(3600) 1/(

6、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; % 200g (2e-6 m/s2)%加速度计常值漂移误差waa(e,n,u)waa=2e-6 2e-6 2e-6./4; % 50g (0.5e-6 m/s2)%姿态误差角噪声wgwg=wt; %状态向量X的斜方差矩阵P = eye(length(x)*eps; % note: for stability, P should

7、never be quite zero%速度误差：（0.1m/s) 位置误差：水平（1m),高度（5m)u=0.1 0.1 0.1 0.000474429 0.000474429 2 wg wtt waa;%IMU系统在载体坐标系下的比力值输出值fbfb=;%IMU系统中陀螺输出量wib=; %为载体坐标系（b）相对于惯性坐标系（i）的角速度向量f w=IMU_tiqushuju; %IMU系统输出的比力值和角速度%通过GPS观测值计算得到的姿态角zitaijiao_gps=xlsread(D:myfileUKFkalmanfilter_MATLABgermany_ukf原始数据zitiaji

8、ao-gps.xls);%-% 循环开始 ：for 1:noutx=;outp=;detajiao=zeros(3,1);NN =1000;for i=1:NN outx=outx;x; outp=outp;diag(P); %大地子午圈曲率半径：RM RM=a*(1-e2)/(1-e2*(sin(x(5)2)(2/3); %地球卯酉圈的曲率半径：RN RN=a/sqrt(1-e2*(sin(x(5)2); %当地坐标系下的比力值fl %IMU系统在载体坐标系下的比力值输出值fb转换到当地坐标系（l）下fl fb=f(i,:); %fl=Rbl*(fb+x(13) x(14) x(15); %

9、初始对准，比力值加上加速度计的测量偏差 fl=Rbl*fb; %计算IMU的速度、位置输出减去GPS相应的输出值：deta(ve,vn,l,h) zd=z(i,:); deta=x(1 2 5 6,:)-zd(1 2 5 6,:); %观测值zz,及观测噪声R zz=z(i+1,:); v=Rz(i+1,:); %zitai_v=0.001 0.0266 0.9664; %GPS观测值姿态角的误差 zitai_v=0.00001 7.0983e-004 0.0006; %GPS观测值姿态角的误差 v=v;zitai_v; R=diag(v.2); %通过GPS观测值，计算得到roll=0 ，p

10、itch=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);tuo3=x(12); fl1=fl(1);fl2=fl(2);fl3=fl(3);deta1=deta(1);

11、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).2); % predict x,P = predict(x, P,u, Q, T); % update x,P = update(x, P, zz, R); % xx=x; x=xx(1:15,1); u=xx(16:30,1);

12、 %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）相对于当地坐标系（l）的转换矩阵：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) cos(x(5)*sin

13、(x(4) sin(x(5); wie=0 0 we; %wie 为地球自转角速度向量%omiga12=; for k=0:1 wib=w(i+k,:)+T.*x(10) x(11) x(12); %初始对准，角速度加上陀螺漂移 wiel=0 we*cos(x(5) we*sin(x(5); %wiel=Rel*wie; well=-x(2)/(RM+x(6) x(1)/(RN+x(6) x(1)*tan(x(5)/(RN+x(6); will=wiel+well; wlb=wib-Rbl*will; %四元数的更新 %计算反对称矩阵omiga omiga=0 wlb(3) -wlb(2) w

14、lb(1) -wlb(3) 0 wlb(1) wlb(2) wlb(2) -wlb(1) 0 wlb(3) wlb(1) wlb(2) wlb(3) 0; omiga12=omiga12,omiga; end omiga1=omiga12(1:4,1:4); omiga2=omiga12(1:4,5:8);%采用四阶龙格库塔法数值积分解Q(K+1) QQ=QQ+(0.5*T.*(omiga1+omiga2)+8(-1)*T2.*(omiga12+omiga1*omiga2+omiga22)+48(-1)*T3.*(omiga12*omiga2+omiga1*omiga22)+384(-1)*T

15、4.*(omiga12*omiga22)*QQ;%由Q(k+1)可得Rbl(k+1) Rbl=QQ(1)2+QQ(2)2-QQ(3)2-QQ(4)2 2*(QQ(2)*QQ(3)-QQ(1)*QQ(4) 2*(QQ(2)*QQ(4)+QQ(1)*QQ(3) 2*(QQ(2)*QQ(3)+QQ(1)*QQ(4) QQ(1)2-QQ(2)2+QQ(3)2-QQ(4)2 2*(QQ(3)*QQ(4)-QQ(1)*QQ(2) 2*(QQ(2)*QQ(4)-QQ(1)*QQ(3) 2*(QQ(2)*QQ(1)+QQ(4)*QQ(3) QQ(1)2-QQ(2)2-QQ(3)2+QQ(4)2;%由Rbl(k

16、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; if abs(x(9)*180*60/pi 10 %当姿态误差角（u），即方位角失准角

17、小于10时的情况； Rbl=Cpnn*Rbl; %小失准角 else Rbl=Cpn*Rbl; %大失准角 end zitai0=atan(-Rbl(3,1)/Rbl(3,3) asin(Rbl(3,2) atan(-Rbl(1,2)/Rbl(2,2); if Rbl(2,2)0 & zitai0(3)0 zitai0(3)=zitai0(3)+pi; end if Rbl(2,2)0 zitai0(3)=zitai0(3)-pi; end % detajiao=zitai0-z_zitai; % detajiao(1)=0; % detajiao=Rbl*detajiao; % if abs

18、(detajiao(2) abs(x(8) % x(8)=detajiao(2); % %zitai0(2)=z_zitai(2); % end %if abs(detajiao(3) abs(x(9) % x(9)=detajiao(3); %zitai0(3)=z_zitai(3); %end zitai=zitai;zitai0;end%将协方差转换成标准差outp=(outp).(1/2);%将位置误差中的经度和纬度形式转换成当地坐标系（e,n,u)形式for k=1:NN %大地子午圈曲率半径：RM RM=a*(1-e2)/(1-e2*(sin(outx(k,5)2)(2/3); %

19、地球卯酉圈的曲率半径：RN RN=a/sqrt(1-e2*(sin(outx(k,5)2); outp(k,4)=(RN+outx(k,6)*cos(outx(k,5)*outp(k,4); outp(k,5)=(RM+outx(k,6)*outp(k,5);end%绘制高度图figureplot(outx(:,6),r-.); title(UKF计算的高度(germany-data);figureplot(180/pi)*outx(:,8),r-); title(UKF姿态误差角pitch（度）(germany-data);% 绘制图：水平运动轨迹figure plot(outx(:,4),

20、outx(:,5),b-);title(UKF Level of Movement(Germany-data);xlabel(Longitude（rad）);ylabel(Latitude（rad）);%hold on %gpsweizhi=xlsread(原始数据sudu-weizhi-gps.xls);%plot(gpsweizhi(:,4),gpsweizhi(:,5),r-);%hold off%绘制图；UKF速度误差figure subplot(3,1,1)plot(outp(:,1),b-);title(UKF Velocity Error(Germany-data);ylabel

21、(Ve(m/s);subplot(3,1,2)plot(outp(:,2),b-);ylabel(Vn(m/s);subplot(3,1,3)plot(outp(:,3),b-);xlabel(t(s）);ylabel(Vu(m/s);%绘制图；UKF位置误差figure subplot(3,1,1)plot(100.*outp(:,4),b-);title(UKF Location Error(Germany-data);ylabel(x(cm);subplot(3,1,2)plot(100.*outp(:,5),b-);ylabel(y(cm);subplot(3,1,3)plot(100

22、.*outp(:,6),b-);xlabel(t(s）);ylabel(z(cm);%绘制图；UKF姿态角误差figure subplot(3,1,1)plot(3600*(180/pi)*outp(:,7),b-);title(UKF Attitude Error(Germany-data);ylabel(roll(second);subplot(3,1,2)plot(3600*(180/pi)*outp(:,8),b-);ylabel(pitch(second);subplot(3,1,3)plot(3600*(180/pi)*outp(:,9),b-);xlabel(t(s）);ylab

23、el(yaw(second);xlswrite(计算结果output_x.xls,outx);xlswrite(计算结果output_p.xls,outp);xlswrite(计算结果output_zitai.xls,zitai);%function x,P = predict(x, P,u, Q, T)%进行无迹变换P = blkdiag(P,Q);x=x;u;% Perform unscented transformx,P = unscented_transform(predict_model, , x, P, T); % notice how the additional paramet

24、er T is passed to the model%function y, Y = unscented_transform(func, dfunc, x,P,varargin):注意其中dfunc不知道？P(1:15,1:15)=P(1:15,1:15)+Q;%function x,P = update(x, P, z, R)x,P = unscented_update(observe_model,observe_residual, x, P, z, R);%function x = predict_model(x, T) global wt Tt Ta wa Rbl%计算预报值% 由于U

25、KF使用的是离散时间非线性模型，% 因此需要对IMU/GPS组合系统模型进行离散化处理；% 这里采用四阶Runge-kuta法以数值积分的形式实现。% Y(k+1)=Y(k)+(y1+2*y2+2*y3+y4)/6% 其中：y1=f(Y(k)*T; y2=f(Y(k)+y1/2)*T; y3=f(Y(k)+y2/2)*T; y4=f(Y(k)+y3)*T;n,N=size(x);y=;for k=1:N xx=x(:,k); %陀螺漂移常值方程xx(10)=xx(10)*exp(-1/Tt(1)+wt(1);xx(11)=xx(11)*exp(-1/Tt(2)+wt(2);xx(12)=xx(

26、12)*exp(-1/Tt(3)+wt(3); %加速度计漂移常值方程xx(13)=xx(13)*exp(-1/Ta(1)+wa(1);xx(14)=xx(14)*exp(-1/Ta(2)+wa(2);xx(15)=xx(15)*exp(-1/Ta(3)+wa(3); x1=detaf(xx,T); % T为采样周期 x2=detaf(xx+x1./2,T); x3=detaf(xx+x2./2,T); x4=detaf(xx+x3,T); xx=xx+(x1+2.*x2+2.*x3+x4)./6+xx(16:30,1);zeros(15,1); y=y,xx; %由K状态到K1状态的预估计e

27、ndx=y;%function dta=detaf(x,T)global we RN RM fl g deta wg Rbl wt Tt %IMU系统的力学编排方程zhongli=9.7803267714*(1+0.00527094*(sin(x(5)2+0.0000232718*(sin(x(5)4)-(0.3086*10(-5)*x(6);dve=(2*we*sin(x(5)+x(1)*tan(x(5)/(RN+x(6)*x(2)-(2*we*cos(x(5)+x(1)/(RN+x(6)*x(3)+fl(1)+fl(2)*x(9)-fl(3)*x(8)+Rbl(1,1) Rbl(1,2)

28、Rbl(1,3)*x(13) x(14) x(15);dvn=-(2*we*sin(x(5)+x(1)*tan(x(5)/(RN+x(6)*x(1)-x(2)*x(3)/(RM+x(6)+fl(2)+fl(1)*x(9)-fl(3)*x(7)+Rbl(2,1) Rbl(2,2) Rbl(2,3)*x(13) x(14) x(15); dvu=(2*we*cos(x(5)+x(1)/(RN+x(6)*x(1)+(x(2)2/(RM+x(6)-zhongli+fl(3)+fl(2)*x(7)-fl(1)*x(8)+Rbl(3,1) Rbl(3,2) Rbl(3,3)*x(13) x(14) x(1

29、5);dl=x(1)/(cos(x(5)*(RN+x(6);dm=x(2)/(RM+x(6);dh=x(3);%IMU系统的姿态误差方程%dfaie=(we*sin(x(5)+x(1)*tan(x(5)/(RN+x(6)*x(8)-(we*cos(x(5)+x(1)/(RN+x(6)*sin(x(9)-x(2)*(1-cos(x(9)/(RM+x(6)-deta(2)/(RM+x(6)+x(2)*deta(4)/(RM+x(6)2+Rbl(1,1) Rbl(1,2) Rbl(1,3)*x(10) x(11) x(12);%dfain=-(we*sin(x(5)+x(1)*tan(x(5)/(RN+x(6)*x(7)-x(2)*sin(x(9)/(RM+x(6)+(we*cos(x(5)+x(1)/(RN+x(6)*(1-cos(x