WO2020062807A1

WO2020062807A1 - 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法

Info

Publication number: WO2020062807A1
Application number: PCT/CN2019/079136
Authority: WO
Inventors: 徐晓苏; 杨阳; 袁杰; 翁铖铖; 梁紫依; 刘兴华
Original assignee: 东南大学
Priority date: 2018-09-30
Filing date: 2019-03-21
Publication date: 2020-04-02
Also published as: CN109141436A

Abstract

一种改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，包括：对需要用到的坐标系进行定义；建立水下潜航阶段SINS/DVL子***状态方程；建立水下潜航阶段SINS/DVL子***量测方程；建立水面位置修正阶段SINS/GPS子***的状态方程；建立水面位置修正阶段SINS/GPS子***的量测方程；建立水下潜航阶段的非线性滤波方程，在水下潜航一段时间以后，AUV浮出水面，建立水面位置修正阶段非线性滤波方程，进行改进的无迹卡尔曼滤波解算，完成时间更新、量测更新和滤波更新，完成定时位置信息校正。该方法能够提高水下组合导航***的滤波解算效率及导航精度，降低了滤波算法计算的复杂程度，保证实时性和稳定性。

Description

改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法

技术领域

本发明属于惯性导航领域，涉及一种改进的无迹卡尔曼滤波算法在水下组合导航SINS/DVL/GPS中的应用方法。

背景技术

水下自主航行器(Autonomous Underwater Vehicle，AUV)无论在民用还是军事方面都有广泛的应用。军事方面如战区侦察、探测扫除水雷、潜艇对抗、海上预警封锁航线或港口、攻击敌舰船或潜艇、破坏石油设施及通讯网络、水下中继通讯等；民用方面如海洋资源勘察与开发、海洋救险和打捞等。就目前的发展而言，导航问题仍然是AUV所面临的主要技术挑战之一。导航***必须提供长航时的精确姿态、航向、速度和位置信息，而且精确的导航能力是AUV有效应用和安全回收的一个关键技术。但由于受其大小、重量、电源使用的限制及水介质的特殊性、隐蔽性等因素的影响，实现AUV的精确导航是一项艰难的任务。

远航程AUV因为其远航程的特点，要求长时间、长距离的在水下航行，这就要求导航必需满足一定的精度。惯性导航是一种自主导航，不需要与外界的联系，隐蔽性好，是AUV理想的导航方式。但是，惯导有误差积累的问题，短时间内惯导可以保持很好的导航精度，但是随着时间的延长，误差积累效应非常明显，无法保证导航精度。因此，通常都是采用其它的辅助导航***配合以修正惯导的累积误差。如多普勒测速，罗兰C等方法。GPS由于具有导航精度高，没有误差积累的特点，是高精度导航定位的首选方法。但是对于水下航行器而言，需要浮出水面才能接收GPS信号，而且为了隐蔽不能长时间漂浮在水面，因此GPS只能作为校正***使用。此外，由于AUV组合导航***在本质上具有非线性特性，传统上采用扩展卡尔曼滤波(EKF)对非线性模型进行线性化处理，在此过程中由于忽略了模型的高阶导数，随着时间的增长，导航精度难以得到保证，而且在计算过程中反复计算***方程的雅各比矩阵，增加了滤波计算量。

发明内容

为解决上述问题，本发明公开了改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，能够提高水下组合导航***的滤波结算效率及导航精度，保证实时性和稳定性。

为了达到上述目的，本发明提供如下技术方案：

改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，包括如下步骤：

(1)对需要用到的坐标系进行定义：

(2)建立水下潜航阶段SINS/DVL子***的状态方程：以SINS的3轴姿态误差，3轴速度误差，位置误差，3轴陀螺仪随机漂移、3轴加速度计零偏和DVL速度偏移误差、刻度系数误差建立21维状态向量，根据***误差方程建立该子***的状态方程；

