[发明专利]基于联邦滤波器的双GPS/SINS的组合导航方法及系统

权利要求书

1.一种基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，所述方法包括以下步骤：

步骤1，基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；

步骤2，利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；

步骤3，将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；

步骤4，根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；

步骤5，将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；

步骤6，利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。

2.根据权利要求1所述的基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，步骤2所述利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量，具体过程包括：

将步骤1中捷联惯导解算出的位置信息(λ_I,L_I,h_I)和速度信息(V_xI,V_yI,V_zI)与GPS采集的定位信息(L_G,λ_G,h_G)以及速度信息(V_xG,V_yG,V_zG)相减，构造组合导航子系统的位置量测方程Z_P(t)和速度量测方程Z_V(t)：

Z_P(t)＝H_P(t)X(t)+V_P(t)

Z_v(t)＝H_v(t)X(t)+V_v(t)

式中，下标为I的量表示SINS系统的输出量，下标为G的量表示GPS的输出量，下标x,y,z分别表示导航坐标系的x方向、y方向和z方向，λ表示经度，L表示纬度，h表示高度，V表示速度，位置量测矩阵和速度量测矩阵分别是位置量测噪声V_P＝[N_N N_E N_U]^T和速度量测噪声V_v＝[M_N M_E M_U]^T均为白噪声，其中，R表示地球半径；L是SINS输出的纬度值；O表示零矩阵，其下标表示零矩阵的维度；N_N,N_E,N_U分别表示GPS接收机沿东北天方向的位置误差；M_N,M_E,M_U为GPS接收机在东北天三个方向的测速误差。