1 简介
设计INS/GPS组合导航系 统时,考虑到观测量GPS位置和速度是正相关的,可通过降低单个滤波器的维度形成两个局部滤波器,主滤波器融合局部滤波器的状态估计,得到整个组合导航系 统的误差状态估计值.同时,根据各局部滤波器的故障情况选择输出,仅利用未失效系统的局部滤波器得到可靠的最优误差状态估计值,使得容错性能大大提高.结 果表明,由于采用了并行运算,增加了系统的余度,有效提高了导航系统的精度和可靠性,有较好的容错性和环境适应性,具有较高的应用价值.
2 部分代码
% GPS/INS/地磁组合导航,采用联邦滤波算法
clear
R=6378137;
omega=7292115.1467e-11;
g=9.78;
T=14.4;
time=3750;
yinzi1=0.5;
yinzi2=0.5;
%initial value
fai0=30*pi/180;
lamda0=30*pi/180;
vxe0=0.01;
vye0=0.01;
faie0=2.0/60*pi/180;
lamdae0=2.0/60*pi/180;
afae0=3.0/60*pi/180;
beitae0=3.0/60*pi/180;
gamae0=5.0/60*pi/180;
hxjz=pi/4;
vx=20*1852/3600*sin(hxjz);