(3)建立水下潜航阶段SINS/DVL子***的量测方程：根据SINS解算的3维速度量和DVL测量的速度量之差作为量测量，并结合步骤(2)选取的误差状态向量建立该子***的量测方程；

(4)建立水面位置修正阶段SINS/GPS子***的状态方程：不考虑天向运动速度及误差，以SINS的3轴姿态误差、速度误差、位置误差、3轴陀螺仪随机漂移、加速度计零偏建立12维状态向量，根据误差方程建立该子***的状态方程；

(5)建立水面位置修正阶段SINS/GPS子***的量测方程：以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量，并结合步骤(4)选取的误差状态向量建立该子***的量测方程；

(6)综合步骤(2)和步骤(3)建立水下潜航阶段的非线性滤波方程，在水下潜航一段时间以后，AUV浮出水面，结合步骤(4)和步骤(5)建立水面位置修正阶段非线性滤波方程，进行改进的无迹卡尔曼滤波解算，完成时间更新、量测更新和滤波更新，完成定时位置信息校正。

进一步的，所述步骤(1)建立的坐标系具体包括：

i——惯性坐标系：不随地球旋转，原点位于地球中心，z _i轴指向北极，x _i轴指向春分点，y _i轴与x _i、 z _i构成右手坐标系；

e——地球坐标系：与地球固联，原点位于地心，x _e轴穿越本初子午线与赤道交点，z _e轴指向北极，y _e轴x _e、z _e构成右手坐标系；

b——载体坐标系：原点位于运载体中心，z _b轴垂直运载体向上，x _b指向运载体前方，y _b与x _b、z _b构成右手坐标系；

p——实际计算得出的平台坐标系；

n——与东-北-天地理坐标系重合的导航坐标系。

进一步的，所述步骤(2)中建立水下潜航阶段SINS/DVL子***的状态方程具体包括如下步骤：

取姿态误差角(φ _E φ _N φ _U)、速度误差(δv _E δv _N δv _U)位置误差(δL δλ δh)、陀螺常值漂移(ε _bx ε _by ε _bz)和加速度计随机常值误差

作为SINS***的状态量，记为:

通过理想惯导比力方程的速度微分方程和捷联惯导***的实际速度微分方程，推导出四元数表示的速度误差方程：

其中，V ⁿ是载体在n系下的理想速度，

是n系下的地球自转角速度，

为n系相对于e系的角速度在n系下的投影，f ^b为b系下的比力；式中，

为p系到n系的转换四元数，

为b系到p系的转换四元数，

和

分别代表p系到n系以及b系到p系的转换矩阵；

式中，“～”表示载体实际测量值，δ表示载体的理想值与实际测量值之间的误差，g ⁿ为n系下的重力加速度，

为b系下的加速度计误差向量；

四元数姿态误差方程：

其中，

表示n系相对于i系的旋转角速度在n系下的投影，

为n系到p系的方向余弦矩阵，ε ^b为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵；

位置误差方程：

其中，R _M和R _N分别表示地球的子午圈曲率半径和卯酉圈曲率半径，L表示载体的纬度，λ表示载体的经度，h表示载体的高度；

SINS***的噪声:

W _N(t)＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az] ^T

则SINS的***误差方程可以表示为：

F _N[·]为非线连续函数；

取DVL速度偏移误差(δV _dx δV _dy δV _dz)和刻度系数误差(Δk _dx Δk _dy Δk _dz)作为DVL***状态变量，记为:

X _D(t)＝[δV _dx δV _dy δV _dz δk _dx δk _dy δk _dz] ^T

DVL的误差模型为：

其中，β _d表示速度偏移误差的相关时间倒数，ω _d表示激励白噪声；

相应的误差状态方程为：

式中：

W _D(t)＝[ω _dx ω _dy ω _dz] ^T；ω _di(i＝x,y,z)为激励白噪声；G _D(t)＝[I _3×3 O _3×3]；τ _di(i＝x,y,z)表示速度偏移误差的相关时间；

