CN109324330A - 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 - Google Patents

Abstract

本发明提供的是一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法，特别适用于水下设备的定位。本发明由超短基线水声定位系统USBL和捷联惯性导航系统SINS组成，采用混合无导数扩展卡尔曼滤波HDEKF进行组合导航。超短基线系统通过计算超声波信号在应答器和水听器之间单向传播的时间，得到两者之间的斜距测量值，并通过坐标转换公式得到观测方程。再根据捷联惯导系统的误差传递公式建立误差状态方程。最后进行混合无导数卡尔曼滤波，使用标准线性卡尔曼滤波进行时间更新，使用无导数扩展滤波进行量测更新。本发明能有效提高USBL/INS组合导航系统的导航精度和稳定性，并减少实时计算量。

Description

基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定 位方法

技术领域

本发明涉及一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法，特别适用于水下设备的定位，且该算法比一般扩展卡尔曼滤波计算量小。

背景技术

随着科技的发展，人们日益看重对海洋资源的开发，对水下设备也越来越来感兴趣。水下设备广泛应用于各类水中探测工作，比如环境检测，港口的水下检查，地质和生物调查等。常用的水下设备，包括各类水中机器人和潜艇。水下设备广阔的应用前景，引起了各国科学家的极大关注。

水下设备与陆地设备一样，需要一个精确的导航系统来提供姿态，速度，位置等导航信息。GPS和INS组合导航系统，是各类空中飞行器和陆地设备中最常用的导航系统，但是由于水中GPS信号的衰减，针对水下设备需要使用不同的导航设备，如超短基线系统USBL。USBL系统是一种水声定位系统，传统的USBL系统一般包含安装在载体上的水听器基阵和安装在固定位置的应答器，可以通过水听器基阵和应答器之间的斜距差进行定位。但是USBL系统容易受到水中环境的影响，如螺旋桨产生的干扰等，因此一个独立的USBL系统一般不足以精确地进行导航。USBL系统多作为辅助系统，与INS系统进行组合导航。

扩展卡尔曼滤波EKF常用于各类紧组合导航系统中。但是因为EKF需要利用一阶泰勒展开来计算雅可比矩阵，因此对于某些非线性系统来说，该方法将十分复杂而难以应用。为了克服EKF的缺点，科学家提出了无迹卡尔曼滤波UKF，UKF具有二阶甚至更高的精度，如今已经广泛用于各类非线性系统中。基于Sigma点的UKF虽然避免了计算雅可比矩阵，且比EKF具有更高的精度。但是无迹卡尔曼滤波计算量巨大，难以用于实时性要求较高的组合导航系统中。而且在惯性组合导航系统中，系统的非线性特性并不强，所以UKF的表现并不比EKF好很多。因此本发明使用了一种由Brendan M.发明的无导数卡尔曼滤波DEKF，针对一些特殊的非线性系统，该算法可以很好地替代EKF和UKF，只需要计算n+1个预测向量，具有更高的精度和更小的计算量，为USBL/INS组合导航系统提供一种新的研究方法。

发明内容

本发明的目的是为了提高USBL/SINS组合导航系统的导航精度和稳定性，并在保证精度的前提下，减少实时计算量，提供一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法。

为达到上述目的，本发明采用的方法是一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法，由超短基线水声定位系统USBL和捷联惯性导航系统SINS组成，采用混合无导数扩展卡尔曼滤波HDEKF进行组合导航，通过下列步骤实现：

(1)、超短基线系统由安装在载体上的水听器基阵和放置在海底已知位置的应答器组成，水听器基阵由4个水听器构成，通过基阵测量超声波信号从声源到应答器，再从应答器回到第i个水听器的往返传播时间t_RTTi，计算出应答器和水听器之间单向传播的时间t_ri，最后根据水速算出应答器与该水听器之间的斜距值ρ_ri；

(2)、根据捷联惯导系统的误差传递方程，以东北天方向上的姿态误差角φ_E,φ_N,φ_U，东北天方向上的速度误差δv_E,δv_N,δv_U，经度和纬度误差δL，δλ，高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移ε_x,ε_y,ε_z和加速度计零偏▽_x,▽_y,▽_z作为状态量，建立15维的状态向量，根据姿态误差方差，速度误差方程和位置误差方程建立误差状态方程，再根据基于坐标转换的超短基线导航解算方程建立观测方程；

