[发明专利]一种GNSS和INS组合导航观测量引入方法

摘要

本发明公开了一种GNSS/INS组合导航观测量引入方法。本发明通过调节Kalman滤波器观测误差方差阵R，从而实现观测量引入，能够进行不同GNSS精度下的GNSS/INS组合导航解算。

主权项

1.一种GNSS和INS组合导航观测量引入方法，其特征在于，包括如下步骤：步骤S1：建立组合导航方程：式中，X为状态变量，各元素依次为东向、北向、天向3个失准角误差，东向、北向、天向3个速度误差，纬度、经度、高度3个位置误差，F为状态转移矩阵，W为系统噪声，Z为系统观测量，H为观测矩阵，V为观测噪声，表示X的微分；步骤S2：将组合方程离散化：式中：Xk‑1、Xk分别表示k‑1时刻、k时刻的状态变量数值，Zk为k时刻的系统观测量数值，Φ为状态转移矩阵、Q为系统噪声矩阵，状态转移矩阵Φ、系统噪声矩阵Q分别由F、W离散化得到；观测噪声方差阵R取值方法为：卫星导航信息有效时观测噪声方差阵R取R1，无效时R取R2；R1＝diag[Δp2 Δp2 Δp2 Δv2 Δv2 Δv2]，R2＝diag[∞ ∞ ∞ ∞ ∞ ∞]，式中：Δp、Δv分别表示卫星导航单向位置、速度均方差；∞表示正无穷；步骤S3：建立Kalman滤波方程：P(‑)＝ΦPk‑1ΦT+Q，K＝P(‑)HT[H Pk‑1HT+R]‑1，Xk＝ΦXk‑1+K[Zk‑HΦXk‑1]，Pk＝[I‑KH]P(‑)[I‑KH]T+KRKT，式中：P阵为状态误差方差阵，I为单位矩阵，Pk‑1、Pk分别表示k‑1时刻、k时刻的P阵数值、P(‑)为中间变量，K为增益矩阵，ΦT为矩阵Φ的转置，A‑1表示矩阵A的逆矩阵，R值影响K的取值：当卫星导航用时R取R1，将观测量对于状态量估计的影响引入；当卫星导航失锁时R取R2，将观测量对于状态量估计的影响屏蔽；通过Kalman滤波，估计得到状态变量X的结果。