CN101819041A - 自进化anfis与ukf结合的gps/mems-ins组合定位误差动态预测方法 - Google Patents

Abstract

自进化ANFIS与UKF结合的GPS/MEMS-INS组合定位误差动态预测方法，步骤为：(1)根据包含载体各种运动特征的大量实验数据离线进行ANFIS结构及初始前提参数的确定；(2)当GPS/MEMS-INS组合导航系统开始工作，且GPS信号完好时，此时UKF包括两种并行工作模式：预测模式及更新模式，以UKF预测模式下对应的MEMS-INS输出的东、北、天速度及GPS信号丢失时间作为ANFIS的输入，以UKF两种模式下输出的位置误差的差值作为ANFIS的期望输出，进行ANFIS模型的自进化实时更新；(3)当GPS信号丢失时，ANFIS模型及UKF均工作于预测模式，此时，利用ANFIS模型预测与UKF预测动态结合的方法来预测位置误差并校正，输出校正后的组合导航系统定位结果。本发明增强了ANFIS模型的动态性能及自适应能力，提高组合导航系统的定位性能。

Description

自进化ANFIS与UKF结合的GPS/MEMS-INS组合定位误差动态预测方法

技术领域

本发明涉及GPS/MEMS-INS(Micro Electro Mechanical System-InertialNavigation System，基于微机电系统的惯性导航系统，简称微型惯性导航系统)组合导航系统定位误差预测领域，具体涉及一种自进化ANFIS(adaptive neuro-fuzzy inferencesystem，自适应神经模糊推理系统)与UKF(Unscented Kalman Filter，无迹卡尔曼滤波)结合的GPS/MEMS-INS组合导航系统在GPS信号丢失时定位误差的动态预测方法。

背景技术

近年来，随着MEMS技术的发展，MEMS惯性传感器开始在导航定位领域获得越来越广泛的应用。其所具有体积小、重量轻、成本低的特点符合了大多数商业应用领域对导航系统的基本要求。由于MEMS-INS与GPS所具有的互补特性，GPS/MEMS-INS组合导航系统现已逐渐成为导航系统一个主要的发展方向之一。

GPS/MEMS-INS组合导航系统最常用的组合滤波算法是卡尔曼滤波器。卡尔曼滤波方法对于具有高斯分布噪声的线性系统，可以得到系统状态的递推最小均方差估计。GPS/MEMS-INS组合导航系统的状态方程是非线性的，故需采用扩展卡尔曼滤波(ExtendedKalman Filtering，EKF)。但EKF只是对非线性的系统方程进行简单的线性化，并未完全解决系统的非线性滤波问题，且非线性方程线性化会带来一定的误差，甚至造成滤波器的不稳定，将其应用于组合导航系统，最终会影响定位性能。UKF是一种基于采样点的非线性滤波算法，它直接使用非线性系统模型，不需要进行线性化近似。因此对于组合导航系统等非线性系统，UKF更为合适。

将UKF应用于GPS/MEMS-INS组合导航系统进行组合滤波，当GPS信号完好时，UKF可以有效地进行信息融合及滤波，获得精确的导航状态量的估计。但当GPS信号丢失时，组合导航系统的精度主要取决于MEMS-INS，而由于MEMS惯性传感器存在严重的非线性漂移误差，导致MEMS-INS单独导航时定位误差随时间快速积累，因此当GPS信号丢失时，GPS/MEMS-INS组合导航系统定位精度会快速下降，造成很大的定位误差。

MEMS惯性传感器的非线性漂移误差要实现对其精确建模是很困难的。在传统的组合滤波器中，一般将其建模为一定的随机过程，例如一阶马尔可夫过程或者自回归模型。然而，这些模型只能对惯性传感器的漂移进行近似的描述，特别是对于MEMS惯性传感器，由于其漂移随时间变化较迅速，模型的有效时间很短。

为解决GPS信号丢失时，GPS/MEMS-INS组合导航系统定位精度下降问题，人工智能的方法被引入GPS/MEMS-INS组合算法中。人工智能方法包括神经网络、模糊逻辑等，其可以较好地对非线性系统进行建模及预测，因此可以被用于解决MEMS惯性传感器的非线性漂移问题，对组合导航中的非线性误差进行建模及预测，从而提高GPS信号丢失时导航系统的定位精度。

多层前馈型神经网络是一种常用的神经网络，已被成功应用于GPS/MEMS-INS组合导航系统中进行误差的非线性预测，一般包括以惯导输出的位置作为神经网络输入、预测组合导航系统的精确位置的PUA模式(位置更新模式)，以及以惯导输出的位置作为神经网络输入，预测组合导航系统输出的位置误差的P-δP(位置-位置误差)模式等。但多层前馈型神经网络用于GPS信号丢失时非线性误差预测中存在训练时间较长，计算量大、实时性难于保证等问题。

基于径向基函数的神经网络RBFNN只有一个隐层，输出单元是线性求和单元，结构简单固定，不需要做太多变化，训练时间短，因此将其引入组合导航系统误差预测中，可以获得较好的实时性，但其预测效果不如多层前馈型神经网络好。

1993年美国加利福尼亚大学的Jyh-Shing Roger Jang提出了一类功能上与模糊推理系统等价的自适应网络，称为ANFIS，即自适应神经模糊推理系统。其可利用基于神经网络训练算法及最小二乘估计的混合算法确定最优参数，减少了其训练时间。但实践证明，将其单独用于GPS/MEMS-INS组合导航系统中的预测效果受到限制。2007年加拿大CALGARY大学的Walid Abdel-Hamid和Aboelmagd Noureldin及加拿大皇家军事学院的Naser El-Sheimy共同将ANFIS应用于低成本MEMS-INS/GPS组合导航系统中，与KF相结合，构成扩展ANFI-KF系统，进行位置误差的自适应模糊预测，取得了较好的效果，当GPS信号丢失30s时，ANFI-KF的定位误差比传统KF减小75％～80％。但当GPS信号丢失时间较短时(5～10s)，传统KF定位精度优于ANFI-KF。由于ANFIS不是递推算法，其采用了30s宽度的滑动窗进行模型的更新，即每采集30s的输入输出样本数据后再采用最小二乘及梯度下降算法调整参数。对于组合导航系统这类时变系统，采用30s的样本数据进行参数调整是不适当的。因为其对30s内的新旧数据采取同等的信任度，这样获得的模型参数实际上对应的是载体在30s内的平均动态特征，而没能表征载体最新的运动特征及系统的误差特性，采用此ANFIS模型来预测随后的组合导航系统定位误差是不够准确的。特别是当载体动态性较强时，模型对当前系统误差特性的反映就更不准确，从而会造成模型预测误差较大的问题。而且还存在计算量大，参数一次调整时间较长，实时性受到影响等问题。