(3)、根据步骤(2)中建立的误差状态方程和观测方程建立基本的扩展卡尔曼滤波方程，再根据扩展卡尔曼滤波滤波方程推导无导数扩展卡尔曼滤波；最后进行混合滤波，使用标准线性卡尔曼滤波进行时间更新，使用无导数扩展卡尔曼滤波进行量测更新。

作为本发明的一种改进，所述的步骤(1)中的计算应答器和水听器之间的斜距测量值方法如下：

(1-1)、通过基阵测量超声波信号从声源到应答器的时间，再从应答器回到基阵上第i个水听器的往返时间，记为t_RTTi；

(1-2)、忽略声源和水听器之间的时差，应答器和第i个水听器之间的声波单向传播时间t_ri，可表示为t_ri＝t_RTTi-t_d-<t_RTT>/2，其中，t_d为应答器延迟时间，<t_RTT>为所有水听器测得的声波往返时间的平均值；

(1-3)、最后考虑水速以及噪声计算得到应答器与该水听器之间的斜距测量值ρ_ri＝v_pt_ri+η，v_p为水速，η为测量噪声。

作为本发明的一种改进，步骤(2)中建立误差状态方程和量测方程的方法如下：

(2-1)、选取东北天坐标系作为导航坐标系，载体坐标系中x轴沿水下载体横轴指向右舷，y轴沿载体纵轴指向前，z轴垂直于x与y轴所确定的平面构成右手坐标系，以惯性导航系统的误差量作为滤波器的状态变量，即选取东北天方向上的姿态误差角φ_E,φ_N,φ_U，东北天方向上的速度误差δv_E,δv_N,δv_U，经度和纬度误差δL，δλ，高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移ε_x,ε_y,ε_z和加速度计零偏▽_x,▽_y,▽_z作为状态量，则有状态量：

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_x,ε_y,ε_z,▽_x,▽_y,▽_z]^T

误差状态方程为：

其中，F(t)为状态转移矩阵，G(t)为噪声分配矩阵

根据捷联惯导系统的误差传递规律，建立姿态、速度和位置误差方程：

姿态误差方程如下:

其中，φ为姿态误差，为导航系相对于惯性系的角速度，为导航计算误差，为陀螺仪测量误差

速度误差方程如下:

其中，fⁿ为加速度计比力输出，δfⁿ为加速度计测量误差，vⁿ为计算速度，为自转角速度计算误差，为导航系旋转计算误差，

位置误差方程如下：

其中，δv_E,δv_N,δv_U为东北天三个方向的速度误差，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径

加速度计和陀螺仪的误差模型可以看作随机常值误差加上白噪声。所以加速度计误差和陀螺仪误差描述如下：

▽_a＝▽_af+w_a

ε_g＝ε_gf+w_g

▽_a为加速度计零偏，ε_g为陀螺仪随机漂移，▽_af，ε_gf为随机常值误差，w_a，w_g为零均值高斯白噪声，方差分别为Q_a，Q_g；

(2-2)、根据基于坐标转换的超短基线导航解算算法，建立量测方程，应答器在载体坐标系的位置t^b可表示为t^e表示应答器在地球坐标系的位置，p^e为安装在载体上的捷联惯导系统的重心在地球坐标系的位置矢量，为载体坐标系到地球坐标系的旋转矩阵；建立基阵坐标系u系，u系原点与超短基线基阵几何中心重合，x,y,z轴与载体坐标系b系的x,y,z轴平行，4个水听器在u系的位置矢量和满足：

应答器与第i个水听器之间的距离ρ_i表示为：

式中，t^u表示为应答器在u系的位置矢量

当忽略声学基阵的偏移和安装误差角等误差项时，b系与u系重合，得：

将捷联惯导位置误差补偿公式和姿态误差补偿公式带入斜距公式中，得：

其中，为捷联惯导实际解算出来的载体中心在地球坐标系的位置矢量，δp^e为位置误差，为实际解算得到的载体坐标系到地球坐标系的旋转矩阵，φ为姿态误差；

假设量如下:

为了书写方便，t^e简写为t，简写为p，δp^e简写为δp，因此斜距方程展开即为下式：

以第i个水听器和第j个水听器的斜距差作为量测量：

Z＝[ρ_I1-ρ_I2 ρ_I1-ρ_I3…ρ_Ij-1-ρ_Ij]^T