选取SINS和DVL子***的误差状态变量，则组合导航***的状态向量为

***的噪声向量为

***的状态方程表示为：

式中，状态函数F ₁[·]为非线性连续函数，Γ ₁(t)为该子***噪声阵。

进一步的，所述步骤(3)中建立水下潜航阶段SINS/DVL子***的量测方程具体包括如下步骤：

由SINS和DVL形成的量测量：

其中，由SINS提供变化所用的姿态矩阵

v _EI,v _NI,v _UI分别表示SINS解算出的三轴速度，v _ED,v _ND,v _UD分别表示DVL测出的三轴速度，v _E,v _N,v _U表示载体在导航坐标系下的真实速度，v _x,v _y,v _z表示在载体坐标系下的真实速度，δv _DE，δv _DN，δv _DU为DVL测速误差换算到导航坐标系后的误差；

将上式展开，并结合之前选取的***误差状态

得该组合***的量测方程为：

Z ₁＝H ₁[X ₁(t),t]+V ₁(t)

式中：H ₁[·]为非线性连续函数；V ₁(t)为量测噪声。

进一步的，所述步骤(4)中建立水面位置修正阶段SINS/GPS子***的状态方程具体包括如下步骤：

取状态变量：

***的噪声误差为：

W ₂(t)＝[0 0 ω _gx ω _gy ω _gz ω _ax ω _ay 0 0 0 0 0] ^T

建立基于SINS/GPS的AUV组合导航连续***状态方程：

式中：F ₂[·]为非线性连续函数，Γ ₂(t)为该子***噪声阵。

进一步的，所述步骤(5)中建立水面位置修正阶段SINS/GPS子***的量测方程具体包括如下步骤：

将SINS解算出来的位置、速度与GPS输出的位置位置、速度之差作为AUV水面位置修正阶段滤波解算的量测方程：

式中L、λ、V _E和V _N分别为SINS解算出来的位置、速度，L _G、λ _G、V _GE和V _GN分别为GPS输出的位置位置、速度，δ·表示相应的误差；将上式展开，并结合之前选取的***误差状态X ₂(t)，得到量测方程为：

Z ₂＝H ₂[X ₂(t),t]+V ₂(t)

式中：H ₂[·]为非线性连续函数，V ₂为量测噪声。

进一步的，所述步骤(6)中综合步骤(2)和步骤(3)建立的水下潜航阶段的非线性滤波方程为：

结合步骤(4)和步骤(5)建立的水面位置修正阶段非线性滤波方程为：

进一步的，所述步骤(6)中进行改进的无迹卡尔曼滤波解算的过程具体包括如下步骤：

对水下潜航阶段的非线性滤波方程离散化有：

其中，X _k和Z _k分别为***在t _k时刻的状态向量以及量测向量，W _k和V _k分别为水下潜航阶段子***的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

Q _k和R _k分别为子***噪声协方差阵和量测噪声协方差阵；具体的算法过程如下：

<1>初始化增广状态向量及估计误差方差

<2>计算sigma点χ _i,k-1和相应的加权因子W _i

W _i＝(1-W ₀)/22,其中，0≤W ₀≤1

其中，

表示21维状态量的第i个采样点；

<3>时间更新方程，得到一步预测

和一步预测误差协方差P _k/k-1

χ _i,k/k-1＝f ₁(χ _i,k-1)

χ _i,k/k-1为t _k时刻预测的第i个样本点；

<4>量测更新方程，得到k时刻的量测预测

量测预测协方差

和状态量与量测量之间的协方差

Z _i,k/k-1＝h ₁(χ _i,k/k-1)

其中，Z _i,k/k-1为第i个量测值；

<5>滤波更新方程，得到滤波增益矩阵K _k、状态量的最优滤波估计

和最优滤波估计误差协方差矩阵P _k：