因而，将ANFIS与UKF构成的组合滤波器用于GPS/MEMS-INS组合导航系统中，可以克服EKF在解决非线性问题时由于进行一阶线性化带来的误差。此外，采用自适应递推算法代替一般最小二乘算法实现ANFIS模型参数的自进化实时调整，使ANFIS模型与当前系统的误差特性相匹配，并保证系统的实时性；同时，利用UKF短期预测效果好与ANFIS长期预测精度高相结合，实现优势互补，动态地预测定位误差，则可以提高GPS/MEMS-INS组合导航系统位置误差的实时预测精度，进而增强组合导航系统的定位性能。

发明内容

本发明的目的在于：克服现有技术的不足，提供一种自进化ANFIS与UKF组合的GPS/MEMS-INS组合导航系统定位误差预测方法。采用渐消自适应卡尔曼滤波及梯度下降算法相结合实现ANFIS模型参数的自进化实时更新，保证了导航系统的实时性，且可解决载体动态性较强情况下预测误差大的问题，增强了模型的动态性能及自适应能力；利用UKF的短期预测及ANFIS的长期预测动态结合的方法来实时预测GPS信号丢失时GPS/MEMS-INS组合导航系统定位误差并进行补偿，从而提高组合导航系统的定位性能。

本发明的目的是通过以下技术方案来实现的：自进化ANFIS与UKF结合的GPS/MEMS-INS组合定位误差动态预测方法，步骤如下：

(1)根据包含载体各种运动特征的大量实验数据离线进行ANFIS结构及初始前提参数的确定，其中ANFIS结构确定包括每个输入变量对应的隶属函数(这里取为高斯函数)个数及最终规则个数的选择；初始前提参数包括：高斯函数的中心v_j，i和方差σ_j，i ²，i＝1，…，m；j＝1，…，n，m为每个输入变量对应的隶属函数个数，n为输入变量的个数。初始前提参数的确定采用模糊C均值聚类法(FCM)；

(2)当GPS/MEMS-INS组合导航系统开始工作，且GPS信号完好时，此时UKF包括两种并行工作模式：预测模式及更新模式，以UKF预测模式下对应的MEMS-INS输出的东、北、天速度及GPS信号丢失时间作为ANFIS的输入，以UKF两种模式下输出的位置误差的差值作为ANFIS的期望输出，进行ANFIS模型的自进化实时更新，对于每一时刻ANFIS的输入，ANFIS模型的自进化实时更新过程如下：

(2.1)当前提参数固定时，由ANFIS的结构及各层数学功能，对于k时刻的输入向量X_k(X_k＝[x_1，k，…，x_n，k]^T)，可得ANFIS总输出y_k，即位置误差为：

$y_k=\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\stackrel{\‾}{w_{i,k}}(\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{i,j,k}{x}_{k,j}+b_{i,k})$

$=A_k\·{\mathrm{\θ}}_k$

其中：

是由激励强度

和输入向量X_k构成的(n+1).M维行向量；

为k时刻各规则对应的归一化激励强度；

θ_k是由k时刻所有规则对应的结论参数a_i，j，k，b_i，k(i＝1，…，M，j＝1，…，n)所组成的(n+1).M维列向量，

(i＝1，…，M)是由k时刻的部分结论参数构成的列向量，M为规则总数，n为输入个数；

采用渐消自适应卡尔曼滤波算法进行结论参数的计算，从而获得更新后的结论参数，其状态方程为：θ_k＝θ_k-1，观测方程为：y_k＝A_k·θ_k+V_k，其中θ_k为状态量，y_k为观测量，V_k为观测噪声，

参数更新过程如下：

$S_{k+1}=\frac{1}{\mathrm{\λ}}(S_k-\frac{S_k{A}_{k+1}{A}_{k+1}^T{S}_k}{\mathrm{\λ}+A_k^T{S}_k{A}_{k+1}})$