因此量测方程为δρ_ij＝ρ_Ii-ρ_Ij。

作为本发明的一种改进，步骤(3)中建立基本的扩展卡尔曼滤波方程，并进行混合无导数扩展卡尔曼滤波的方法如下：

(3-1)一个基本的离散时间非线性系统描述如下：

x_k+1＝f(x_k,u_k)+w_k

z_k+1＝h(z_k,u_k)+v_k

x_k为状态量，z_k为观测量，w_k和v_k为协方差矩阵，期望和方差如下

E(w_k)＝0

var(v_k)＝Q_k

E(w_k)＝0

var(v_k)＝R_k

所建立的扩展卡尔曼滤波基本公式如下：

状态一步预测：

状态一步预测均方差：

滤波增益：

状态估计量:

均方差估计量：

P_k＝(I-K_kH_k)P_k/k-1

Φ_k/k-1为状态转移矩阵，H_k为量测矩阵，为状态量预测值，P_k/k-1为均方差预测值，K_k为滤波增益，状态估计，P_k为均方差估计

一般的扩展卡尔曼滤波方程中的状态转移矩阵和量测矩阵由下式求得：

其中传递矩阵和量测矩阵的计算一般具有一阶精度；

(3-2)无导数扩展卡尔曼滤波利用了无迹卡尔曼滤波的原理，不需要进行泰勒展开，避免了一阶截断误差。而且与无迹卡尔曼滤波相比计算量更小，只需要计算n+1个构造得到的预测向量，具体滤波过程如下：

使用初始状态向量和初始均方差矩阵P_xx来构建n+1个预测向量X_i：

其中，Vx_i为n+1个期望为均方误差矩阵P_xx的离散向量，即E[Δx_iΔx_i ^T]＝P_xx，Δx_i为矩阵的n+1个列向量，即n是状态向量x的维数，α是一个固定参数，1≤α＜∞，取决于所使用的非线性模型，与估计量平均值的离散程度成正比，与非线性模型的阶数成反比

用非线性状态方程f(·)作用于X_i和得到下一时刻预测向量的预测值以及状态向量的预测值

计算均方差矩阵的预测值

用非线性观测方程h(·)作用于预测向量X_i，得到预测向量的观测值用非线性状态方程f(·)作用于观测向量得到观测向量的预测值

计算量测均方误差阵P_ZZ，协方差阵P_XZ：

最后根据卡尔曼滤波原理，遵循最小均方误差估计准则，利用计算得到的状态向量预测值方差阵P_XX，P_ZZ，P_XZ和新获取的观测量z对状态向量和状态均方误差阵P_XX进行更新：

(3-3)通过分析，可以发现捷联惯导系统的误差状态方程是可以线性化的，但超短基线和捷联惯导组合系统的量测方程是非线性的，为了简化算法并减少运算量，本专利提出了混合无导数滤波，结合了线性估计和非线性估计算法的优点，在混合滤波中，使用标准线性卡尔曼滤波进行时间更新，使用无导数扩展卡尔曼滤波进行量测更新。

有益效果：

本发明是一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法，弥补了SINS长时间工作误差发散的问题，满足水下设备长时间工作的要求，并提高了定位精度。以应答器和水听器之间的斜距公式作为观测方程，其中考虑到坐标转换方程等问题，保证了超短基线系统计算结果的精确度。利用混合无导数扩展卡尔曼滤波算法，没有使用泰勒展开，避免了传统扩展卡尔曼滤波算法存在的一阶截断误差。并且使用了混合滤波，大大减少了计算量，提高了算法实时性。

附图说明

图1为本发明的整体流程图；

图2为本发明中USBL系统坐标系转换示意图。

具体实施方式

下面结合附图对本发明进一步详细说明，但是本发明的保护范围不局限于所述实施例。

如图1所示，本发明由超短基线水声定位系统USBL和捷联惯性导航系统SINS组成，采用混合无导数扩展卡尔曼滤波HDEKF进行组合导航，具体实现步骤如下：：

步骤1：超短基线系统由安装在载体上的水听器基阵和放置在海底已知位置的应答器组成，水听器基阵由4个水听器构成，通过基阵测量超声波信号从声源到应答器，再从应答器回到第i个水听器的往返传播时间t_RTTi，表示为：