对水面位置修正阶段非线性滤波方程进行离散化有：

其中，x _k和z _k分别为***在t _k时刻的状态向量以及量测向量，w _k和v _k分别为水面位置修正阶段子***的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

q _k和r _k分别为该子***噪声协方差阵和量测噪声协方差阵；

具体算法步骤如下：

<1>初始化增广状态向量及估计误差方差

<2>计算sigma点χ _i,k-1和相应的加权因子W _i

W _i＝(1-W ₀)/13,其中，0≤W ₀≤1

其中，

表示12维状态量的第i个采样点；

<3>时间更新方程，得到一步预测

和一步预测误差协方差P _k/k-1

χ _i,k/k-1＝f ₂(χ _i,k-1)

χ _i,k/k-1为t _k时刻预测的第i个样本点；

<4>量测更新方程，得到k时刻的量测预测

量测预测协方差

和状态量与量测量之间的协方差

Z _i,k/k-1＝h ₂(χ _i,k/k-1)

其中，Z _i,k/k-1为第i个量测值；

和最优滤波估计误差协方差矩阵P _k：

将每次AUV浮出水面的时的滤波结果的位置信息作为下一次潜航的新的位置信息，定时修正位置。

与现有技术相比，本发明具有如下优点和有益效果：

本发明提出的改进的无迹卡尔曼滤波方法在组合导航中的应用方法，基于复杂加性噪声的球面分布单形采样简化UKF导航算法，大大减少了***状态向量的维数，降低了滤波算法计算的复杂程度，具备较好的实时性、稳定性和精确度。

附图说明

图1为实现本发明方法的组合***导航原理图。

图2是组合导航***的工作流程图。

图3是改进的无迹卡尔曼滤波方法所用到的UT变换原理图。

图4是改进的无迹卡尔曼滤波方法的解算流程框图。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明提供的一种改进的无迹卡尔曼滤波算法在水下组合导航***SINS/DVL/GPS中的应用方法，实现原理如图1-图4所示，其流程主要包括以下步骤：

步骤一：定义需要用到的坐标系；

(1)对需要用到的坐标系做如下定义：

i——惯性坐标系：不随地球旋转，原点位于地球中心，z _i轴指向北极，x _i轴指向春分点，y _i轴与x _i、z _i构成右手坐标系；

p——实际计算得出的平台坐标系；

n——与东-北-天地理坐标系重合的导航坐标系。

步骤二：选取状态变量和量测量建立水下潜航阶段的非线性***模型；取姿态误差角(φ _E φ _N φ _U)、速度误差(δv _E δv _N δv _U)位置误差(δL δλ δh)、陀螺常值漂移(ε _bx ε _by ε _bz)和加速度计随机常值误差

作为SINS***的状态量，记为:

其中，V ⁿ是载体在n系下的理想速度，

是n系下的地球自转角速度，

为p系到n系的转换四元数，

为b系到p系的转换四元数，

和

分别代表p系到n系以及b系到p系的转换矩阵。

为b系下的加速度计误差向量。

四元数姿态误差方程：

其中，

表示n系相对于i系的旋转角速度在n系下的投影，

为n系到p系的方向余弦矩阵，ε ^b为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵。

位置误差方程：

其中，R _M和R _N分别表示地球的子午圈曲率半径和卯酉圈曲率半径，h表示载体所处的位置高度。

SINS***的噪声:

W _N(t)＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az] ^T

式中，ω _gi(i＝x,y,z)表示三轴陀螺高斯白噪声，ω _ai(i＝x,y,z)表示三轴加速度计三轴高斯白噪声。

则SINS的***误差方程可以表示为：

F _N[·]为非线连续函数。

X _D(t)＝[δV _dx δV _dy δV _dz δk _dx δk _dy δk _dz] ^T

DVL的误差模型为：

其中，β _d表示速度偏移误差的相关时间倒数，ω _d表示激励白噪声。

