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

权利要求书

1.一种GNSS和INS组合导航观测量引入方法，其特征在于，包括如下步骤：

步骤S1：建立组合导航方程：

式中，X为状态变量，各元素依次为东向、北向、天向3个失准角误差，东向、北向、天向3个速度误差，纬度、经度、高度3个位置误差，F为状态转移矩阵，W为系统噪声，Z为系统观测量，H为观测矩阵，V为观测噪声，表示X的微分；

步骤S2：将组合方程离散化：

式中：X_k-1、X_k分别表示k-1时刻、k时刻的状态变量数值，Z_k为k时刻的系统观测量数值，Φ为状态转移矩阵、Q为系统噪声矩阵，状态转移矩阵Φ、系统噪声矩阵Q分别由F、W离散化得到；

观测噪声方差阵R取值方法为：卫星导航信息有效时观测噪声方差阵R取R1，无效时R取R2；

R1＝diag[Δp² Δp² Δp² Δv² Δv² Δv²]，

R2＝diag[∞ ∞ ∞ ∞ ∞ ∞]，

式中：Δp、Δv分别表示卫星导航单向位置、速度均方差；∞表示正无穷；

步骤S3：建立Kalman滤波方程：

P(-)＝ΦP_k-1Φ^T+Q，

K＝P(-)H^T[H P_k-1H^T+R]^-1，

X_k＝ΦX_k-1+K[Z_k-HΦX_k-1]，

P_k＝[I-KH]P(-)[I-KH]^T+KRK^T，

式中：P阵为状态误差方差阵，I为单位矩阵，P_k-1、P_k分别表示k-1时刻、k时刻的P阵数值、P(-)为中间变量，K为增益矩阵，Φ^T为矩阵Φ的转置，A^-1表示矩阵A的逆矩阵，

R值影响K的取值：当卫星导航用时R取R1，将观测量对于状态量估计的影响引入；当卫星导航失锁时R取R2，将观测量对于状态量估计的影响屏蔽；

通过Kalman滤波，估计得到状态变量X的结果。