t_p为声源到应答器的真实时间，t_ri为应答器到第i个水听器的时间，ε_t和ε_c为应答器噪声和水听器接收噪声，ε_di为额外的微分误差，应答延迟时间t_d一般为已知，为取整函数，round(·)为四舍五入函数

假设当接受和返回信号时，载体处于静止状态(一般速度较低)，此时可以忽略发射器和接收器之间的时差，因此应答器和接收器之间的单向传播时间可以通过减去延迟时间和平均循环时间的一半得到：

t_ri＝t_RTTi-t_d-<t_RTT>/2

其中，t_d为应答器延迟时间，<t_RTT>为所有水听器测得的声波往返时间的平均值

最后根据水速算出应答器和水听器之间的斜距测量值：

ρ_ri＝v_pt_ri+η

其中，η为测量噪声。

步骤2：选取东北天坐标系作为导航坐标系，载体坐标系x轴沿水下载体横轴指向右舷，y轴沿载体纵轴指向前，z轴垂直于x与y轴所确定的平面构成右手坐标系。以惯性导航系统的误差量作为滤波器的状态变量，即选取东北天方向上的姿态误差角φ_E,φ_N,φ_U，东北天方向上的速度误差δv_E,δv_N,δv_U，经度和纬度误差δL，δλ，高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移ε_x,ε_y,ε_z和x,y,z轴上的加速度计零偏▽_x,▽_y,▽_z作为状态量，则有状态量：

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_x,ε_y,ε_z,▽_x,▽_y,▽_z]^T

误差状态方程为：

其中，F(t)为状态转移矩阵，G(t)为噪声分配矩阵

根据捷联惯导系统的误差传递规律，建立姿态、速度和位置误差方程：

姿态误差方程如下:

速度误差方程如下:

其中，fⁿ为加速度计比力输出，δfⁿ为加速度计测量误差，vⁿ为计算速度，为自转角速度计算误差，为导航系旋转计算误差，

位置误差方程如下：

其中，δv_E,δv_N,δv_U为东北天三个方向的速度误差，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径

加速度计和陀螺仪的误差模型可以看作随机常值误差加上高斯白噪声。所以加速度计误差和陀螺仪误差描述如下：

▽_a＝▽_af+w_a

ε_g＝ε_gf+w_g

▽_a为加速度计零偏，ε_g为陀螺仪随机漂移，▽_af，ε_gf为随机常值误差，w_a，w_g为零均值高斯白噪声，方差分别为Q_a，Q_g；

根据误差方程整理可得：

系统噪声为：

W(t)＝[w_a,w_g]^T

噪声分配矩阵为：

其中，为载体坐标系到导航坐标系的姿态转移矩阵

状态转移矩阵为：

其中：

可以看出捷联惯性导航系统的误差状态方程是线性的。

根据基于坐标转换的超短基线导航解算算法，建立量测方程

如图2所示，根据坐标变换关系，应答器在载体坐标系的位置，表示为

t^e表示应答器在地球坐标系的位置，p^e为安装在载体上的捷联惯导系统的重心在地球坐标系的位置矢量，为载体坐标系到地球坐标系的旋转矩阵

建立基阵坐标系u系，u系原点与超短基线基阵几何中心重合，x,y,z轴与载体坐标系b系的x,y,z轴平行，4个水听器在u系的位置矢量和满足：

应答器与第i个水听器之间的斜距可表示为：

式中，t^u表示为应答器在u系的位置

水听器基阵是安装在载体上的，因此基阵的位置可以用一组欧拉角表示，u系到b系的姿态矩阵用表示，所以b系中的任意向量在u系中表示如下：

假设u系与b系之间的安装误差为应答器与水听器之间的斜距可以改写为：

当忽略声学基阵的偏移和安装误差角等误差项时，b系与u系重合，上式可以简化为：

根据斜距公式，利用捷联惯导系统误差补偿公式，建立量测方程

位置误差补偿公式如下：

和姿态误差补偿公式如下：

带入斜距公式中，则可得量测方程为：

其中，为捷联惯导实际解算出来的捷联惯导系统在地球坐标系的位置矢量，δp^e为位置误差，为实际解算得到的载体坐标系到地球坐标系的旋转矩阵，φ为姿态误差；

假设量如下:

为了书写方便，t^e简写为t，简写为p，δp^e简写为δp，因此斜距方程展开即为下式：

以第i个水听器和第j个水听器的斜距差作为量测量：