相应的误差状态方程为：

式中：

W _D(t)＝[ω _dx ω _dy ω _dz] ^T；ω _di(i＝x,y,z)为激励白噪声；G _D(t)＝[I _3×3 O _3×3]；τ _di(i＝x,y,z)表示速度偏移误差的相关时间。

***的噪声向量为

***的状态方程可表示为：

由于DVL测得地速在载体坐标系内的分量，要使其输出的速度与SINS输出的速度形成量测量，必须将DVL的输出速度变换到导航坐标系中。其中，

由SINS提供变化所用的姿态矩阵

因此，由SINS和DVL形成的量测量

将上式展开，并结合之前选取的***误差状态

可得该组合***的量测方程为：

式中：H ₁[·]为非线性连续函数；V ₁(t)为量测噪声。

据此，得到潜航阶段卡尔曼滤波模型的状态方程和量测方程：

步骤三：选取状态量和量测量建立水面位置修正阶段的非线性***模型，取状态变量：

***的噪声误差为：

W ₂(t)＝[0 0 ω _gx ω _gy ω _gz ω _ax ω _ay 0 0 0 0 0] ^T

建立基于SINS/GPS的AUV组合导航连续***状态方程：

式中：F ₂[·]为非线性连续函数，Γ ₂(t)为该子***噪声阵。

式中L、λ、V _E和V _N分别为SINS解算出来的位置、速度，L _G、λ _G、V _GE和V _GN分别为GPS输出的位置位置、速度。将上式展开，并结合之前选取的***误差状态X ₂(t)，可得到量测方程为：

Z ₂＝H ₂[X ₂(t),t]+V ₂(t)

式中：H ₂[·]为非线性连续函数，V ₂为量测噪声。

据此，得到水面位置修正阶段卡尔曼滤波模型的状态方程和量测方程为：

步骤四：对上述建立的方程离散化，进行按照图4所示的算法改进的无迹卡尔曼滤波解算，实现时间更新、量测更新、滤波更新。

改进的卡尔曼滤波算法介绍如下：

针对l维状态变量，至少需要l+1样点来描述其均值和方差。球面分布单形采样变换(SSUT)就是通过l+1个分布在以状态均值为原点的球面上的权值相等的采样点来近似状态的概率分布。这样，l+1个球面分布的采样点和状态的均值点构成了l+2个无迹(UT)采样点。将这些点带入非线性函数中，相应的得到非线性函数数值点集，通过这些点集求取变换后的均值和协方差。

对于l维状态变量，球面分布单形采样变换(SSUT)采样点的具体选取步骤如下：

<1>选择0≤W ₀≤1

<2>确定sigma点权值

W _i＝(1-W ₀)/(L+1),i＝1,2,...,l+1

<3>初始化向量序列

<4>扩展向量序列(维数j＝2,...l)

其中，

表示j维随机变量的第i个采样点，O ^j表示j维零向量。

<5>均值为

均方差为P _xx的L维随机变量x的球面分布点为：

在无迹卡尔曼(UKF)推算法中，一般须对***噪声和量测噪声进行状态增广，但是当***噪声和量测噪声均为加性噪声时，可不做增广处理，有利于进一步降低滤波计算。本发明研究了一种基于复杂加性噪声的SSUT采样简化UKF算法。复杂加性噪声非线性离散***模型可表示为：

式中：f[·]、g[·]、h[·]、j[·]均为非线性函数；x _k、z _k分别为状态向量和观测向量；ω _k和v _k分别为***状态噪声和量测噪声向量。其统计特性如下：E[W _k]＝0,

E[V _k]＝0，

由上式定义的***模型可知，复杂加性噪声模型的特点是模型关于噪声是线性的，具体的算法流程如下：

<1>初始化增广状态向量及估计误差方差

<2>计算sigma点和相应的加权因子

W _i＝(1-W ₀)/L+1,其中，0≤W ₀≤1

