[发明专利]一种基于级联自适应鲁棒联邦滤波的车载导航计算方法

权利要求书

1.一种基于级联自适应鲁棒联邦滤波的车载导航计算方法，其特征在于，具体的实现步骤为：

步骤1.建立SINS的导航解算方程以及SINS、GPS和Odometer的误差模型；

步骤2.以残差作为统计量构建三段式的降权函数，利用降权因子对含有量测异常值的量测噪声进行缩减；

步骤2.1：根据观测值与预测观测值的差异，获得系统的残差；

v_i＝Z_k-H_kX_k,k-1

其中，v_i为观测量对应的残差向量；Z_k表示系统的m维观测向量，下角标k代表时刻；X_k,k-1为时刻介于k与k-1之间时系统的n维状态向量；

步骤2.2：建立基于残差统计量的三段式降权函数；

其中，为v_i的均方差，实际情况下，为v_i的权倒数；方差因子k₀的取值范围为1.5～2.5；k_i的取值范围为3.0～5.0；

步骤2.3：根据降权函数对含有异常值误差的量测噪声进行缩减；

其中，P_i为原观测值；

步骤3.通过级联方式，在量测噪声R达到稳定的基础上，基于Sage-Husa自适应滤波器构建系统噪声估计器实时在线估计先验信息Q，获得子系统的初步状态最优估计值；

步骤3.1：当R_k经过降权得到后，获得k时刻的滤波增益K_k；

步骤3.1.1：计算k时刻的状态一步预测

步骤3.1.2：计算状态一步预测的误差协方差矩阵P_k,k-1；

其中，是由时变噪声统计估值器获得的；

步骤3.1.3：计算k时刻的滤波增益K_k：

步骤3.2：获得k时刻的状态最优估计与最优状态误差协方差矩阵P_k；

P_k＝[I-K_kH_k]P_k,k-1

其中，与由以下时变噪声统计估值器获得：

其中，d_k-2＝(1-b)/(1-b^k-2)；b为遗忘因子，0.95＜b＜0.99；

步骤4.对两个导航子系统的最优估计值进行全局融合，获得最终的最优估计值，然后进行反馈过程，重复步骤1至步骤4；

所述的对两个导航子系统的最优估计值进行全局融合，获得最终的最优估计值的具体步骤为：

步骤4.1：初始化协方差矩阵P_i,0和系统噪声协方差阵Q_i,0；

P_i,0＝β_i^-1P_g,0,Q_i,0＝β_i^-1Q_g,0

其中，β_i为子系统的信息分配系数；Q_g,0取粗略值；

步骤4.2：在各子滤波器之间独立进行时间更新；

步骤4.3：各子滤波器接收子系统量测信息并独立进行量测更新；

P_i,k＝(I-K_i,kH_i,k)P_i,k/k-1

步骤4.4：信息分配和全局最优化估计；

P_i＝β_i^-1P_g

步骤4.5：把各子滤波器的局部最优估计信息进行融合，从而得到全局最优估计；