${\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k+S_{k+1}{A}_{k+1}(y_{k+1}^T-A_{k+1}^T{\mathrm{\θ}}_k)$

S_k为(n+1)M×(n+1)M的协方差矩阵，λ为渐消因子；上式中初始参数θ₀＝0，S₀＝ηI，η为一个任意的很大的正数，I为单位矩阵；

(2.2)结论参数确定后，采用梯度下降算法进行前提参数v_j，i，σ_j，i ²(i＝1，…，m；j＝1，…，n)的调整，设k时刻的训练误差为其中y_k为k时刻的输出，y_d为k时刻的期望输出；

(2.3)利用调整后的前提参数，根据步骤(2.1)重新计算结论参数；

(2.4)计算此时的训练误差

如果E_k＞预先设定的误差目标值，则返回执行步骤(2.2)，依次往下；否则，就获得最优结论及前提参数，然后根据下一时刻的训练样本，即输入-输出数据，从步骤(2.2)开始进行下一时刻参数的学习；

(3)当GPS信号丢失时，ANFIS模型及UKF均工作于预测模式，此时，利用ANFIS模型预测与UKF预测动态结合的方法来预测位置误差并校正，输出校正后的组合导航系统定位结果。

本发明与现有技术相比的有益效果主要体现在：与现有GPS/MEMS-INS组合导航系统定位误差预测方法相比，本发明提出了一种基于ANFIS-UKF的GPS/MEMS-INS组合导航系统定位误差预测方法。利用渐消自适应卡尔曼滤波算法与梯度下降算法相结合进行参数在线调整，实现ANFIS模型的自进化实时更新，保证了导航系统的实时性，且可解决现有方法在载体动态性较强情况下预测误差大的问题，提高了ANFIS模型的动态适用性能及自适应能力；利用UKF的短期预测及ANFIS的长期预测动态结合的方法来预测GPS信号丢失时GPS/MEMS-INS组合导航系统定位误差，可克服ANFIS在GPS信号丢失的开始阶段预测精度不高的问题，且一定程度上减小了计算量，保证了系统进行预测时的实时性，最终增强了GPS信号丢失时组合导航系统的定位性能。

附图说明

图1是本发明的GPS/MEMS-INS位置误差预测方法流程图；

图2是本发明的ANFIS结构示意图；

图3是本发明的GPS信号完好时ANFIS工作于更新模式示意图；

图4是本发明的GPS信号丢失时ANFIS工作于预测模式示意图；

图5是本发明的GPS信号丢失时，ANFIS与UKF相结合进行位置误差动态预测的示意图。

具体实施方式

ANFIS结构图如图2所示，此处假设模糊推理系统有两输入：INS输出的速度V_INS及GPS信号丢失时间T_GRS，单输出为UKF处于预测状态时的位置估计误差。采用一阶Sugeno模糊模型，假设每个输入对应三个模糊集合，具有M(此处假设M为3)条模糊if-then规则的普通规则集如下：

规则i：如果V_INS∈V_i，T_GPS∈T_i，那么δP_i＝p_iV_INS+q_iT_GPS+r_i，i＝1，2…，c

这里同一层结点具有相同函数(记层1的第i个结点的输出为O_l，i)，以下分别介绍每层的数学功能：

层1在这一层的每个结点i是有一个结点函数的自适应结点。

$O_{1,i}={\mathrm{\μ}}_{P_i}\left(V_{\mathrm{INS}}\right),i=\mathrm{1,2,3}$ 或者 $O_{1,i}={\mathrm{\μ}}_{T_{i-3}}\left(T_{\mathrm{GPS}}\right),i=\mathrm{4,5,6}$

这里O_1，i是模糊集V(＝V₁，V₂，V₃)或T(＝T₁，T₂，T₃)的隶属度，它确定了给定输入满足模糊集的程度。这里模糊集V和T的隶属函数选择高斯函数：

${\mathrm{\μ}}_{P_i}\left(V_{\mathrm{INS}}\right)=\exp (-\frac{1}{2}\frac{{(V_{\mathrm{INS}}-v_{1,i})}^2}{{\mathrm{\σ}}_{1,i}^2}),(i=\mathrm{1,2,3})$

${\mathrm{\μ}}_{T_i}\left(T_{\mathrm{GPS}}\right)=\exp (-\frac{1}{2}\frac{{(T_{\mathrm{GPS}}-v_{2,i})}^2}{{\mathrm{\σ}}_{2,i}^2}),(i=\mathrm{1,2,3})$

式中{v_j，i，σ_j，i ²}(i＝1，2，3；j＝1，2)是参数集，称为前提参数。层2在这一层的每个结点是一个标以∏的固定结点，它的输出是所有输入信号的积：

$O_{2,i}={\mathrm{\μ}}_{V_i}\left(V_{\mathrm{INS}}\right){\mathrm{\μ}}_{T_i}\left(T_{\mathrm{GPS}}\right),i=\mathrm{1,2,3}$

每个结点的输出表示一条规则的激励强度。一般来说，本层结点函数可以用任意其它执行模糊“与”的T范式算子。

层3在这一层的每个结点是一个标以N的固定结点。第i个结点计算第i条规则的激励强度与所有激励强度之和的比值：

$O_{3,i}=\stackrel{\‾}{w_i}=\frac{w_i}{\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_j},i=\mathrm{1,2,3}$

本层的输出称为归一化激励强度，其中w_i＝O_2，i(i＝1，2，3)。

层4在这一层的每个结点i是一个有结点函数的自适应结点

$O_{4,i}=\stackrel{\‾}{w_i}\mathrm{\δ}{P}_i=\stackrel{\‾}{w_i}(p_i{V}_{\mathrm{INS}}+q_i{T}_{\mathrm{GPS}}+r_i),i=\mathrm{1,2,3}$

式中

(i＝1，2，3)是从层3传来的归一化激励强度，{p_i，q_i，r_i}是该结点的参数集。本层的参数称为结论参数。

层5这一层的单结点是一个标以∑的固定结点，它计算所有传来信号之和作为总输出：

基于ANFIS-UKF的GPS/MEMS-INS组合导航系统定位误差预测方法的流程图如图1所示。

其具体实施步骤如下：

1、根据包含载体各种运动特征的大量实验数据离线进行ANFIS结构及初始参数的确定。其中ANFIS结构确定包括每个输入变量对应的隶属函数个数及最终规则个数的选择。一般每个输入变量对应的隶属函数个数为2～4，规则个数最多为mⁿ，其中m为隶属函数个数，n为输入变量个数。此处输入变量个数n为4，包括MEMS-INS输出的东、北、天速度和GPS信号丢失时间；m取为2，规则个数M取为16个。

初始前提参数v_j，i，σ_j，i ²，i＝1，…，m；j＝1，…，n的确定采用模糊C均值聚类法(FCM)，其具体实施过程如下：

模糊C均值聚类法把N组输入数据向量X_k＝[x_1，k，…，x_n，k]^T(k＝1，2，…N)分为m个模糊组，并求每组的聚类中心V_i(v_1，i，…v_n，i)，使非相似性指标的价值函数达到最小。具体步骤如下：

1.1FCM采用模糊划分，使得每个给定数据用值在0，1间的隶属度来确定其属于各个组的程度，各个输入的隶属度则构成了隶属矩阵U。用值在0，1间的随机数初始化隶属矩阵U，并使其满足下式的约束条件：

$\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{u}_{j,i,k}=1,\∀k=1,\·\·\·,N,j=1\·\·\·,n$

其中u_j，i，k，i＝1，…，m；j＝1，…，n；k＝1，…，N为隶属矩阵U的某一元素。

1.2计算m组聚类中心V_i(v_1，i，…v_n，i)(即高斯函数的中心v_j，i)和高斯函数的方差σ_j，i ²：

$v_{j,i}=\frac{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c{x}_{j,k}}{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c}$

${\mathrm{\σ}}_{j,i}^2=\frac{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c{(x_{j,k}-v_{j,i})}^2}{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c},j=1,\·\·\·,n;i=1,\·\·\·,m$

其中c∈[1，∞]是一个加权指数，一般取为2。

1.3计算价值函数： $J_j=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c{\left|\right|x_{j,k}-v_{j,i}\left|\right|}^2=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c{D}_{j,i,k}^2,$

其中D_j，i，k＝||x_j，k-v_j，i||表示x_j，k和v_j，i之间的欧几里得距离。

如果它小于某个确定的阈值或它相对上次价值函数值的改变量小于某个阈值，则算法停止，获得所需的前提参数；否则继续执行步骤1.4。

1.4计算新的U阵：

再返回执行步骤1.2。

2当GPS/MEMS-INS组合导航系统开始工作，且GPS信号完好时，此时UKF包括两种并行工作模式：预测模式及更新模式。以UKF预测模式下对应的MEMS-INS输出的东、北、天速度及GPS信号丢失时间作为ANFIS的输入，以UKF两种模式下输出的位置误差的差值作为ANFIS的期望输出，进行ANFIS模型的自进化实时更新。对于每一时刻ANFIS的输入，ANFIS模型更新过程如下：

2.1当前提参数固定时，由ANFIS的结构及各层数学功能，对于k时刻的输入向量X_K(X_k＝[x_1，k，…，x_n，k]^T)，可得ANFIS总输出y_k(即位置误差)为：

$y_k=\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\stackrel{\‾}{w_{i,k}}(\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{i,j,k}{x}_{k,j}+b_{i,k})$

$=A_k\·{\mathrm{\θ}}_k$

其中：

是由激励强度

和输入向量X_k构成的(n+1).M维行向量；

(i＝1，…，M)为k时刻各规则对应的归一化激励强度；

θ_k是由k时刻所有规则对应的结论参数a_i，j，k，b_i，k(i＝1，…，M，j＝1，…，n)所组成的(n+1).M维列向量，a_i，k＝[a_{i， 1，k}，…，a_i，n，k]^T  (i＝1，…，M)是由k时刻的部分结论参数构成的列向量，M为规则总数，n为输入个数。

采用渐消自适应卡尔曼滤波算法进行结论参数的计算，从而获得更新后的结论参数。其状态方程为：θ_k＝θ_k-1，观测方程为：y_k＝A_k·θ_k+V_k，其中θ_k为状态量，y_k为观测量，V_k为观测噪声，

参数更新过程如下：

$S_{k+1}=\frac{1}{\mathrm{\λ}}(S_k-\frac{S_k{A}_{k+1}{A}_{k+1}^T{S}_k}{\mathrm{\λ}+A_k^T{S}_k{A}_{k+1}})$

${\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k+S_{k+1}{A}_{k+1}(y_{k+1}^T-A_{k+1}^T{\mathrm{\θ}}_k)$

由于组合导航系统属于时变系统，故此处采用带渐消因子的渐消自适应卡尔曼滤波算法。渐消因子λ的作用是控制旧数据的衰减，增强对新数据的信任度，在实践中一般取值为0.9到1之间(此处取0.9)，S_k为(n+1)M×(n+1)M的协方差矩阵。上式中初始参数θ₀＝0，S₀＝ηI，η为一个任意的很大的正数，I为单位矩阵。

2.2结论参数确定后，采用梯度下降算法进行前提参数v_j，i，σ_j，i ²(i＝1，…，m；j＝1，…，n)的调整。设k时刻的训练误差为

其中y_k为k时刻的输出，y_d为k时刻的期望输出。梯度下降算法的具体步骤如下：

(1)计算ANFIS第5层输出节点的误差率： $\frac{{\∂E}_k}{{\∂y}_k}=(y_k-y_d)$

(2)计算ANFIS第4层各节点的误差率：

$\frac{{\∂E}_k}{{\∂O}_{4,t}}=\frac{{\∂E}_k}{{\∂y}_k}\frac{{\∂y}_k}{{\∂O}_{4,t}}=(y_k-y_d).1=(y_k-y_d),t=1,\·\·\·,M$

其中O_4，t表示ANFIS第4层第t个节点的输出，M为规则个数。

(3)计算ANFIS第3层各节点的误差率：

$\frac{{\∂E}_k}{{\∂O}_{3,s}}=\underset{t=1}{\overset{M}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{4,t}}\frac{{\∂O}_{4,t}}{{\∂O}_{3,s}}=\frac{{\∂E}_k}{{\∂O}_{4,s}}\frac{{\∂O}_{4,s}}{{\∂O}_{3,s}}=(y_k-y_d)(a_{s,k}^T{X}_k+b_{s,k}),s=1,\·\·\·,M$

其中O_3，s表示ANFIS第3层第s个节点的输出，a_s，k ^T、b_s，k为k时刻第s个规则对应的结论参数，X_k为k时刻的输入向量。

(4)计算ANFIS第2层各节点的误差率：

$\frac{{\∂E}_k}{{\∂O}_{2,p}}=\underset{s=1}{\overset{M}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{3,s}}\frac{{\∂O}_{3,s}}{{\∂O}_{2,p}}=\underset{s=1,s\≠p}{\overset{M}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{3,s}}\frac{-w_s}{{\left(\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{w}_i\right)}^2}+\frac{{\∂E}_k}{{\∂O}_{3,p}}\frac{\underset{i=1,i\≠p}{\overset{M}{\mathrm{\Σ}}}{w}_i}{{\left(\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{w}_i\right)}^2},p=1,\·\·\·,M$

其中O_2，p表示ANFIS第2层第p个节点的输出，w_i，i＝1，…，M为ANFIS第2层第i个节点输出。

(5)计算ANFIS第1层各节点的误差率：

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1}{\overset{M}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{{\∂O}_{2,p}}{{\∂O}_{1,q}},q=1,\·\·\·,n.m$

O_1，q表示ANFIS第1层第q个节点的输出，n为输入变量的个数，m为每个输入变量对应的隶属函数个数。

其中：

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1+8(q-1)}{\overset{8+8(q-1)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}(q=\mathrm{1,2})$

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1+4(q-3)}{\overset{4+4(q-3)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}+\underset{[=9+4(q-3)}{\overset{12+4(q-3)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}(q=\mathrm{3,4})$

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1+2(q-5)}{\overset{2+2(q-5)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}+\underset{p=5+2(q-5)}{\overset{6+2(q-5)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}+\underset{p=13+2(q-5)}{\overset{14+2(q-5)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}(q=\mathrm{5,6})$

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1+(q-7),p=p+2}{\overset{M+q-8}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}(q=\mathrm{7,8})$

(6)计算

$\frac{{\∂E}_k}{{\∂v}_{j,i}}=\frac{{\∂E}_k}{{\∂O}_{\mathrm{1,2}(j-1)+i}}\frac{{\∂O}_{\mathrm{1,2}(j-1)+i}}{{\∂v}_{j,i}}=\frac{{\∂E}_k}{{\∂O}_{\mathrm{1,2}(j-1)+i}}\exp (-\frac{1}{2}\frac{{(x_j-v_{j,i})}^2}{{\mathrm{\σ}}_{j,i}^2})\frac{(x_j-v_{j,i})}{{\mathrm{\σ}}_{j,i}^2}$

$\frac{{\∂E}_k}{{\∂\mathrm{\σ}}_{j,i}}=\frac{{\∂E}_k}{\∂O_{\mathrm{1,2}(j-1)+i}}\frac{{\∂O}_{\mathrm{1,2}(i-1)+i}}{{\∂\mathrm{\σ}}_{j,i}}=\frac{{\∂E}_k}{{\∂O}_{\mathrm{1,2}(j-1)+i}}\exp (-\frac{1}{2}\frac{{(x_j-v_{j,i})}^2}{{\mathrm{\σ}}_{j,i}^2})\frac{{(x_j-v_{j,i})}^2}{{\mathrm{\σ}}_{j,i}^3},i=1,\·\·\·,m;j=1,\·\·\·,n$

(7)前提参数v_j，i、σ_j，i的调整：

$v_{j,i}=v_{j,i}-\mathrm{\η}\frac{{\∂E}_k}{{\∂v}_{j,i}}$

${\mathrm{\σ}}_{j,i}={\mathrm{\σ}}_{j,i}-\mathrm{\η}\frac{{\∂E}_k}{{\∂\mathrm{\σ}}_{j,i}},i=1,\·\·\·,m;j=1,\·\·\·,n$

其中η是一个学习系数，一般取0.01～0.8，可在实际应用中通过在一定范围内以一定的递增或递减率进行自适应调整，获取最优学习系数值。这样就实现了前提参数的一步调整。

2.3利用调整后的前提参数，根据步骤2.1重新计算结论参数；

2.4计算此时的训练误差

如果E_k＞预先设定的误差目标值(这里取10^-5)，则返回执行步骤2.2，依次往下；否则，就获得最优结论及前提参数，然后根据下一时刻的训练样本(输入-输出数据)，从步骤2.2开始进行下一时刻参数的学习。

为了保证系统的实时性，以GPS/MEMS-INS组合导航系统输出周期T的整数倍进行ANFIS模型的更新，此处以5T为一个周期更新ANFIS模型参数，这样可以避免由于ANFIS参数学习的耗时问题影响组合导航系统的实时性，减小了计算量。

3.当GPS信号丢失时，ANFIS模型及UKF均工作于预测模式，此时，利用ANFIS模型预测与UKF预测动态结合的方法来预测位置误差并校正，输出校正后的组合导航系统定位结果。具体过程如下：

3.1UKF工作于预测模式，其预测过程如下：

假设组合导航系统离散时间非线性状态方程如下式所示，

x(k+1)＝f[x(k)，w(k)]

其中f[·，·，·]是过程模型，x(k)是k时刻系统状态，在组合导航中其一般包括三维位置误差、三维速度误差和三个姿态误差角。w(k)为驱动噪声序列。

系统观测方程为：

z(k+1)＝h[x(k+1)，v(k+1)]

其中z(k+1)是观测向量，h[·，·，·]是量测方程，v(k)为量测噪声序列。w(k)和v(k)是互不相关的零均值高斯白噪声序列。

(1)计算sigma点

$X(k-1)=\left[\begin{array}{ccc}\hat{x}(k-1)& \hat{x}(k-1)+\mathrm{\γ}\sqrt{P(k-1)}& {\hat{x}}_{k-1}-\mathrm{\γ}\sqrt{P(k-1)}\end{array}\right]$

其中P(k-1)为k-1时刻状态量的协方差阵，为k-1时刻的状态估计。

(2)时间预测：

X^x(k/k-1)＝f[X(k-1)]

$\hat{x}(k.k-1)=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^m{X}_i(k/k-1)$

其中：

i＝1...2L，L为状态量维数，λ＝α²(L+κ)-L是一个标量，常量α决定了sigma点离均值

的分布情况，通常设置为一个小的正数(如1e-4≤α≤1)。常数κ是第二个标量参数，通常设置为0或3-L。

这样就可获得UKF预测的位置误差、速度误差和姿态角误差。

3.2ANFIS预测输出的计算：

对于k时刻MEMS-INS输出的速度和GPS信号丢失时间，将其作为ANFIS的输入x_j，j＝1，…，n

则ANFIS层1的输出为：

${\mathrm{\μ}}_{j,i}\left(x_j\right)=\exp (-\frac{1}{2}\frac{{(x_j-v_{j,i})}^2}{{\mathrm{\σ}}_{j,i}^2}),i=1,\·\·\·m;j=1,\·\·\·,n$

其中v_j，i、σ_j，i ²为前提参数，x_j(j＝1，…，n)为ANFIS此时刻的输入。

ANFIS层2的输出为：

w_{8(s-1)+4(t-1)+2(p-1)+q}＝μ_1，s(x₁).μ_2，t(x₂).μ_3，p(x₃).μ_4，q(x₄)(s＝1，2，t＝1，2，p＝1，2，q＝1，2)

ANFIS层3的输出为：

$\stackrel{\‾}{w_i}=\frac{w_i}{\underset{j=1}{\overset{M}{\mathrm{\Σ}}}{w}_j},i=1,\·\·\·,M$

其中M为规则个数。

则最终ANFIS的预测输出为：

$y=\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\stackrel{\‾}{w_i}(\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{i,j}{x}_j+b_i)$

其中，y为ANFIS预测的UKF工作于预测模式时的位置误差，x_j(j＝1，…，n)为此时刻MEMS-INS输出的三轴速度及GPS信号丢失时间，a_i，j，b_i为结论参数。

3.3设UKF预测的状态量中的位置误差为

此时刻ANFIS输出为y。如果此时GPS信号丢失时间小于10s，则预测的MEMS-INS输出的位置误差

如果GPS信号丢失时间大于10s，则采用附图5所示的动态方法进行位置误差预测。每隔T时刻处采用UKF预测输出的位置误差

作为MEMS-INS输出的位置误差

当到达5T的整数倍时刻处，采用ANFIS的预测输出y对此时刻UKF的预测输出

进行校正，得此时刻MEMS-INS输出的位置误差

即

3.4利用

对MEMS-INS输出的位置p_INS(k)进行校正，则获得组合导航系统该时刻的最终位置输出：

综上所述，本发明提出了一种自适应ANFIS与UKF结合的组合导航系统位置误差动态预测方法。在ANFIS模型更新阶段，引入渐消自适应卡尔曼滤波和梯度下降的混合训练算法实时地进行参数的自进化实时调整，保证了系统的实时性，且增强了模型的动态性能及自适应能力；在ANFIS预测阶段，将ANFIS的长期预测精度高和UKF短期预测精度高的特点相结合，动态地来预测位置误差，保证了短期及长期的位置误差预测精度及实时性，提高了GPS信号丢失时组合导航系统的定位精度。

本发明未详细阐述的部分属于本领域公知技术。

以上仅是本发明的具体应用范例，对本发明的保护范围不构成任何限制。其可扩展应用于所有组合导航位置误差预测的应用领域，凡采用等同变换或者等效替换而形成的技术方案，均落在本发明权利保护范围之内。

Claims (4)

1.自进化ANFIS与UKF结合的GPS/MEMS-INS组合定位误差动态预测方法，其特征在于步骤如下：

(1)根据包含载体各种运动特征的大量实验数据离线进行ANFIS结构及初始前提参数的确定，其中ANFIS结构确定包括每个输入变量对应的隶属函数个数及最终规则个数的选择，所述隶属函数为高斯函数；初始前提参数包括：高斯函数的中心v_j，i和方差σ_j，i ²，i＝1，…，m；j＝1，…，n，m为每个输入对应的隶属函数个数，n为输入变量的个数，初始前提参数的确定采用模糊C均值聚类法；

(2)当GPS/MEMS-INS组合导航系统开始工作，且GPS信号完好时，此时UKF包括两种并行工作模式：预测模式及更新模式，以UKF预测模式下对应的MEMS-INS输出的东、北、天速度及GPS信号丢失时间作为ANFIS的输入，以UKF预测及更新模式下输出的位置误差的差值作为ANFIS的期望输出，进行ANFIS模型的自进化实时更新，对于每一时刻ANFIS的输入，ANFIS模型的自进化实时更新过程如下：

(2.1)当前提参数固定时，由ANFIS的结构及各层数学功能，对于k时刻的输入向量X_k(X_k＝[x_1，k，…，x_n，k]^T)，得ANFIS总输出y_k，即位置误差为：

$y_k=\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\stackrel{\‾}{w_{i,k}}(\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{i,j,k}{x}_{k,j}+b_{i,k})$

$=A_k\·{\mathrm{\θ}}_k$

其中：是由激励强度

和输入向量X_k构成的(n+1).M维行向量；(i＝1，…，M)为k时刻各规则对应的归一化激励强度；θ_k是由k时刻所有规则对应的结论参数a_i，j，k，b_i，k(i＝1，…，M，j＝1，…，n)所组成的(n+1).M维列向量，a_i，k＝[a_i，1，k，…，a_i，n，k]^T(i＝1，…，M)是由k时刻的部分结论参数构成的列向量，M为规则总数，n为输入个数；

采用渐消自适应卡尔曼滤波算法进行结论参数的计算，从而获得更新后的结论参数，其状态方程为：θ_k＝θ_k-1，观测方程为：y_k＝A_k·θ_k+V_k，其中θ_k为状态量，y_k为观测量，V_k为观测噪声，

参数更新过程如下：

$S_{k+1}=\frac{1}{\mathrm{\λ}}(S_k-\frac{S_k{A}_{k+1}{A}_{k+1}^T{S}_k}{\mathrm{\λ}+A_{k+1}^T{S}_k{A}_{k+1}})$

${\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k+S_{k+1}{A}_{k+1}(y_{k+1}^T-A_{k+1}^T{\mathrm{\θ}}_k)$

 S_k为(n+1)M×(n+1)M的协方差矩阵，λ为渐消因子；上式中初始参数θ₀＝0，S₀＝ηI，η为一个任意的很大的正数，I为单位矩阵；

(2.2)结论参数确定后，采用梯度下降算法进行前提参数v_j，i，σ_j，i ²(i＝1，…，m；j＝1，…，n)的调整，设k时刻的训练误差为

其中y_k为k时刻的输出，y_d为k时刻的期望输出；

(2.3)利用调整后的前提参数，根据步骤(2.1)重新计算结论参数；

(2.4)计算此时的训练误差

如果E_k＞预先设定的误差目标值，则返回执行步骤(2.2)，依次往下；否则，就获得最优结论及前提参数，然后根据下一时刻的训练样本，即输入-输出数据，从步骤(2.2)开始进行下一时刻参数的学习；

(3)当GPS信号丢失时，ANFIS模型及UKF均工作于预测模式，此时，利用ANFIS模型预测与UKF预测动态结合的方法来预测位置误差并校正，输出校正后的组合导航系统定位结果。

2.根据权利要求1所述的自进化ANFIS与UKF结合的GPS/MEMS-INS组合定位误差动态预测方法，其特征在于：所述步骤(1)中模糊C均值聚类法过程如下：

模糊C均值聚类法把N组输入数据向量X_k＝[x_1，k，…，x_n，k]^T(k＝1，2，…N)分为m个模糊组，并求每组的聚类中心V_i(v_1，i，…v_n，i)，使非相似性指标的价值函数达到最小，具体步骤如下：

(1.1)FCM采用模糊划分，使得每个给定数据用值在0，1间的隶属度来确定其属于各个组的程度，各个输入的隶属度则构成了隶属矩阵U，用值在0，1间的随机数初始化隶属矩阵U，并使其满足下式的约束条件：

$\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{u}_{j,i,k}=1,\∀k=1,...,N,j=1,...,n$

其中u_j，i，k，i＝1，…，m；j＝1，…，n；k＝1，…，N为隶属矩阵U的某一元素；

(1.2)计算m组聚类中心V_i(v_1，i，…v_n，i)，即高斯函数的中心v_j，i和高斯函数的方差σ_j，i ²：

$v_{j,i}=\frac{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c{x}_{j,k}}{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c}$

${\mathrm{\σ}}_{j,i}^2=\frac{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c{(x_{j,k}-v_{j,i})}^2}{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\left(u_{j,i,k}\right)}^c},j=1,...,n;i=1,...,m$

其中：c∈[1，∞]是一个加权指数，n为每一时刻输入的个数；

(1.3)计算价值函数：

其中D_j，i，k＝||k_j，k-v_j，i‖表示x_j，k和v_j，i之间的欧几里得距离；

如果价值函数小于某个确定的阈值或它相对上次价值函数值的改变量小于某个阈值，则停止，获得所需的前提参数；否则继续执行步骤(1.4)；

(1.4)计算新的U阵：

再返回执行步骤(1.2)。

3.根据权利要求1所述的自进化ANFIS与UKF结合的GPS/MEMS-INS组合定位误差动态预测方法，其特征在于：所述步骤(2.2)中梯度下降算法的具体步骤如下：

(a)计算ANFIS第5层输出节点的误差率：

其中

为训练误差，y_k为k时刻的输出，y_d为k时刻的期望输出；

(b)计算ANFIS第4层各节点的误差率：

$\frac{{\∂E}_k}{{\∂O}_{4,t}}=\frac{{\∂E}_k}{{\∂y}_k}\frac{{\∂y}_k}{{\∂O}_{4,t}}=(y_k-y_d).1=(y_k-y_d),t=1,...,M$

其中O_4，t表示ANFIS第4层第t个节点的输出，M为规则个数；

(c)计算ANFIS第3层各节点的误差率：

$\frac{{\∂E}_k}{{\∂O}_{3,s}}=\underset{t=1}{\overset{M}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{4,t}}\frac{{\∂O}_{4,t}}{{\∂O}_{3,s}}=\frac{{\∂E}_k}{{\∂O}_{4,s}}\frac{{\∂O}_{4,s}}{{\∂O}_{3,s}}=(y_k-y_d)(a_{s,k}^T{X}_k+b_{s,k}),s=1,...,M$

其中O_3，s表示ANFIS第3层第s个节点的输出，a_s，k ^T、b_s，k为k时刻第s个规则对应的结论参数；

(d)计算ANFIS第2层各节点的误差率：

$\frac{{\∂E}_k}{{\∂O}_{2,p}}=\underset{s=1}{\overset{M}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{3,s}}\frac{{\∂O}_{3,s}}{{\∂O}_{2,p}}=\underset{s=1,s\≠p}{\overset{M}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{3,s}}\frac{{-w}_s}{{\left(\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{w}_i\right)}^2}+\frac{{\∂E}_k}{{\∂O}_{3,p}}\frac{\underset{i=1,i\≠p}{\overset{M}{\mathrm{\Σ}}}{w}_i}{{\left(\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{w}_i\right)}^2},p=1,...,M$

其中O_2，p表示ANFIS第2层第p个节点的输出，w_i，i＝1，…，M为ANFIS第2层第i个节点输出；

(e)计算ANFIS第1层各节点的误差率：

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1}{\overset{M}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{{\∂O}_{2,p}}{{\∂O}_{1,q}},q=1,...,n.m$

O_1，q表示ANFIS第1层第q个节点的输出，n为输入变量的个数，m为每个输入变量对应的隶属函数个数。

其中：

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1+8(q-1)}{\overset{8+8(q-1)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}(q=\mathrm{1,2})$

$\frac{\∂E_k}{{\∂O}_{1,q}}=\underset{p=1+4(q-3)}{\overset{4+4(q-3)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}+\underset{p=9+4(q-3)}{\overset{12+4(q-3)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}(q=\mathrm{3,4})$

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1+2(q-5)}{\overset{2+2(q-5)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}+\underset{p=5+2(q-5)}{\overset{6+2(q-5)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}+\underset{p=9+2(q-5)}{\overset{10+2(q-5)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}+\underset{p=13+2(q-5)}{\overset{14+2(q-5)}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}(q=\mathrm{5,6})$

$\frac{{\∂E}_k}{{\∂O}_{1,q}}=\underset{p=1+(q-7),p=p+2}{\overset{M+q-8}{\mathrm{\Σ}}}\frac{{\∂E}_k}{{\∂O}_{2,p}}\frac{w_p}{O_{1,q}}(q=\mathrm{7,8});$

(f)计算

和

$\frac{{\∂E}_k}{{\∂v}_{j,i}}=\frac{{\∂E}_k}{{\∂O}_{\mathrm{1,2}(j-1)+i}}\frac{{\∂O}_{\mathrm{1,2}(j-1)+i}}{{\∂v}_{j,i}}=\frac{{\∂E}_k}{{\∂O}_{\mathrm{1,2}(j-1)+i}}\exp (-\frac{1}{2}\frac{{(x_j-v_{j,i})}^2}{{\mathrm{\σ}}_{j,i}^2})\frac{(x_j-v_{j,i})}{{\mathrm{\σ}}_{j,i}^2}$

$\frac{{\∂E}_k}{{\∂\mathrm{\σ}}_{j,i}}=\frac{{\∂E}_k}{{\∂O}_{\mathrm{1,2}(j-1)+i}}\frac{{\∂O}_{\mathrm{1,2}(j-1)+i}}{{\∂\mathrm{\σ}}_{j,i}}=\frac{{\∂E}_k}{{\∂O}_{\mathrm{1,2}(j-1)+i}}\exp (-\frac{1}{2}\frac{{(x_j-v_{j,i})}^2}{{\mathrm{\σ}}_{j,i}^2})\frac{{(x_j-v_{j,i})}^2}{{\mathrm{\σ}}_{j,i}^3},i=1,...,m;j=1,...,n;$

(g)前提参数v_j，i、σ_j，i的调整：

$v_{j,i}=v_{j,i}-\mathrm{\η}\frac{{\∂E}_k}{{\∂v}_{j,i}}$

${\mathrm{\σ}}_{j,i}={\mathrm{\σ}}_{j,i}-\mathrm{\η}\frac{{\∂E}_k}{{\∂\mathrm{\σ}}_{j,i}},i=1,...,m;j=1,...,n$

其中η是一个学习系数，这样就实现了前提参数的调整。

4.根据权利要求1所述的自进化ANFIS与UKF结合的GPS/MEMS-INS组合定位误差动态预测方法，其特征在于：所述步骤(3)中当GPS信号丢失时，ANFIS模型及UKF均工作于预测模式，此时，利用ANFIS模型预测与UKF预测动态结合的方法来预测位置误差并校正，输出校正后的组合导航系统定位结果，具体过程如下：

(3.1)UKF工作于预测模式，其预测过程如下：

假设组合导航系统离散时间非线性状态方程如下式所示，

x(k+1)＝f[x(k)，w(k)]

其中f[·，·，·]是过程模型，x(k)是k时刻系统状态，在组合导航中其一般包括三维位置误差、三维速度误差和三个姿态误差角，w(k)为驱动噪声序列；

系统观测方程为：

z(k+1)＝h[x(k+1)，v(k+1)]

其中z(k+1)是观测向量，h[·，·，·]是量测方程，v(k)为量测噪声序列，w(k)和v(k)是互不相关的零均值高斯白噪声序列；

(1)计算sigma点

$X(k-1)=\left[\begin{array}{ccc}\hat{x}(k-1)& \hat{x}(k-1)+\mathrm{\γ}\sqrt{P(k-1)}& {\hat{x}}_{k-1}-\mathrm{\γ}\sqrt{P(k-1)}\end{array}\right]$

其中P(k-1)为k-1时刻状态量的协方差阵，

为k-1时刻的状态估计；

(2)时间预测：

X^x(k/k-1)＝f[X(k-1)]

$\hat{x}(k/k-1)=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^m{X}_i(k/k-1)$

其中：

i＝1...2L，L为状态量维数，λ＝α²(L+κ)-L是一个标量，常量α决定了sigma点离均值

的分布情况，常数κ是第二个标量参数，这样就可获得UKF预测的位置误差、速度误差和姿态角误差；

(3.2)ANFIS预测输出的计算：

对于k时刻MEMS-INS输出的速度和GPS信号丢失时间，将其作为ANFIS的输入x_j，j＝1，…，n

则ANFIS层1的输出为：

${\mathrm{\μ}}_{j,i}\left(x_j\right)=\exp (-\frac{1}{2}\frac{{(x_j-v_{j,i})}^2}{{\mathrm{\σ}}_{j,i}^2}),i=1,...,m;j=1,...,n$

ANFIS层2的输出为：

$w_t=\underset{i=\mathrm{1,2};j=1,...,4}{\mathrm{\Π}}{\mathrm{\μ}}_{j,i}\left(x_j\right)$

ANFIS层3的输出为：

$\stackrel{\‾}{w_i}=\frac{w_i}{\underset{j=1}{\overset{M}{\mathrm{\Σ}}}{w}_j},i=1,...,M$

则最终ANFIS的预测输出为：

$y=\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\stackrel{\‾}{w_i}(\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{i,j}{x}_j+b_i)$

其中，y为ANFIS预测的UKF工作于预测模式时的位置误差，x_j(j＝1，…，N)为此时刻MEMS-INS输出的三轴速度及GPS信号丢失时间，a_i，j，b_i为结论参数；

(3.3)设UKF预测的状态量中的位置误差为

此时刻ANFIS输出为y；如果此时GPS信号丢失时间小于10s，则预测的MEMS-INS输出的位置误差

如果GPS信号丢失时间大于10s，则采用动态方法进行位置误差预测，每隔T时刻处采用UKF预测输出的位置误差

作为MEMS-INS输出的位置误差当到达5T的整数倍时刻处，采用ANFIS的预测输出y对此时刻UKF的预测输出

进行校正，得此时刻MEMS-INS输出的位置误差即

(3.4)利用

对MEMS-INS输出的位置p_INS(k)进行校正，则获得组合导航系统该时刻的最终位置输出：

Images