function yy=ukf_IMUgps()
%function ukf_IMUgps()
% UKF在IMU/GPS组合导航系统中应用
%
% 以IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量;
% 以GPS中的位置、速度为观测量。
%
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 0.81837365 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系统的状态向量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/s) 位置误差:水平(1m),高度(5m)
u=[0.1 0.1 0.1 0.000474429 0.000474429 2 wg wtt waa]';
%IMU系统在载体坐标系下的比力值输出值fb
fb=[];
%IMU系统中陀螺输出量
wib=[]; %为载体坐标系(b)相对于惯性坐标系(i)的角速度向量
[f w]=IMU_tiqushuju; %IMU系统输出的比力值和角速度
%%%%%%%%%通过GPS观测值计算得到的姿态角
zitaijiao_gps=xlsread('D:\myfile\UKF\kalmanfilter_MATLAB\germany_ukf\原始数据\zitiajiao-gps.xls');
%%%------------------------------------------------------------
%% 循环开始 :for 1:n
outx=[];
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)]'); %初始对准,比力值加上加速度计的测量偏差
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 ,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: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) 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];
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)*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))*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+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; %小失准角
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)>0
zitai0(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);
% %z