<3>时间更新

χ _i,k/k-1＝f(χ _i,k-1)

Z _i,k/k-1＝h(χ _i,k/k-1)

<4>量测更新

根据上述算法，对上述建立的水下潜航阶段卡尔曼滤波方程进行离散化。

对于水下潜航阶段的滤波方程

离散化有，

Q _k和R _k分别为子***噪声协方差阵和量测噪声协方差阵。

具体的算法流程如下：

<1>初始化增广状态向量及估计误差方差

<2>计算sigma点和相应的加权因子

W _i＝(1-W ₀)/22,其中，0≤W ₀≤1

其中，

表示21维状态量的第i个采样点。

<3>时间更新方程，得到一步预测

和一步预测误差协方差P _k/k-1

χ _i,k/k-1＝f ₁(χ _i,k-1)

<4>量测更新方程，得到k时刻的量测预测

量测预测协方差

和状态量与量测量之间的协方差

Z _i,k/k-1＝h ₁(χ _i,k/k-1)

和最优滤波估计误差协方差矩阵P _k：

对上述建立的水面位置修正阶段卡尔曼滤波方程进行离散化。对于水面位置修正阶段的滤波方程

离散化有

其中，x _k和z _k分别为***在t _k时刻的状态向量以及量测向量，w _k和v _k分别为水面位置修正阶段子***的噪声阵和量测噪声阵，且均值均为零，，统计特性如下：

q _k和r _k分别为该子***噪声协方差阵和量测噪声协方差阵。具体算法流程如下：

<1>初始化增广状态向量及估计误差方差

<2>计算sigma点和相应的加权因子

W _i＝(1-W ₀)/13,其中，0≤W ₀≤1

其中，

表示12维状态量的第i个采样点。

<3>时间更新方程，得到一步预测

和一步预测误差协方差P _k/k-1

χ _i,k/k-1＝f ₂(χ _i,k-1)

<4>量测更新方程，得到k时刻的量测预测

量测预测协方差

和状态量与量测量之间的协方差

Z _i,k/k-1＝h ₂(χ _i,k/k-1)

和最优滤波估计误差协方差矩阵P _k：

将每次AUV浮出水面的时的滤波结果的位置信息作为下一次潜航的新的位置信息，从而实现位置的定时修正，克服惯导的累积误差。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims

改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，包括如下步骤：

(1)对需要用到的坐标系进行定义：

(2)建立水下潜航阶段SINS/DVL子***的状态方程：以SINS的3轴姿态误差，3轴速度误差，位置误差，3轴陀螺仪随机漂移、3轴加速度计零偏和DVL速度偏移误差、刻度系数误差建立21维状态向量，根据***误差方程建立该子***的状态方程；

(3)建立水下潜航阶段SINS/DVL子***的量测方程：根据SINS解算的3维速度量和DVL测量的速度量之差作为量测量，并结合步骤(2)选取的误差状态向量建立该子***的量测方程；

(4)建立水面位置修正阶段SINS/GPS子***的状态方程：不考虑天向运动速度及误差，以SINS的3轴姿态误差、速度误差、位置误差、3轴陀螺仪随机漂移、加速度计零偏建立12维状态向量，根据误差方程建立该子***的状态方程；

(5)建立水面位置修正阶段SINS/GPS子***的量测方程：以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量，并结合步骤(4)选取的误差状态向量建立该子***的量测方程；

(6)综合步骤(2)和步骤(3)建立水下潜航阶段的非线性滤波方程，在水下潜航一段时间以后，AUV浮出水面，结合步骤(4)和步骤(5)建立水面位置修正阶段非线性滤波方程，进行改进的无迹卡尔曼滤波解算，完成时间更新、量测更新和滤波更新，完成定时位置信息校正。
根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(1)建立的坐标系具体包括：

i——惯性坐标系：不随地球旋转，原点位于地球中心，z _i轴指向北极，x _i轴指向春分点，y _i轴与x _i、z _i构成右手坐标系；