Z＝[ρ_I1-ρ_I2 ρ_I1-ρ_I3…ρ_Ij-1-ρ_Ij]^T

因此量测方程为δρ_ij＝ρ_Ii-ρ_Ij。

步骤3：利用误差状态方程和观测方程建立基本的扩展卡尔曼滤波方程，根据基本滤波方程进行无导数扩展卡尔曼滤波。

一个基本的离散时间非线性系统描述如下：

x_k+1＝f(x_k,u_k)+w_k

z_k+1＝f(z_k,u_k)+v_k

x_k为状态量，z_k为观测量，w_k和v_k为均方差矩阵，期望和方差如下

E(w_k)＝0

var(v_k)＝Q_k

E(w_k)＝0

var(v_k)＝R_k

所建立的扩展卡尔曼滤波基本公式如下：

状态一步预测：

状态一步预测均方差：

滤波增益：

状态估计量:

均方差估计量：

P_k＝(I-K_kH_k)P_k/k-1

为估计值P_k/k-1为均方差Kk滤波增益状态估计

一般的扩展卡尔曼滤波方程中的状态转移矩阵和量测矩阵由下式求得

其中传递矩阵和量测矩阵的计算一般具有一阶精度；

无导数扩展卡尔曼滤波利用了无迹卡尔曼滤波的原理，不需要进行泰勒展开，避免了一阶截断误差。而且与无迹卡尔曼滤波相比计算量更小，只需要计算n+1个构造得到的预测向量，具体滤波过程如下：

使用初始状态向量和初始均方差矩阵P_xx来构建n+1个预测向量X_i：

其中，Vx_i为n+1个期望为均方误差矩阵P_xx的离散向量，即E[Δx_iΔx_i ^T]＝P_xx，Δx_i为矩阵的n+1个列向量，即n是状态向量x的维数；在实际计算时，由于计算精度有限，需要对矩阵进行规范化，转换为对称矩阵，具体公式如下：

其中是α一个固定参数，1≤α＜∞,取决于所使用的非线性模型。与估计量平均值的离散程度成正比，与非线性模型的阶数成反比。

用非线性状态方程f(·)作用于X_i和得到下一时刻预测向量的预测值以及状态向量的预测值

计算均方差矩阵的预测值

用非线性观测方程h(·)作用于预测向量X_i，得到预测向量的观测值用非线性状态方程f(·)作用于观测向量得到观测向量的预测值

计算量测均方误差阵P_ZZ，协方差阵P_XZ：

最后根据卡尔曼滤波原理，遵循最小均方误差估计准则，利用计算得到的状态向量预测值方差阵P_XX，P_ZZ，P_XZ和新获取的观测量z对状态向量和状态均方误差阵P_XX进行更新：

通过分析，可以发现捷联惯导系统的误差模型是线性的，但超短基线和捷联惯导组合系统的量测方程是非线性的。为了简化算法并减少运算量，本专利提出的混合无导数滤波，结合了线性估计和非线性估计算法的优点。在混合滤波中，使用标准线性卡尔曼滤波进行时间更新，使用无导数扩展卡尔曼滤波进行量测更新。混合滤波的具体过程如下：

利用基本的状态预测方程和均方差预测方程进行时间更新，方程如下：

量测更新部分利用无导数扩展卡尔曼滤波。

使用上一步的估计量和来构建预测向量

用状态方程f(·)作用于进行更新：

用非线性观测方程h(·)作用于预测向量得到预测向量的观测值用状态方程f(·)作用于观测向量得到观测向量的预测值

计算量测均方误差阵P_ZZ，协方差阵P_XZ：

最后根据卡尔曼滤波原理，遵循最小均方误差估计准则，利用计算得到的状态向量预测值方差阵P_XX，P_ZZ，P_XZ和新获取的观测量z对状态向量和状态均方误差阵P_XX进行更新：

Claims (4)

1.一种基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法，其特征在于：由超短基线水声定位系统USBL和捷联惯性导航系统SINS组成，采用混合无导数扩展卡尔曼滤波HDEKF进行组合导航，通过下列步骤实现：

(1)、超短基线系统由安装在载体上的水听器基阵和放置在海底已知位置的应答器组成，水听器基阵由4个水听器构成，通过基阵测量超声波信号从声源到应答器，再从应答器回到第i个水听器的往返传播时间t_RTTi，计算出应答器和水听器之间单向传播的时间t_ri，最后根据水速算出应答器与该水听器之间的斜距值ρ_ri；

