1 简介
设计INS/GPS组合导航系 统时,考虑到观测量GPS位置和速度是正相关的,可通过降低单个滤波器的维度形成两个局部滤波器,主滤波器融合局部滤波器的状态估计,得到整个组合导航系 统的误差状态估计值.同时,根据各局部滤波器的故障情况选择输出,仅利用未失效系统的局部滤波器得到可靠的最优误差状态估计值,使得容错性能大大提高.结 果表明,由于采用了并行运算,增加了系统的余度,有效提高了导航系统的精度和可靠性,有较好的容错性和环境适应性,具有较高的应用价值.
2 部分代码
% GPS/INS/地磁组合导航，采用联邦滤波算法clearR=6378137;omega=7292115.1467e-11;g=9.78;T=14.4;time=3750;yinzi1=0.5;yinzi2=0.5;%initial valuefai0=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);
