[发明专利]一种GNSS与惯导组合导航位置输出的滤波校正方法

权利要求书

1.一种GNSS与惯导组合导航位置输出的滤波校正方法，其特征在于，包括以下步骤：

首先，由INS输出导航参数，GNSS系统输出列车位置，将INS与GNSS系统输出位置之差作为观测量，进行卡尔曼滤波估计，根据估计结果对INS输出的位置进行反馈校正；

其次，将反馈校正后的INS输出的位置与GNSS输出的位置再次求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对反馈校正后的INS输出的位置再次进行输出校正；

最后获得的INS位置为系统位置的最终结果；

具体包括以下步骤：

S0：建立GNSS/INS组合导航系统的误差状态方程

上式中X(t)的初始值为0，W(t)为系统噪声矩阵，G(t)为噪声驱动矩阵，F(t)为状态转移矩阵；

S1：列车启动时，GNSS/INS组合导航系统开始工作，并向滤波系统输入初始的列车位置信息、 系统估计协方差阵P₀、系统噪声方差阵Q₀、 量测信息的方差阵R₀以及惯导系统的导航参数；

S2：根据S1中的信息，构建卡尔曼滤波估计所需的矩阵，包括离散状态方程X_k＝A_k,k-1X_k-1+B_k,k-1W_k-1中的A_k,k-1＝I+F_k,k-1T_s，一步预测误差方差阵滤波增益矩阵：估计协方差阵P_k＝[I-K_kH_k]×P_k,k-1；

S3：利用t_k时刻INS与GNSS系统输出的位置计算t_k时刻的量测值利用S2中的滤波增益矩阵K_k，根据公式计算卡尔曼滤波反馈校正的估计结果，根据估计结果对INS的位置进行校正：获得经过反馈校正后的INS系统位置；

S4：根据GNSS位置以及校正后的INS位置获得量测值根据公式计算卡尔曼滤波输出校正的估计结果；

利用估计结果对反馈校正后的INS位置进行校正：最终得到t_k时刻卡尔曼滤波估计校正后的系统位置；

其中，状态转移矩阵为：

F(1,5)＝1/(R_M+h)，F(2,1)＝-(ω_iesinL+V_E/(R_N+h)×tgL)，F(2,3)＝-V_N/(R_M+h)，F(2,4)＝1/(R_N+h)，F(3,1)＝ω_iecosL+V_E/(R_N+h)，F(2,7)＝-ω_iesinL，F(3,2)＝V_N/(R_M+h)，F(3,7)＝ω_iecosL+V_E/(R_N+h)×sec²L，F(3,4)＝tgL/(R_N+h)，F(4,4)＝(V_N/(R_M+h)×tgL-V_U/(R_M+h))，F(4,2)＝-f_U，F(4,3)＝f_U，F(4,5)＝2ω_iesinL+V_E/(R_N+h)×tgL，F(4,6)＝-(2ω_iecosL+V_E/(R_N+h))，F(5,1)＝f_U，F(5,3)＝-f_U，F(4,7)＝2ω_iecosLV_N-V_EV_N/sec²L+2ω_iesinLV_U，F(5,5)＝-V_U/(R_M+h)，F(5,6)＝-V_N/(R_M+h)，F(5,4)＝-2(ω_iesinL+V_E/(R_N+h)×tgL)，F(6,1)＝-f_N，F(6,2)＝-f_E，F(6,4)＝-2(ω_iecosL-V_E/(R_E+h))，F(6,5)＝-2V_N/(R_M+h)，F(6,7)＝-2V_Eω_iesinL，F(7,5)＝1/(R_M+h)，F(8,4)＝secL/(R_N+L)，F(9,6)＝1，其中ω_ie为地球自转角速度，[f_E f_N f_U]为东北天方向的比力，R_M＝R_e(1-2f+3f sin²L)，R_N＝R_e(1+f sin²L)，f＝1/298.257，R_e＝6378137m，[V_E V_N V_U]为系统在东北天方向的速度，其中q＝[q(1)q(2) q(3) q(4)]为姿态四元数，F_M＝diag{0 0 0 -1/Tg(1) -1/Tg(2) -1/Tg(3) -1/Ta(1) -1/Ta(2) -1/Ta(3)}，其中Tg＝300×ones(3,1)为陀螺仪误差漂移相关时间，Ta＝1000×ones(3,1)为加速度计误差漂移相关时间。