[发明专利]适于高维GNSS/INS深耦合的容积卡尔曼滤波方法

摘要

本发明公开了一种适于高维GNSS/INS深耦合的容积卡尔曼滤波方法，包括：S1、构造高维GNSS/INS深耦合滤波模型；S2、对构造的滤波模型采用标准容积规则产生初始化容积点；S3、采用新型容积点更新规则进行CKF滤波。本发明适于高维GNSS/INS深耦合滤波，且精度高，稳定性高。

主权项

1.一种适于高维GNSS/INS深耦合的容积卡尔曼滤波方法，其特征在于该方法包括：S1、构造高维GNSS/INS深耦合滤波模型；S2、对构造的滤波模型采用标准容积规则产生初始化容积点；S3、采用新型容积点更新规则进行CKF滤波；具体包括以及下步骤：S31、采用下列公式计算得到k时刻状态先验分布：式中，表示k时刻状态先验估计，为其均值，P_k|k‑1为其方差，表示由k‑1时刻的量测和状态推测出的k时刻的状态估计，P_k|k‑1表示的协方差，w_i＝1/2n_x，Q_k‑1为系统噪声方差阵；S32、采用下式计算得到预测过程的容积点误差矩阵并定义Ξ_k‑＝P_k|k‑1‑Q_k‐1为先验PDF逼近过程Sigma点统计线性回归的误差方差；其中，为经系统方程传播后的容积点；S33、将经系统方程传播后的容积点作为量测更新过程的容积点；S34、采用CKF量测更新计算量测值的似然分布函数；其中，式中，其中表示k时刻量测似然估计，为其均值，P_zz,k|k‑1为其方差，为由k‑1时刻的状态预测到的k时刻的量测，h(x)为量测方程，w_i＝1/2n_x，R_k为量测噪声方差阵；S35、计算状态量x的后验分布函数；其中，式中，为k时刻的状态量的后验估计，其均值和方差分别为和P_k|k，K_k＝P_xz,k|k‑1(P_zz,k|k‑1)^‐1为卡尔曼增益矩阵，P_xz,k|k‑1为状态量的后验估计值与量测似然估计值的互协方差；S36、定义Sigma点逼近后验分布产生的误差为为CKF容积点SLR的权值，k时刻的先验分布的SLR应精确捕获状态的均值和协方差，且考虑系统不确定性和噪声的影响，则有式中，为预测过程容积点误差阵，Σ＝diag(w)表示采用w对角线元素构造该矩阵Σ，类似的后验分布的SLR中，容积点至少能精确匹配到状态的均值和方差即式中，为更新后的容积点；S37、设和均为对称正定矩阵，且存在则有其中B为待求解的转换矩阵，为更新后的容积点误差阵；进而有B＝L_k+1Γ(L_k)^‑1，其中Γ为任意正交矩阵满足取Γ为单位阵时则得到B＝L_k+1(L_k)^‑1；S38、基于后验的状态估计均值和更新后的容积点误差阵得到更新的容积点为0≤i≤2n_x。