e——地球坐标系：与地球固联，原点位于地心，x _e轴穿越本初子午线与赤道交点，z _e轴指向北极，y _e轴x _e、z _e构成右手坐标系；

b——载体坐标系：原点位于运载体中心，z _b轴垂直运载体向上，x _b指向运载体前方，y _b与x _b、z _b构成右手坐标系；

p——实际计算得出的平台坐标系；

n——与东-北-天地理坐标系重合的导航坐标系。
根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(2)中建立水下潜航阶段SINS/DVL子***的状态方程具体包括如下步骤：

取姿态误差角(φ _E φ _N φ _U)、速度误差(δv _E δv _N δv _U)位置误差(δL δλ δh)、陀螺常值漂移(ε _bx ε _by ε _bz)和加速度计随机常值误差
作为SINS***的状态量，记为:

通过理想惯导比力方程的速度微分方程和捷联惯导***的实际速度微分方程，推导出四元数表示的速度误差方程：

其中，Vn是载体在n系下的理想速度，
是n系下的地球自转角速度，
为n系相对于e系的角速度在n系下的投影，f ^b为b系下的比力；式中，
为p系到n系的转换四元数，
为b系到p系的转换四元数，
和
分别代表p系到n系以及b系到p系的转换矩阵；

式中，“～”表示载体实际测量值，δ表示载体的理想值与实际测量值之间的误差，g ⁿ为n系下的重力加速度，
为b系下的加速度计误差向量；

四元数姿态误差方程：

其中，
表示n系相对于i系的旋转角速度在n系下的投影，
为n系到p系的方向余弦矩阵，ε ^b为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵；

位置误差方程：

其中，R _M和R _N分别表示地球的子午圈曲率半径和卯酉圈曲率半径，L表示载体的纬度，λ表示载体的经度，h表示载体的高度；

SINS***的噪声:

W _N(t)＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az] ^T

则SINS的***误差方程可以表示为：

F _N[·]为非线连续函数；

取DVL速度偏移误差(δV _dx δV _dy δV _dz)和刻度系数误差(Δk _dx Δk _dy Δk _dz)作为DVL***状态变量，记为:

X _D(t)＝[δV _dx δV _dy δV _dz δk _dx δk _dy δk _dz] ^T

DVL的误差模型为：

其中，β _d表示速度偏移误差的相关时间倒数，ω _d表示激励白噪声；

相应的误差状态方程为：

式中：
W _D(t)＝[ω _dx ω _dy ω _dz] ^T；ω _di(i＝x,y,z)为激励白噪声；G _D(t)＝[I _3×3 O _3×3]；τ _di(i＝x,y,z)表示速度偏移误差的相关时间；

选取SINS和DVL子***的误差状态变量，则组合导航***的状态向量为
*** 的噪声向量为
***的状态方程表示为：

式中，状态函数F ₁[·]为非线性连续函数，Γ ₁(t)为该子***噪声阵。
根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(3)中建立水下潜航阶段SINS/DVL子***的量测方程具体包括如下步骤：

由SINS和DVL形成的量测量：

其中，由SINS提供变化所用的姿态矩阵
v _EI,v _NI,v _UI分别表示SINS解算出的三轴速度，v _ED,v _ND,v _UD分别表示DVL测出的三轴速度，v _E,v _N,v _U表示载体在导航坐标系下的真实速度，v _x,v _y,v _z表示在载体坐标系下的真实速度，δv _DE，δv _DN，δv _DU为DVL测速误差换算到导航坐标系后的误差；

将上式展开，并结合之前选取的***误差状态
得该组合***的量测方程为：

Z ₁＝H ₁[X ₁(t),t]+V ₁(t)

式中：H ₁[·]为非线性连续函数；V ₁(t)为量测噪声。
根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(4)中建立水面位置修正阶段SINS/GPS子***的状态方程具体包括如下步骤：

