UKF应用于GPSIMU组合导航系统的MATLAB代码.docx
《UKF应用于GPSIMU组合导航系统的MATLAB代码.docx》由会员分享，可在线阅读，更多相关《UKF应用于GPSIMU组合导航系统的MATLAB代码.docx（19页珍藏版）》请在冰豆网上搜索。
UKF应用于GPSIMU组合导航系统的MATLAB代码
组合导航系统的计算程序代码
functionyy=ukf_IMUgps（）
%functionukf_IMUgps（）
%UKF在IMU/GPS组合导航系统中应用
%
%以IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量；
%以GPS中的位置、速度为观测量。
%
%7,July2008.
clc
%Initialisestate
globalweRNRMgfldetawgTtwtdwwvuWRblTawa
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）.*[02.0282196.9087];
%姿态误差角
fai=（pi/180）.*[1/361/365/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.6310273.10094];
point_IMU=[0.1470229860.818373653122.826];
T=1;%UKF滤波的采样周期（s）
%陀螺常值漂移初始值tuo（e,n,u）（单位：
。
/s）
tuo=[000];
%陀螺一阶相关时间Tt（s）
Tt=[606060];
%加速度计常值漂移初始值jiasu（e,n,u）（单位：
m/s^2）
jiasu=[000];
%加速度计一阶反相关时间Ta（s）
Ta=[606060];
%IMU系统的状态向量X
x=[vIMUpoint_IMUfaituojiasu]';
%观测量噪声V的斜方差矩阵
R=[];
%GPS系统的量测值Z（vn,ve,vd,m,l,h）
[zRz]=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-62e-62e-6];%200μg（2e-6m/s^2）
%加速度计常值漂移误差waa（e,n,u）
waa=[2e-62e-62e-6]./4;%50μg（0.5e-6m/s^2）
%姿态误差角噪声wg
wg=wt;
%状态向量X的斜方差矩阵
P=eye（length（x））*eps;%note:
forstability,Pshouldneverbequitezero
%速度误差：
（0.1m/s）位置误差：
水平（1m）,高度（5m）
u=[0.10.10.10.0004744290.0004744292wgwttwaa]';
%IMU系统在载体坐标系下的比力值输出值fb
fb=[];
%IMU系统中陀螺输出量
wib=[];%为载体坐标系（b）相对于惯性坐标系（i）的角速度向量
[fw]=IMU_tiqushuju;%IMU系统输出的比力值和角速度
%%%%%%%%%通过GPS观测值计算得到的姿态角
zitaijiao_gps=xlsread（'D:
\myfile\UKF\kalmanfilter_MATLAB\germany_ukf\原始数据\zitiajiao-gps.xls'）;
%%%------------------------------------------------------------
%%循环开始：
for1:
n
outx=[];
outp=[];
detajiao=zeros（3,1）;
NN=1000;
fori=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（[1256],:
）-zd（[1256],:
）;
%观测值zz,及观测噪声R
zz=z（i+1,:
）';
v=Rz（i+1,:
）';
%zitai_v=[0.0010.02660.9664]';%GPS观测值姿态角的误差
zitai_v=[0.000017.0983e-0040.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=[00we]';%wie为地球自转角速度向量
%%%%%%%
omiga12=[];
fork=0:
1
wib=w（i+k,:
）'+T.*[x（10）x（11）x（12）]';%初始对准，角速度加上陀螺漂移
wiel=[0we*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=[0wlb（3）-wlb
（2）wlb
（1）
-wlb（3）0wlb
（1）wlb
（2）
wlb
（2）-wlb
（1）0wlb（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）^22*（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）^22*（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];
ifabs（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））];
ifRbl（2,2）<0&zitai0（3）<0
zitai0（3）=zitai0（3）+pi;
end
ifRbl（2,2）<0&zitai0（3）>0
zitai0（3）=zitai0（3）-pi;
end
%detajiao=zitai0-z_zitai';
%detajiao
（1）=0;
%detajiao=Rbl*detajiao;
%ifabs（detajiao
（2））%x（8）=detajiao
（2）;
%%zitai0
（2）=z_zitai
（2）;
%end
%ifabs（detajiao（3））%x（9）=detajiao（3）;
%zitai0（3）=z_zitai（3）;
%end
zitai=[zitai;zitai0'];
end
%将协方差转换成标准差
outp=（outp）.^（1/2）;
%将位置误差中的经度和纬度形式转换成当地坐标系（e,n,u）形式
fork=1:
NN
%大地子午圈曲率半径：
RM
RM=a*（1-e2）/（1-e2*（sin（outx（k,5）））^2）^（2/3）;
%地球卯酉圈的曲率半径：
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
%绘制高度图
figure
plot（outx（:
6）,'r-.'）;
title（'UKF计算的高度（germany-data）'）;
figure
plot（（180/pi）*outx（:
8）,'r-'）;
title（'UKF姿态误差角pitch（度）（germany-data）'）;
%绘制图：
水平运动轨迹
figure
plot（outx（:
4）,outx（:
5）,'b-'）;
title（'UKFLevelofMovement（Germany-data）'）;
xlabel（'Longitude（rad）'）;
ylabel（'Latitude（rad）'）;
%holdon
%gpsweizhi=xlsread（'原始数据\sudu-weizhi-gps.xls'）;
%plot（gpsweizhi（:
4）,gpsweizhi（:
5）,'r-'）;
%holdoff
%绘制图；UKF速度误差
figure
subplot（3,1,1）
plot（outp（:
1）,'b-'）;
title（'UKFVelocityError（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位置误差
figure
subplot（3,1,1）
plot（100.*outp（:
4）,'b-'）;
title（'UKFLocationError（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姿态角误差
figure
subplot（3,1,1）
plot（3600*（180/pi）*outp（:
7）,'b-'）;
title（'UKFAttitudeError（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.xls',zitai）;
%
%
function[x,P]=predict（x,P,u,Q,T）
%进行无迹变换
P=blkdiag（P,Q）;
x=[x;u];
%Performunscentedtransform
[x,P]=unscented_transform（@predict_model,[],x,P,T）;%noticehowtheadditionalparameter'T'ispassedtothemodel
%%%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）;
%
functionx=predict_model（x,T）
globalwtTtTawaRbl
%计算预报值
%由于UKF使用的是离散时间非线性模型，
%因此需要对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=[];
fork=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（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状态到K＋1状态的预估计
end
x=y;
%%%
functiondta=detaf（x,T）
globalweRNRMflgdetawgRblwtTt
%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）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（15）]';
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
升级会员 