应用于组合导航系统的代码Word文档格式.doc
《应用于组合导航系统的代码Word文档格式.doc》由会员分享，可在线阅读，更多相关《应用于组合导航系统的代码Word文档格式.doc（18页珍藏版）》请在冰豆网上搜索。
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.631027 3.10094];
point_IMU=[0.147022986 0.81837365 3122.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（。
%加速度计常值漂移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]'
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+
升级会员 