取状态变量：

***的噪声误差为：

W ₂(t)＝[0 0 ω _gx ω _gy ω _gz ω _ax ω _ay 0 0 0 0 0] ^T

建立基于SINS/GPS的AUV组合导航连续***状态方程：

式中：F ₂[·]为非线性连续函数，Γ ₂(t)为该子***噪声阵。
根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(5)中建立水面位置修正阶段SINS/GPS子***的量测方程具体包括如下步骤：

将SINS解算出来的位置、速度与GPS输出的位置位置、速度之差作为AUV水面位置修正阶段滤波解算的量测方程：

式中L、λ、V _E和V _N分别为SINS解算出来的位置、速度，L _G、λ _G、V _GE和V _GN分别为GPS输出的位置位置、速度，δ·表示相应的误差；将上式展开，并结合之前选取的***误差状态X ₂(t)，得到量测方程为：

Z ₂＝H ₂[X ₂(t),t]+V ₂(t)

式中：H ₂[·]为非线性连续函数，V ₂为量测噪声。
根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(6)中综合步骤(2)和步骤(3)建立的水下潜航阶段的非线性滤波方程为：

结合步骤(4)和步骤(5)建立的水面位置修正阶段非线性滤波方程为：
根据权利要求7所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(6)中进行改进的无迹卡尔曼滤波解算的过程具体包括如下步骤：

对水下潜航阶段的非线性滤波方程离散化有：

其中，X _k和Z _k分别为***在t _k时刻的状态向量以及量测向量，W _k和V _k分别为水下潜航阶段子***的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

Q _k和R _k分别为子***噪声协方差阵和量测噪声协方差阵；具体的算法过程如下：

<1>初始化增广状态向量及估计误差方差

<2>计算sigma点χ _i,k-1和相应的加权因子W _i

W _i＝(1-W ₀)/22,其中，0≤W ₀≤1

其中，
表示21维状态量的第i个采样点；

<3>时间更新方程，得到一步预测
和一步预测误差协方差P _k/k-1

χ _i,k/k-1＝f ₁(χ _i,k-1)

χ _i,k/k-1为t _k时刻预测的第i个样本点；

<4>量测更新方程，得到k时刻的量测预测
量测预测协方差
和状态量与量测量之间的协方差

Z _i,k/k-1＝h ₁(χ _i,k/k-1)

其中，Z _i,k/k-1为第i个量测值；

<5>滤波更新方程，得到滤波增益矩阵K _k、状态量的最优滤波估计
和最优滤波估计误差协方差矩阵P _k：

对水面位置修正阶段非线性滤波方程进行离散化有：

其中，x _k和z _k分别为***在t _k时刻的状态向量以及量测向量，w _k和v _k分别为水面位置修正阶段子***的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

q _k和r _k分别为该子***噪声协方差阵和量测噪声协方差阵；

具体算法步骤如下：

<1>初始化增广状态向量及估计误差方差

<2>计算sigma点χ _i,k-1和相应的加权因子W _i

W _i＝(1-W ₀)/13,其中，0≤W ₀≤1

其中，
表示12维状态量的第i个采样点；

<3>时间更新方程，得到一步预测
和一步预测误差协方差P _k/k-1

χ _i,k/k-1＝f ₂(χ _i,k-1)

χ _i,k/k-1为t _k时刻预测的第i个样本点；

<4>量测更新方程，得到k时刻的量测预测
量测预测协方差
和状态量与量测量之间的协方差

Z _i,k/k-1＝h ₂(χ _i,k/k-1)

其中，Z _i,k/k-1为第i个量测值；

<5>滤波更新方程，得到滤波增益矩阵K _k、状态量的最优滤波估计
和最优滤波估计误差协方差矩阵P _k：

将每次AUV浮出水面的时的滤波结果的位置信息作为下一次潜航的新的位置信息，定时修正位置。