组合导航系统的计算程序代码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; %地球重力加速度（m/s^2）a=6.378137e+6; %地球长半轴e2=0.; %地球第一偏心率的平方%姿态角初始值（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]'=[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. 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 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/(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)%姿态误差角噪声wgwg=wt;%状态向量X的斜方差矩阵P = eye(length(x))*eps; % note: for stability, P should never be quite zero%速度误差：（0.1m/s) 位置误差：水平（1m),高度（5m)u=[0.1 0.1 0.1 0. 0. 2 wg wtt waa]';%IMU系统在载体坐标系下的比力值输出值fbfb=[];%IMU系统中陀螺输出量wib=[]; %为载体坐标系（b）相对于惯性坐标系（i）的角速度向量[f w]=IMU_tiqushuju; %IMU系统输出的比力值和角速度%%%%%%%%%通过GPS观测值计算得到的姿态角zitaijiao_gps=xlsread('D:\myfile\UKF\kalmanfilter_MATLAB\germany_ukf\原始数据\');%%%------------------------------------------------------------%% 循环开始：for 1:noutx=[];outp=[];detajiao=zeros(3,1);NN =1000;for i=1:NNoutx=[outx;x'];outp=[outp;diag(P)'];%大地子午圈曲率半径：RMRM=a*(1-e2)/(1-e2*(sin(x(5)))^2)^(2/3);%地球卯酉圈的曲率半径：RNRN=a/sqrt(1-e2*(sin(x(5)))^2);%当地坐标系下的比力值fl%IMU系统在载体坐标系下的比力值输出值fb转换到当地坐标系（l）下flfb=f(i,:)';%fl=Rbl*(fb+[x(13) x(14) x(15)]'); %初始对准，比力值加上加速度计的测量偏差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,及观测噪声Rzz=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 ，pitch=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); 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);%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(x(4)) sin(x(5))];wie=[0 0 we]'; %wie 为地球自转角速度向量%%%%%%%omiga12=[];for k=0:1wib=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;%四元数的更新%计算反对称矩阵omigaomiga=[0 wlb(3) -wlb(2) wlb(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];endomiga1=omiga12(1:4,1:4);omiga2=omiga12(1:4,5:8);%采用四阶龙格－库塔法数值积分解Q(K+1)=+(0.5*T.*(omiga1+omiga2)+8^(-1)*T^2.*(omiga1^2+omiga1*omiga2+omiga2^2)+48^(-1)*T^3 .*(omiga1^2*omiga2+omiga1*omiga2^2)+384^(-1)*T^4.*(omiga1^2*omiga2^2))*;%由Q(k+1)可得Rbl(k+1)Rbl=[(1)^2+(2)^2-(3)^2-(4)^2 2*((2)*(3)-(1)*(4)) 2*((2)*(4)+(1)*(3))2*((2)*(3)+(1)*(4)) (1)^2-(2)^2+(3)^2-(4)^2 2*((3)*(4)-(1)*(2))2*((2)*(4)-(1)*(3)) 2*((2)*(1)+(4)*(3)) (1)^2-(2)^2-(3)^2+(4)^2];%由Rbl(k＋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），即方位角失准角小于10’时的情况；Rbl=Cpnn*Rbl; %小失准角elseRbl=Cpn*Rbl; %大失准角endzitai0=[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)<0zitai0(3)=zitai0(3)+pi;endif Rbl(2,2)<0 & zitai0(3)>0zitai0(3)=zitai0(3)-pi;end% detajiao=zitai0-z_zitai';% detajiao(1)=0;% detajiao=Rbl*detajiao;% if abs(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);%endzitai=[zitai;zitai0'];end%将协方差转换成标准差outp=(outp).^(1/2);%将位置误差中的经度和纬度形式转换成当地坐标系（e,n,u)形式for k=1:NN%大地子午圈曲率半径：RMRM=a*(1-e2)/(1-e2*(sin(outx(k,5)))^2)^(2/3);%地球卯酉圈的曲率半径：RNRN=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)'); % 绘制图：水平运动轨迹figureplot(outx(:,4),outx(:,5),'b-');title('UKF Level of Movement(Germany-data)'); xlabel('Longitude（rad）');ylabel('Latitude（rad）');%hold on%gpsweizhi=xlsread('原始数据\');%plot(gpsweizhi(:,4),gpsweizhi(:,5),'r-');%hold off%绘制图；UKF速度误差figuresubplot(3,1,1)plot(outp(:,1),'b-');title('UKF Velocity Error(Germany-data)');ylabel('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位置误差figuresubplot(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.*outp(:,6),'b-');xlabel('t(s）');ylabel('z(cm)');%绘制图；UKF姿态角误差figuresubplot(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）');ylabel('yaw(second)');xlswrite('计算结果\output_x.xls',outx); xlswrite('计算结果\output_p.xls',outp); xlswrite('计算结果\output_',zitai);%%function [x,P] = predict(x, P,u, Q, T)%进行无迹变换P = blkdiag(P,Q);x=[x;u];% Perform unscented transform[x,P] = unscented_transform(@predict_model, [], x, P, T); % notice how the additional parameter '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%计算预报值% 由于UKF使用的是离散时间非线性模型，% 因此需要对IMU/GPS组合系统模型进行离散化处理；% 这里采用四阶Runge-kuta法以数值积分的形式实现。