(2)、根据捷联惯导系统的误差传递方程，以东北天方向上的姿态误差角φ_E,φ_N,φ_U，东北天方向上的速度误差δv_E,δv_N,δv_U，经度和纬度误差δL，δλ，高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移ε_x,ε_y,ε_z和加速度计零偏▽_x,▽_y,▽_z，建立15维的状态向量，根据姿态误差方差，速度误差方程和位置误差方程建立误差状态方程，再根据基于坐标转换的超短基线导航解算方程建立观测方程；

(3)、根据步骤(2)中建立的误差状态方程和观测方程建立基本的扩展卡尔曼滤波方程，再根据扩展卡尔曼滤波滤波方程推导无导数扩展卡尔曼滤波方程；最后进行混合滤波，使用标准线性卡尔曼滤波进行时间更新，使用无导数扩展卡尔曼滤波进行量测更新。

2.根据权利要求1所述的基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法，其特征在于，步骤(1)的具体步骤如下：

(1-1)、通过基阵测量超声波信号从声源到应答器的时间，再从应答器回到基阵上第i个水听器的往返时间，记为t_RTTi；

(1-2)、忽略声源和水听器之间的时差，应答器和第i个水听器之间的超声波信号单向传播时间t_ri，可表示为t_ri＝t_RTTi-t_d-<t_RTT>/2，其中，t_d为应答器延迟时间，<t_RTT>为所有水听器测得的声波往返时间的平均值；

(1-3)、最后考虑水速以及噪声计算得到应答器与该水听器之间的斜距测量值ρ_ri＝v_pt_ri+η，v_p为水速，η为测量噪声。

3.根据权利要求1所述的基于混合无导数扩展卡尔曼滤波的USBL/SINS紧组合导航定位方法，其特征在于，步骤(2)的具体步骤如下：

(2-1)、选取东北天坐标系作为导航坐标系，载体坐标系中x轴沿水下载体横轴指向右舷，y轴沿载体纵轴指向前，z轴垂直于x与y轴所确定的平面构成右手坐标系，以惯性导航系统的误差量作为滤波器的状态变量，即选取东北天方向上的姿态误差角φ_E,φ_N,φ_U，东北天方向上的速度误差δv_E,δv_N,δv_U，经度和纬度误差δL，δλ，高度误差δh以及沿载体坐标系x,y,z轴上的陀螺仪随机漂移ε_x,ε_y,ε_z和加速度计零偏▽_x,▽_y,▽_z作为状态量，则有状态量：

X＝[φ_E φ_N φ_U δv_E δv_N δv_U δL δL δh ε_x ε_y ε_z ▽_x ▽_y ▽_z]^T

误差状态方程为：

其中，F(t)为状态转移矩阵，G(t)为噪声分配矩阵

根据捷联惯导系统的误差传递规律，建立姿态、速度和位置误差方程：

姿态误差方程如下：

其中，φ为姿态误差，为导航系相对于惯性系的角速度，为导航计算误差，为陀螺仪测量误差

速度误差方程如下：

其中，fⁿ为加速度计比力输出，δfⁿ为加速度计测量误差，vⁿ为计算速度，为自转角速度计算误差，为导航系旋转计算误差

位置误差方程如下：

其中，δv_E,δv_N,δv_U为东北天三个方向的速度误差，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径；

加速度计和陀螺仪的误差模型可以看作随机常值误差加上高斯白噪声，所以加速度计误差和陀螺仪误差描述如下：

▽_a＝▽_af+w_a

ε_g＝ε_gf+w_g

▽_a为加速度计零偏，ε_g为陀螺仪随机漂移，▽_af，ε_gf为随机常值误差，w_a，w_g为零均值高斯白噪声，方差分别为Q_a，Q_g；

根据误差方程整理可得：

系统噪声为：

W(t)＝[w_a,w_g]^T

噪声分配矩阵为：

其中，为载体坐标系到导航坐标系的姿态转移矩阵；

状态转移矩阵为：

其中：

C_ij为姿态转移矩阵的元素

可以看出捷联惯性导航系统的误差状态方程是线性的；

