基于自适应鲁棒无迹卡尔曼滤波的重力匹配方法与流程

本发明属于导航、制导与控制技术领域，具体涉及一种基于自适应鲁棒无迹卡尔曼滤波的重力匹配方法。

背景技术：

海洋中蕴含着丰富的资源，可以为我国的资源安全问题提供战略储备，因此我国越来越重视对海洋资源的开发。而无论是海洋资源开发还是军事上的综合制海都需要水下潜器技术，因此对水下潜器导航技术的研究具有十分重要的战略意义。水下导航与定位通常采用的是惯性导航，但惯导的位置误差会随时间累积，其定位精度无法满足长航时的需求，因此需要利用其它导航技术对惯导进行辅助。重力场特征信息具有良好的时空分布特性，利用重力进行辅助导航能在不破坏水下潜器隐蔽性的前提下有效提升导航精度，因此对重力辅助惯性导航系统的研究十分有意义。重力辅助惯性导航系统由惯导系统、重力异常背景图、重力测量仪器和重力匹配算法构成。其中重力匹配算法是重力辅助惯导系统的核心技术。

重力匹配算法按照采样方式可以分为两类，单点匹配算法和序列匹配算法。但序列匹配算法需要在匹配之前获得一定数量的采样点，因此算法的实时性较差，最终获得的匹配位置会存在一定延时。相对来说单点匹配算法就拥有较好的实时性。单点匹配算法以桑迪亚地形辅助(sitan)算法为核心代表，该算法利用ekf滤波器对位置误差、速度误差甚至姿态误差进行修正，具有较好的实时性，但sitan算法的抗干扰能力较差，ekf算法在对系统方程和观测方程进行线性化的过程中会引入较大线性化误差，因此传统sitan算法的应用受到了限制。利用ukf算法进行匹配，充分考虑了系统状态在采样点附近的概率分布特性且不引入线性化误差，针对强非线性系统，其精度要高于传统sitan算法。但由于水下环境较为复杂，存在各种干扰，ukf算法暴露出一定的缺点，即在系统过程噪声不确定性较强和观测噪声被污染的情况下会出现滤波效果下降甚至滤波发散的问题。

技术实现要素：

有鉴于此，本发明提供了一种基于自适应鲁棒无迹卡尔曼滤波(arukf)的重力匹配方法，在系统过程噪声不确定性较强和观测噪声被污染的情况下能有效降低匹配误差，取得比ukf算法更高的匹配精度。

实现本发明的技术方案如下：

一种基于自适应鲁棒无迹卡尔曼滤波的重力匹配方法，包括如下步骤：

步骤一，基于递推滤波的单点匹配算法，建立重力辅助惯性导航系统滤波器的系统状态方程和观测方程；

步骤二，将当前时刻重力异常图中提取的重力异常参考值与重力异常实测值作差，得到重力异常差值；

步骤三，基于改进的arukf算法，利用所述重力辅助惯性导航系统滤波器的系统状态方程和观测方程，对所述重力异常差值进行滤波，得到下一时刻潜器的位置和方差；

所述改进的arukf算法为：在观测部分，加入鲁棒huber函数ψ(εk)，在预测协方差矩阵中，引入自适应因子αk；

步骤四，利用所述下一时刻潜器的位置和方差，按照步骤二～三的方式，计算出每一滤波采样时刻的潜器位置，最终获得一条完整的匹配轨迹。

进一步地，本发明所述鲁棒huber函数ψ(εk)为：

其中，εk为代价函数，zi,k为sigma点观测值，为一步预测观测值，rk为量测噪声，τ为设定的参数，，sgn()表示符号函数。

进一步地，本发明所述自适应因子αk为：

其中，α0表示自适应因子。

进一步地，本发明所建立重力辅助惯性导航系统滤波器系统的状态方程，如式(1)所示，

其中，为系统状态，和δλ表示的是水下潜器的纬度和经度误差，δvn和δve代表的是水下潜器在北向和东向的速度误差，f为系统转移矩阵，w为系统过程噪声；

系统的观测方程为：

其中，是从重力异常背景图中提取出的重力异常值，和λins为惯导解算的水下潜器的经纬度，和δλk为滤波器在k时刻估计的水下潜器的位置，为实测重力异常值。

进一步地，本发明所述步骤二的具体过程为：将滤波器估计的潜器的位置误差和ins解算的潜器位置作差，利用得到的差值在重力异常图中进行搜索，提取出相应的重力参考值，同时对当前时刻的重力异常值进行测量，将重力异常参考值与实测值作差，得到重力异常差值。

有益效果：

(1)本发明采用基于递推滤波的单点匹配算法，与序列匹配算法相比，很大程度上提高了算法的实时性。

