[发明专利]一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法

权利要求书

1.一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法，包括：

步骤一：利用IMU惯性测量单元测量得到沿载体轴相对于惯性空间的角速率和加速度分 量信息，与INS惯性导航系统的初始位置、速度、姿态及初始姿态矩阵信息，进行捷联惯导 实时解算，得到INS位置、速度、姿态的输出值；

步骤二：根据INS的平台误差角方程、速度误差方程、位置误差方程以及陀螺、加速度 计误差模型，并根据INS与GPS的位置差、速度差，分别建立GPS/INS组合导航系统卡 尔曼滤波器的状态方程与观测方程；

步骤三：通过连续系统离散化，建立离散型卡尔曼滤波器的递推方程；

其特征在于：还包括下述步骤：

步骤四：根据GPS数据采集频率，计算GPS/INS双系统测量互差分序列；

（1）INS测量差分序列为：

ΔINS(k)=ZINS(k)-ZINS(k-1)k≤t0Z^INS(k)-ZGPS/INS(k-1)k>t0]]>

其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；t₀为惯性可信时间阈 值，根据惯导系统测量精度而确定；Z_INS(k)为k时刻捷联惯导实时解算的位置、速度； Z_GPS/INS(k-1)为k-1时刻的组合导航系统输出的位置、速度；通过如下方式得到：以 Z_GPS/INS(k-1)为k-1时刻的初值，采用k-1~k时间段内IMU测量数据，通过捷联惯导解算步骤， 进行k-1~k时间段的惯性递推运算，得到k时刻的位置、速度，即为

（2）GPS测量差分序列为：

ΔGPS(k)＝Z_GPS(k)-Z_GPS(k-1)    k＝1,2,3,...

其中，Z_GPS(k)为k时刻GPS输出的位置、速度；

（3）GPS/INS双系统测量互差分序列为C(k)：

C(k)＝ΔINS(k)-ΔGPS(k)k＝1，2,3,...；

步骤五：设定时间阈值T，若组合导航系统滤波解算时刻k大于时间阈值T则直接进行 步骤六；若组合导航系统滤波解算时刻k不大于时间阈值T，则对步骤四中得到的GPS/INS 双系统测量互差分序列进行小样本统计的可信度判别：

1）对GPS/INS双系统测量互差分序列，进行无重叠时间段的开窗统计，得到GPS/INS 双系统测量的方差值序列D(k)：

其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；N为无重叠统计窗的 窗口宽度，C(i)为GPS/INS的双系统测量互差分序列在i时刻的值；

2）对1）中得到的GPS/INS双系统测量的方差值序列D(k)，进行小样本统计的可信度 判别，该判别条件为：

max(|D(j)-1k-N+1Σi=NkD(i)|)<0.4*1k-N+1Σi=NkD(i)]]>k≥3N,j＝{k-2N,k-N,k}

其中，max为求最大值的函数；

若组合导航系统滤波解算时刻k小于或等于时间阈值T时已满足上述可信度判别条件， 则执行步骤六，且进入步骤六时的GPS测量噪声协方差估计值为：

R^k=12D(k)]]>k≤T

若组合导航系统滤波解算时刻k等于时间阈值T时仍不满足所述的可信度判别条件，则 执行步骤六，且进入步骤六时GPS测量噪声协方差估计值为：

R^k=1k-1Σi=1k[C(i)-1kΣi=1kC(i)]2/2]]>k＝T；

若组合导航系统滤波解算时刻k小于时间阈值T时不满足上述可信度判别条件，则等待 下一时刻的统计结果；

步骤六：根据GPS/INS双系统测量互差分序列，采用连续滑动

窗口法计算GPS测量噪声协方差估计值，将其代入步骤三所述的卡尔曼滤波器的递推方程， 进行自适应的卡尔曼滤波解算；

①将步骤四中的GPS/INS的双系统测量互差分序列C作为统计样本，计算该统计样本 在连续滑动窗内的均值和方差，计算公式依次为：

E(k)=1MΣi=k-M+1kC(i)σ2(k)=1M-1Σi=k-M+1k[C(i)-E(k)]2,k≥M]]>

其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；M为连续滑动窗的窗 口宽度，C(i)为GPS/INS的双系统测量互差分序列在i时刻的值，E(k)为k时刻的均值，σ(k) 为k时刻的标准差，σ²(k)为k时刻的方差；

②针对连续滑动窗内的GPS/INS的双系统测量互差分序列，进行野值点判别，该判别 条件为：

|C(i)-E(k)|＞α·σ(k)    k-M+1≤i≤k

其中，α为可调剔除因子，若C(i)满足上述野值点判别条件，则将C(i)置为均值E(k)，得 到新的统计样本，再执行步骤①，重新计算该统计样本在连续滑动窗内的均值和方差，最后 执行步骤③；否则直接执行步骤③；

③基于统计样本在连续滑动窗内的均值和方差，计算GPS测量噪声协方差的初步估值

R^GPS(k)=12σ2(k)]]>

④根据该GPS测量噪声协方差的初步估值计算GPS测量噪声协方差估计值， 计算公式为：

dk=(1-b)/(1-bk+1)R^k=(1-dk)R^k-1+dkR^GPS(k),0<b<1]]>

其中，b为遗忘因子，0＜b<1；d_k为k时刻的权系数，为k时刻的GPS测量噪声协方 差估计值；为k-1时刻的GPS测量噪声协方差估计值；

⑤将GPS测量噪声协方差估计值代入步骤三中卡尔曼滤波器的递推方程，实时调节 滤波增益矩阵K_k，进行自适应卡尔曼滤波解算：

X^k|k-1=Φk,k-1X^k-1X^k=X^k|k-1+Kk(Zk-HkX^k|k-1)Kk=Pk|k-1HTk(HkPk|k-1HTk+R^k)-1Pk|k-1=Φk,k-1Pk-1ΦTk,k-1+Γk-1Qk-1Γk-1TPk=(I-KkHk)Pk|k-1(I-KkHk)T+KkR^kKTk]]>

其中，X_k为k时刻的状态向量，即被估计向量，为状态X_k-1的卡尔曼滤波估计值， 为利用得到的对X_k的一步预测，Φ_k，k-1为k-1到k时刻的状态转移矩阵，Γ_k-1为系统 噪声矩阵，K_k为k时刻的滤波增益矩阵，P_k|k-1为一步预测均方误差矩阵，R_k为观测噪声协方 差矩阵，Q_k-1为k-1时刻系统噪声协方差矩阵，P_k为估计均方误差矩阵，H_k为k时刻的量测 矩阵，Z_k为k时刻的观测向量，I为单位矩阵。