[发明专利]一种基于快速ISS集员滤波的MIMU/GPS组合导航方法

权利要求书

1.一种基于快速ISS集员滤波的MIMU/GPS组合导航方法，其特征在于，包括以下步骤：

步骤1：获取GPS卫星系统的位置、速度数据以及MIMU采集的陀螺仪和加速度数据，并利用MIMU采集的数据解算位置、速度、姿态信息；

步骤2：建立MIMU/GPS组合导航系统的离散化状态方程和量测方程：

其中，和分别为状态向量和观测向量，且状态向量为15维误差向量；观测向量为GPS位置、误差数据与MIMU解算位置、误差结果的差值；F_k为状态转移矩阵；Γ_k-1为系统噪声分配矩阵；H_k为量测阵；和分别为系统噪声和量测噪声，其中，系统噪声元素为各轴的陀螺仪白噪声和加速度计白噪声，量测噪声元素为GPS沿各轴向的距离、速度测量误差，这两种噪声属于如下的椭球集合：

其中，Q_k和R_k为已知的正定矩阵，表示噪声椭球集合的形状；

步骤3：采用输入状态稳定的集员滤波ISMF算法，更新椭球；

步骤4：利用基于松弛优化目标的FISMF算法进行参数的求解和状态估计，得到组合导航的误差估计值；

步骤3所述的椭球更新分为状态更新阶段和量测更新阶段，不同阶段的椭球更新公式为：

步骤31：在状态更新阶段，更新椭球其计算公式为：

σ_k|k-1＝σ_k-1 (8)，

其中，σ_k-1作为系统李雅普诺夫函数的上界，与P_k-1共同决定了k-1时刻状态椭球的形状，p_k∈(0,+∞)可以用来优化椭球ε_k|k-1的大小；

步骤32：在量测更新阶段，更新椭球其计算公式为：

P_k＝(I_n-K_kH_k)P_k|k-1 (9)，

其中，滤波增益残差

步骤4的具体操作步骤包括：

步骤41：将式(11)转换为：

其中，而则由Cholesky分解得到，即

由式(13)可以得出，σ_k的上界为：

其中，为的最大特征值；

步骤42：以为优化目标，通过式(14)得到参数q_k的最优值的计算公式为：

步骤43：初始化P₀,σ₀，设定k←1；

步骤44：根据式(6)，(7)和(8)来计算时间更新椭球

步骤45：如果取P_k＝P_k|k-1，σ_k＝σ_k|k-1；否则,根据(9)，(10)和(11)计算P_k，σ_k，参数q_k根据式(15)计算；

步骤46：令k←k+1并返回步骤44，直到程序终止。