(2)本发明在系统一步预测协方差矩阵中引入自适应因子，在观测一步预测误差较大的时候，自适应因子可以自适应地增大自身的值，从而使状态预测协方差增大，减小状态预测的可信度，使系统更加相信观测值，这样就减小了系统过程噪声不确定性所产生的影响，增强了系统的鲁棒性。

(3)本发明在观测部分引入了鲁棒函数来提升观测部分的鲁棒性，引入的huber鲁棒函数能够在观测噪声方差较大的时候进一步增大噪声方差的权值，从而使观测协方差增大，减小了观测部分的可信度，使系统更加相信一步预测，增强了系统观测部分的鲁棒性，使系统在观测噪声被污染的情况下仍能保持较高的匹配精度。

附图说明

图1为重力辅助惯性导航系统的系统框图；

图2为基于自适应鲁棒无迹卡尔曼滤波的重力匹配方法的流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细阐述。

本发明提供了一种于自适应鲁棒无迹卡尔曼滤波(arukf)的重力匹配方法，通过引入自适应因子和鲁棒huber函数来增强系统的鲁棒性，从而使系统在系统过程噪声不确定性较强和观测噪声被污染的情况下仍能得到较高的匹配精度，系统框图如图1所示。

具体流程图如图2所示，包括以下步骤：

步骤1，采用基于递推滤波的单点匹配算法，根据重力辅助惯性导航系统的特点，将惯导误差模型作为滤波器的状态模型，取为系统状态，其中和δλ表示水下潜器的纬度和经度误差，δvn和δve代表水下潜器在北向和东向的速度误差，建立位置和速度误差方程：

其中，rm是地球的子午面曲率半径，rn为地球的卯酉面曲率半径，ωie为地球自转角速度，φn和φe分别为北向和东向姿态角误差，δae和δan分别为东向和北向的加速度计零偏，和λ表示水下潜器的纬度和经度，vn和ve表示水下潜器在北向和东向的速度；

由此可以建立滤波器的系统状态方程：

其中

其中，w为系统过程噪声，由白噪声和速度计零偏及姿态误差引起的干扰组成。

滤波器的观测模型是建立在重力异常背景图基础上的，观测方程可以表示为：

其中是从重力异常背景图中提取出的重力异常值参考值，和λins为水下潜器上惯导解算的经纬度信息，和δλk为滤波器在k时刻的位置误差，为实测重力异常值实测值。

步骤2，将滤波估计的水下潜器的位置误差和ins解算的水下潜器的位置作差利用差值结果在重力异常图中进行搜索，提取出相应的重力异常参考值同时对当前时刻的重力异常值进行测量，得到重力异常实测值并将重力异常参考值与重力异常实测值作差，得到重力异常差值。

步骤3，该步骤提出一种改进的arukf算法，所述改进为：

在现有arukf算法的量测协方差更新过程中，加入鲁棒huber函数ψ(εk)

在预测协方差矩阵中，引入自适应因子αk

运用本发明提出的arukf算法，利用步骤1建立的重力辅助惯性导航系统滤波器的系统状态方程和观测方程，对步骤2得到的重力异常差值进行滤波，得到下一时刻水下潜器的位置和方差；

具体步骤如下：

(1)状态参数初始化：

其中，x0为初始位置误差和初始速度误差，为初始状态，p0为估计误差方差矩阵初始值。

(2)ut变换：

计算2n+1个sigma点的散布程度

式中，n为系统维度，λ＝α²(n+κ)-n，系数α决定sigma点的散布程度，取值范围为0.0001～1；系数β用来描述x的分布信息，高斯噪声情况下，β最优值为2；系数κ为待选参数，一般选为(3-n)。表示矩阵平方根的第i列；为求取一阶统计特性时的权系数；为求取二阶统计特性时的权系数。

(3)时间更新：

χi,k|k-1＝f(χi,k-1)(11)

式中，χi,k|k-1为sigma点一步预测值，为状态一步预测值。

(4)量测协方差更新：

zi,k＝h(χi,k|k-1)(13)

其中，zi,k为sigma点观测值，为一步预测观测值，εk为代价函数，ψ(εk)为鲁棒代价函数，为经鲁棒处理后的量测噪声，rk为量测噪声，τ为设定的参数，一般设置为1.345，sgn()符号函数。

(5)自适应因子更新：

其中，vk为新息，zk为观测值，γ为遗忘因子，通常设置为0.95，α0为自适应因子。

(6)状态和观测更新

其中，pk,k-1为状态一步预测方差矩阵，为观测方差矩阵，为状态观测协方差矩阵，qk为系统过程噪声方差，kk为卡尔曼增益矩阵，pk为估计误差方差矩阵。

步骤4，采用由arukf滤波得到的下一时刻水下潜器位置和方差，重复步骤2、步骤3直至滤波结束，能够得到每个滤波采样点的水下潜器的位置，最终可以获得一条完整的匹配轨迹。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。