[发明专利]GNSS/SINS紧组合滤波平滑的高精度快速算法

权利要求书

1.一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：将Kalman滤波的状态预报进行效率优化，在平滑过程中剔除GNSS状态参数和分解SINS状态参数，利用状态降维后的RTSS和FBC相融合的平滑算法，采用低频保存的滤波结果进行平滑处理，以SINS机械编排高频内插获取紧组合解；

具体包括以下步骤；

步骤1，采用双线程设置前后向两个独立的滤波器，将SINS观测值、GNSS观测值和前后向初始信息分别输入两个滤波器中进行初始化，双向滤波器共用同一内存中的观测数据；

步骤2，采用效率优化的Kalman滤波算法进行GNSS/SINS组合滤波；

在GNSS/SINS组合滤波过程中，两个GNSS观测更新历元间通过SINS机械编排传递，对应的误差信息采用Kalman滤波一步预测协方差矩阵传递；在机械编排历元时采用效率优化的Kalman滤波处理，使用状态转移矩阵累积和近似过程噪声计算取代预报方差传递；

步骤3，在步骤2中，按照GNSS观测更新频率，逐历元保存GNSS/SINS组合滤波结果及其中间信息，完成前后向滤波后，将其输入到改进的前向和后向RTSS平滑器，利用改进的GNSS/SINS紧组合RTSS平滑算法进行低频率的双向RTSS平滑处理，分别得到低频率的前向和后向RTSS平滑处理的结果；

在进行RTSS平滑处理时，将GNSS相关的钟差、对流层和模糊度参数剔除，以降低状态维数，避免模糊度参数过多带来的计算负担，加速RTSS计算效率；从SINS状态参数中只分解出位置、速度和姿态这三类参数，对这三类参数分别进行后续FBC平滑处理；

步骤4，将步骤3中的前向和后向RTSS平滑结果输入到改进的FBC平滑器，利用改进的GNSS/SINS紧组合FBC平滑算法以及RTSS和FBC相融合的平滑算法得到低频率的FBC平滑处理的结果；

步骤5，将SINS观测值和步骤4中的低频率FBC平滑结果输入高频内插模块，通过SINS机械编排高频内插算法得到平滑结果；

首先使用平滑结果对SINS原始数据进行系统误差改正，然后在两个低频平滑结果间进行前后向SINS机械编排，取均值后获取内插时刻高精度的紧组合导航结果。

2.根据权利要求1所述的一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：所述步骤2中效率优化的Kalman滤波算法的具体实施步骤如下：

设起始历元为k₀，下一观测更新历元为k₂₀₀，当前历元为k，具体实施为：

步骤2.1，将上一历元的导航解作为初值，使用SINS机械编排算法计算当前历元k的导航解；

步骤2.2，将步骤2.1所得机械编排导航解以及观测信息代入状态微分方程系数矩阵F中，并对F矩阵使用离散化公式得到当前历元的状态转移矩阵Φ_k,k-1；

步骤2.3，判断当前历元是否存在GNSS观测信息：

若没有GNSS观测信息，则按照式(1)计算由起始历元k₀至当前历元k的状态转移矩阵

式(1)中，为起始历元k₀至前一历元k-1的状态转移矩阵；

若有GNSS观测信息，则按照式(2)计算Kalman滤波的一步预测状态及其方差，且由于预报状态为起始历元k₀至当前历元k，所以式(2)中第一式的状态转移矩阵为过程噪声计算使用式(2)中第三式近似，其中Δt_k＝t_k-t₀，t₀为起始时刻，t_k为当前时刻；计算得到预报状态及其方差后，按照式(3)进行观测更新，得到Kalman滤波的最终解；

式(2)和式(3)如下：

其中，X_k-1为上一时刻的状态，X_k,k-1为预报状态，Φ_k,k-1为状态转移矩阵，Q_k-1为过程噪声，Δt_k为两个历元间的时间间隔，P_k-1为上一时刻的状态方差，P_k,k-1为预报方差，I为单位矩阵，F_k,k-1为状态微分方程系数矩阵，为过程噪声矩阵；

其中，z_k为观测值，H_k为观测设计矩阵，K_k为增益系数，R_k为观测噪声，X_k和P_k为滤波状态及其方差。