(2-2)、根据基于坐标转换的超短基线导航解算算法，建立量测方程，应答器在载体坐标系的位置t^b可表示为t^e表示应答器在地球坐标系的位置，p^e为安装在载体上的捷联惯导系统的重心在地球坐标系的位置矢量，为载体坐标系到地球坐标系的旋转矩阵；建立基阵坐标系u系，u系原点与超短基线基阵几何中心重合，x,y,z轴与载体坐标系b系的x,y,z轴平行，4个水听器在u系的位置矢量和满足：

应答器与第i个水听器之间的距离ρ_i表示为：

ρ_i＝||r_i ^u-t^u||

式中，t^u表示为应答器在u系的位置矢量

当忽略声学基阵的偏移和安装误差角等误差项时，b系与u系重合，得：

将捷联惯导位置误差补偿公式和姿态误差补偿公式带入斜距公式中，得：

其中，为捷联惯导实际解算出来的捷联惯导系统在地球坐标系的位置矢量，δp^e为位置误差，为实际解算得到的载体坐标系到地球坐标系的旋转矩阵，φ为姿态误差；

假设量如下:

为了书写方便，t^e简写为t，简写为p，δp^e简写为δp，因此斜距方程展开即为下式：

以第i个水听器和第j个水听器的斜距差作为量测量：

Z＝[ρ_I1-ρ_I2 ρ_I1-ρ_I3 … ρ_Ij-1-ρ_Ij]^T

因此量测方程为δρ_ij＝ρ_Ii-ρ_Ij。

4.根据权利要求1所述的基于混合无导数扩展卡尔曼滤波的超短基线与捷联惯性组合导航定位方法，其特征在于，步骤(3)的具体步骤如下：

(3-1)建立扩展卡尔曼滤波基本方程一个基本的离散时间非线性系统描述如下：

x_k+1＝f(x_k,u_k)+w_k

z_k+1＝h(z_k,u_k)+v_k

x_k为状态量，z_k为观测量，w_k为系统噪声向量和v_k为量测噪声向量，期望和方差如下：

E(w_k)＝0

var(v_k)＝Q_k

E(w_k)＝0

var(v_k)＝R_k

所建立的扩展卡尔曼滤波基本公式如下：

状态一步预测：

状态一步预测均方差：

滤波增益：

状态估计量:

均方差估计量：

P_k＝(I-K_kH_k)P_k/k-1

Φ_k/k-1为状态转移矩阵，H_k为量测矩阵，为状态量预测值，P_k/k-1为均方差预测值，K_k为滤波增益，状态估计，P_k为均方差估计；

一般的扩展卡尔曼滤波方程中的状态转移矩阵和量测矩阵由下式求得：

其中传递矩阵和量测矩阵的计算一般具有一阶精度；

(3-2)无导数扩展卡尔曼滤波利用了无迹卡尔曼滤波的原理，不需要进行泰勒展开，避免了一阶截断误差。而且与无迹卡尔曼滤波相比计算量更小，只需要计算n+1个构造得到的预测向量，具体滤波过程如下：

使用初始状态向量和初始均方差矩阵P_xx来构建n+1个预测向量X_i：

其中，Vx_i为n+1个期望为均方误差矩阵P_xx的离散向量，即E[Δx_iΔx_i ^T]＝P_xx，Δx_i为矩阵的n+1个列向量，即n是状态向量x的维数，α是一个固定参数，1≤α＜∞，取决于所使用的非线性模型，与估计量平均值的离散程度成正比，与非线性模型的阶数成反比；

用非线性状态方程f(·)作用于X_i和得到下一时刻预测向量的预测值以及状态向量的预测值

计算均方差矩阵的预测值

用非线性观测方程h(·)作用于预测向量X_i，得到预测向量的观测值用非线性状态方程f(·)作用于观测向量得到观测向量的预测值

计算量测均方误差阵P_ZZ，协方差阵P_XZ：

最后根据卡尔曼滤波原理，遵循最小均方误差估计准则，利用计算得到的状态向量预测值方差阵P_XX，P_ZZ，P_XZ和新获取的观测量z对状态向量和状态均方误差阵P_XX进行更新：

(3-3)通过分析，可以发现捷联惯导系统的误差状态方程是可以线性化的，但超短基线和捷联惯导组合系统的量测方程是非线性的，为了简化算法并减少运算量，本专利提出了混合无导数滤波，结合了线性估计和非线性估计算法的优点，在混合滤波中，使用标准线性卡尔曼滤波进行时间更新，使用无导数扩展卡尔曼滤波进行量测更新。