技术领域
[0001] 本
发明属于水下定位技术领域，特别涉及一种水下航行器的单信标定位方法。
背景技术
[0002] 精确的
位置反馈是水下航行器完成既定水下任务的
基础。由于水下
电磁波信号衰减较快，广泛应用于陆地与天空定位的GNSS系统在水下无法应用。现有主流的水下定位方式包括以惯性导航为代表的航位推算方法以及以长基线定位为代表的水下声学定位方法。其中惯性导航设备往往会随时间增长产生较大累计误差，无法长时间用于水下定位，而高
精度的惯性导航设备成本极高，限制了其在水下航行器中的应用。现有主流的水下声学定位方式包括长基线定位、超短基线定位、单信标定位等。长基线定位与超短基线定位发展均较为成熟，但其成本通常较高，且实时性通常较差，这限制了其在水下航行器中的应用。而新兴的水下单信标定位系统融合航位推算数据与单水声信标的测距信息，在定位成本和实时性方面均有较大的优势。水声测距是通过检测水声信号的传递时间乘以水声声速获得。
目前的水下单信标定位方法通常假设水声声速完全已知，但实际的水声声速收到水域
温度、
盐度、
密度、水深等因素的影响，通常为时变未知的。水声声速的设置误差会导致测距误差，进而会影响单信标定位系统定位精度。而现有基于未知有效声速的水下单信标定位方法均将有效声速不确定性建模为均值和方差均已知的Gauss分布。但在实际的水下定位当中，受到水下多变环境的影响，水声声速不确定性的统计参数通常为时变的，且无法准确获得。而噪声统计参数设置不准确会恶化定位系统的性能，甚至可能会导致滤波发散，在这种情况下，传统的基于已知噪声参数的Gauss分布对水声声速不确定性进行建模的单信标定位方法的定位性能会受到影响，这会影响其实际应用。
发明内容
[0003] 本发明的目的是：针对水下单信标定位当中水声声速的未知性及其不确定性统计参数的未知性，结合扩展Kalman滤波与变分贝叶斯近似提出一种可估计未知有效声速的自适应水下信标定位方法。
[0004] 本发明的技术方案是：一种可估计未知有效声速的自适应水下单信标定位方法，水下航行器搭载
水听器、多普勒测速仪、深度计、
电子罗盘及GPS；水声信标周期性广播水声信号，水下航行器通过所搭载的多普勒测速仪周期性的观测其绝对速度；水下航行器通过GPS获取初始位置，并且通过读取自身螺旋桨转速获得航行器与水的相对速度。水下航行器未接收到水声信号时通过自身携带的
电子罗盘及读取自身螺旋桨转速信息进行航位推算；水下航行器在接收到多普勒测速仪提供的绝对速度观测时，通过结合螺旋桨转速信息及电子罗盘信息构造海流观测变量，基于Kalman滤波进行海流速度更新；水下航行器接收到水声信号后，考虑水下声速的未知性以及声速不确定性噪声参数的未知性，将未知水声声速建模为均值及方差均未知的Gauss分布，基于扩展Kalman滤波
算法及变分贝叶斯近似，以水声信号传递时间为观测变量，进行水下航行器的位置更新。本发明的步骤包括：
[0005] A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性
坐标系；
[0006] B.通过水下航行器所搭载的GPS系统获取该所述水下航行器在水下局部惯性系当中的初始位置；
[0007] C.建立所述水下航行器的
运动学模型以及观测模型并进行离散化；
[0008] D.建立有效声速及其不确定性统计参数的随机模型以及函数模型；
[0009] E.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知；所述水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，同时进行有效声速随机模型参数预测；所述水下航行器在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取所述螺旋桨转速信息及所述电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；
[0010] F.所述水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速及其不确定性统计参数的未知性，以此基于扩展Kalman 滤波算法及变分贝叶斯近似，以水声信号传递时间为观测变量，进行所述水下航行器的位置更新。
[0011] 进一步的，所述C步骤中，所述运动学模型的建立方法为：
[0012] 定义状态向量为：
[0013] x＝[x y vcx vcy]T
[0014] 其中：x，y为所述水下航行器在所述水下局部惯性坐标系中的水平位置；vcx，vcy为未知的海流速度；
[0015] 对x求导并加入所述水下航行器运动运动模型噪声影响，得到所述水下航行器的运动学模型：
[0016]
[0017] 其中：vwx为所述水下航行器x方向的对水速度；vwy为所述水下航行器y方向的对水速度；vwx及vwy通过读取所述螺旋桨转速与所述电子罗盘测得的航行器艏向
角计算得出；ωx为所述水下航行器在x方向的位置不确定性；ωy为所述水下航行器在y方向的位置不确定性；ωcx为x方向的海流不确定性；ωcy为y方向的海流不确定性；
[0018] vwx及vwy的计算公式为： 式中vw为根据所述螺旋桨转速得出的所述水下航行器对水速度， 为所述电子罗盘测得的艏向角。
[0019] 进一步的，所述C步骤中，所述观测模型的建立方法为：
[0020] S1.建立水声信号传递时间的观测模型；
[0021] 设所述水下航行器获得所述水声信标发射水声信号的时刻为Te，所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为XTe，YTe，ZTe，所述水下航行器接收到该水声信号的时刻为Ta，观测方程为：
[0022]
[0023] 其中：νt为对应的观测噪声；z为所述水下航行器的深度，由深度计精确测得，为已知量；ve为有效声速；
[0024] 将观测方程记作m＝h(x,ve)，其中：
[0025]
[0026] S2.建立海流流速观测模型；
[0027] 根据所述多普勒测速仪测得的所述水下航行器的绝对速度vg，结合所述电子罗盘测得的艏向角 计算得到所述水下航行器绝对速度在局部惯性坐标系下的分量vgx,vgy；
[0028] 根据vgx,vgy以及vwx,vwy，计算得到海流速度观测分量为：
[0029]
[0030] 海流观测量为线性，满足mvc＝Hx+νvc；
[0031] 其中：观测向量mvc＝[mcx mcy]T；mcx，mcy分别为x，y方向海流速度观测；νvc为海流观测噪声向量，νvc＝[νv,cx νv,cy]T，其中νv,cx为x方向的海流不确定性；νv,cy为y方向的海流不确定性；H为海流
观测矩阵，满足：
[0032]
[0033] 进一步的，所述C步骤中，所述运动学模型以及观测模型离散化方法为：
[0034] S1.运动学模型离散化；
[0035] 以下标k为时间索引，以Δt＝tk+1-tk为离散间隔，运动学模型离散为：
[0036] xk+1＝Akxk+Bkuk+wk
[0037] 其中：Ak为运动学方程，满足：
[0038]
[0039] Bk为控制方程，满足：
[0040]
[0041] uk为控制向量，满足uk＝[vwx,k vwy,k]T，为已知量；
[0042] wk过程噪声向量，满足wk＝[ωx,k ωy,k ωcx,k ωcy,k]T，对应各个状态变量的不确定性；将系统状态xk,yk,vcx,k,vcy,k的过程噪声建模为零均值Gauss分布，其协方差矩阵满足：
[0043]
[0044] 其中，σw为所述水下航行器对水速度观测不确定性的标准差；σc为海流不确定性的标准差；
[0045] S2.观测模型离散化；
[0046] 所述水下航行器在k-1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：
[0047]
[0048] 其中，νt,k为观测噪声，假设其满足方差为Rt,k的Gauss分布；考虑到有效声速ve,k的时变未知性，将ve,k也看作随机变量；离散形式的观测方程写作：
[0049] mk＝hk(xk,ve,k)，
[0050]
[0051] 由于海流观测
采样频率较高，假设在每一个离散时间点k处均可以得到海流观测，故离散后的海流观测方程为：
[0052] mvc，k＝Hkxk+νvc,k
[0053] 其中，Hk为k时刻海流速度观测矩阵，满足：
[0054]
[0055] νvc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：
[0056]
[0057] 其中，σvc,m为海流速度观测噪声的标准差。
[0058] 进一步的，所述D步骤中，有效声速及其不确定性统计参数的随机模型以及函数模型建立方法为：
[0059] 将有效声速初始先验分布建模为Gauss分布：
[0060]
[0061] 其中：N(x|μ,Σ)表示以μ为均值，Σ为协方差矩阵，满足Gauss分布的随机向量x；为初始声速估计均值；Pe,0为初始声速不确定性方差；
[0062] 将k-1时刻有效声速的后验分布也建模为Gauss分布：
[0063]
[0064] 分别为k-1时刻有效声速的后验均值与方差；
[0065] 有效声速的动态扩散的函数模型记作：
[0066] ve,k＝ve,k-1+ωe,k-1
[0067] 其中：ωe,k-1为有效声速的不确定性，将其建模为均值及方差均未知的Gauss分布：
[0068]
[0069] 其中：μk-1与σe,k-1分别为声速不确定性的均值与标准差；
[0070] 据此，可以将有效声速的概率扩散方程记作：
[0071]
[0072] 结合上式，获得有效声速的
预测模型为：
[0073]
[0074] 其中 分别为k时刻有效声速的先验均值与方差，其计算公式为：
[0075]
[0076]
[0077] 由于真实的有效声速的不确定性均值和方差未知，以其名义值 与 进行声速预测，预测方程为：
[0078]
[0079]
[0080] 其中： 与 表示有效声速先验统计参数(均值与方差)的名义值；
[0081] 通过在线估计Pe,k|k-1来补偿σe,k-1设置误差的影响；将μk-1与Pe,k|k-1的先验分布建模为其共轭先验，即Gauss-inverse Gama(GIG)分布：
[0082]
[0083] 其中： 表示μk-1与Pe,k|k-1的先验统计参数，GIG(a,A|τ,α,λ,ν) 表示以τ,α,λ,ν为参数的GIG分布，可以被分解为：
[0084] GIG(a,A|τ,α,λ,ν)＝N(a|τ,αA)IG(A|λ,ν)
[0085] 其中：IG(A|λ,ν)表示以λ,ν为参数的逆Gama分布；
[0086] μk-1与Pe,k|k-1的先验分布可以被分解为：
[0087]
[0088]
[0089] μk-1与Pe,k|k-1的先验估计需要与其名义值匹配，即μk-1与Pe,k|k-1的先验统计参数满足：
[0090]
[0091]
[0092] 可设计μk-1与Pe,k|k-1统计参数预测方程为：
[0093]
[0094]
[0095]
[0096]
[0097] 其中：ρα与ρλ为调制参数。
[0098] 进一步的，所述E步骤中，所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：
[0099] 根据Kalman滤波的预测环节，将系统状态的先验分布近似为Gauss分布：
[0100]
[0101] 其中： 与Pk|k-1分别为k时刻的先验状态和先验方差，其计算方法为：
[0102]
[0103]
[0104] 其中： 与Pk-1|k-1分别为k-1时刻的后验状态和后验方差。
[0105] 进一步的，所述E步骤中，所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：
[0106]
[0107]
[0108] Pk|k＝Pk|k-1-KkHkPk|k-1
[0109] 其中：Kk为海流更新Kalman增益。
[0110] 进一步的，所述步骤F中，通过扩展Kalman滤波及变分贝叶斯近似进行水声信号传递时间校正的方法为：
[0111] S1.建立观测似然函数、有效声速后验分布及其不确定性统计参数后验概率密度分布模型；
[0112] 根据离散形式的水声信号传递时间观测模型，得到水声信号传递时间的观测似然密度为：
[0113] p(mk|ve,k,xk)＝N(mk|hk(xk,ve,k),Rk)
[0114] 将有效声速后验分布建模为Gauss分布，即：
[0115]
[0116] μk-1与Pe,k|k-1的后验分布同样建模为GIG分布，分解为：
[0117]
[0118]
[0119] S2.定义状态变量、有效声速及声速不确定性统计参数后验估计近似值；
[0120] 通过变分贝叶斯近似，将状态变量、有效声速及声速不确定性统计参数的联合后验估计近似为：
[0121] p(xk,ve,k,μk-1,Pe,k|k-1|m1:k)≈q(xk)q(ve,k)q(μk-1)q(Pe,k|k-1)
[0122] 以最小化近似前与近似后两个概率密度函数之间的Kullback-Leibler散度(KLD)最小化为目标，得到近似解为：
[0123]
[0124] 其中：log(·)表示对数运算；θ表示xk,ve,k,μk-1,Pe,k|k-1中的任意元素；Ex[·]表示相对于x的期望；上标(-θ)表示整个集合当中除了θ以外的其它元素；cθ表示与θ无关的常数；采用固定点
迭代来求解q(θ)；
[0125] S3.求解状态变量、有效声速及声速不确定性统计参数后验估计近似值；
[0126] S3.1求解联合概率密度对数值；
[0127]
[0128] 其对数形式表示为：
[0129]
[0130] S3.2求解声速不确定性统计参数Pe,k|k-1估计值；
[0131] 令θ＝Pe,k|k-1，得到：
[0132]
[0133] 其中：
[0134]
[0135]
[0136] 变量加上标(i)表示该变量在第i次迭代中的估计值；进而得到：
[0137]
[0138]
[0139] 取Pe,k|k-1在第i+1次迭代中的估计值为：
[0140]
[0141] S3.3求解声速不确定性统计参数μk-1估计值；
[0142] 令θ＝μk-1，得到：
[0143]
[0144] 进而得到：
[0145]
[0146]
[0147] 取μk-1在第i+1次迭代中的估计值为其期望值，即：
[0148]
[0149] S3.4求解有效声速ve,k估计值；
[0150] 记 选择 为展开点，对非线性的水声信号传递时间观测方程mk＝hk(xk,ve,k)进行线性化，只保留一阶项，得到：
[0151]
[0152] 其中：
[0153]
[0154] 为观测方程的雅克比矩阵；
[0155] 由此得到线性化后的水声信号传递时间观测方程为：
[0156]
[0157] 令θ＝ve,k，得到：
[0158]
[0159] 其中：
[0160]
[0161] 根据Kalman滤波更新式，得到有效声速更新方程为：
[0162]
[0163]
[0164]
[0165] 其中： 为第i+1次迭代中有效声速更新的Kalman增益；
[0166] S3.5求解系统状态xk估计值；
[0167] 以 为展开点，对非线性的水声信号传递时间观测方程mk＝hk(xk,ve,k)进行线性化，只保留一阶项，得到：
[0168]
[0169] 其中：
[0170]
[0171] 为观测方程的雅克比矩阵；
[0172] 由此得到线性化后的水声信号传递时间观测方程为：
[0173]
[0174] 令θ＝xk，得到：
[0175]
[0176] 其中：
[0177]
[0178] 根据Kalman滤波更新式，得到系统状态更新方程为：
[0179]
[0180]
[0181]
[0182] 其中： 为第i+1次迭代中系统状态更新的Kalman增益；
[0183] 记总迭代次数为N，则最终有效声速均值及方差、系统状态均值及协方差矩阵的估计值分别为：
[0184]
[0185]
[0186]
[0187]
[0188] 参数 需要用于k+1时刻的预测过程，记其估计值为
[0189] 有益效果：本发明通过结合Kalman滤波、扩展Kalman滤波及变分贝叶斯近似，将未知有效声速不确定性建模为均值及方差均未知的Gauss分布,以水声信号传递时间为观测变量，进行水下航行器的位置更新。相比于基于已知定常有效声速以及基于声速不确定性统计参数完全已知的水下单信标定位方法，本发明所提出方法可以更好的
跟踪水声声速变化的趋势，进而也可以得到更好的定位结果，增强水下单信标定位系统的实际应用能
力。
附图说明
[0191] 图2为水面船的真实运动轨迹、本发明与两种传统水下单信标定位方法所估计出的运动轨迹；
[0192] 图3为本发明与两种传统水下单信标定位方法的水平定位误差随时间变化的曲线；
[0193] 图4为真实声速值、基于传统方法1与本发明提出方法估计出的水声声速。
具体实施方式
[0194]
实施例1，参见附图1，一种可估计未知有效声速的自适应水下单信标定位方法，包括以下步骤：
[0195] A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
[0196] B.通过水下航行器所搭载的GPS系统获取该所述水下航行器在水下局部惯性系当中的初始位置；
[0197] C.建立所述水下航行器的运动学模型以及观测模型并进行离散化；
[0198] 所述运动学模型的建立方法为：
[0199] 定义状态向量为：
[0200] x＝[x y vcx vcy]T
[0201] 其中：x，y为所述水下航行器在所述水下局部惯性坐标系中的水平位置；vcx，vcy为未知的海流速度；
[0202] 对x求导并加入所述水下航行器运动运动模型噪声影响，得到所述水下航行器的运动学模型：
[0203]
[0204] 其中：vwx为所述水下航行器x方向的对水速度；vwy为所述水下航行器y方向的对水速度；vwx及vwy通过读取所述螺旋桨转速与所述电子罗盘测得的航行器艏向角计算得出；ωx为所述水下航行器在x方向的位置不确定性；ωy为所述水下航行器在y方向的位置不确定性；ωcx为x方向的海流不确定性；ωcy为y方向的海流不确定性；
[0205] vwx及vwy的计算公式为： 式中vw为根据所述螺旋桨转速得出的所述水下航行器对水速度， 为所述电子罗盘测得的艏向角；
[0206] 所述C步骤中，所述观测模型的建立方法为：
[0207] S1.建立水声信号传递时间的观测模型；
[0208] 设所述水下航行器获得所述水声信标发射水声信号的时刻为Te，所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为XTe，YTe，ZTe，所述水下航行器接收到该水声信号的时刻为Ta，观测方程为：
[0209]
[0210] 其中：νt为对应的观测噪声；z为所述水下航行器的深度，由深度计精确测得，为已知量；ve为有效声速；
[0211] 将观测方程记作m＝h(x,ve)，其中：
[0212]
[0213] S2.建立海流流速观测模型；
[0214] 根据所述多普勒测速仪测得的所述水下航行器的绝对速度vg，结合所述电子罗盘测得的艏向角 计算得到所述水下航行器绝对速度在局部惯性坐标系下的分量vgx,vgy；
[0215] 根据vgx,vgy以及vwx,vwy，计算得到海流速度观测分量为：
[0216]
[0217] 海流观测量为线性，满足mvc＝Hx+νvc；
[0218] 其中：观测向量mvc＝[mcx mcy]T；mcx，mcy分别为x，y方向海流速度观测；νvc为海流观测噪声向量，νvc＝[νv,cx νv,cy]T，其中νv,cx为x方向的海流不确定性；νv,cy为y方向的海流不确定性；H为海流观测矩阵，满足：
[0219]
[0220] 所述运动学模型以及观测模型离散化方法为：
[0221] S1.运动学模型离散化；
[0222] 以下标k为时间索引，以Δt＝tk+1-tk为离散间隔，运动学模型离散为：
[0223] xk+1＝Akxk+Bkuk+wk
[0224] 其中：Ak为运动学方程，满足：
[0225]
[0226] Bk为控制方程，满足：
[0227]
[0228] uk为控制向量，满足uk＝[vwx,k vwy,k]T，为已知量；
[0229] wk过程噪声向量，满足wk＝[ωx,k ωy,k ωcx,k ωcy,k]T，对应各个状态变量的不确定性；将系统状态xk,yk,vcx,k,vcy,k的过程噪声建模为零均值Gauss分布，其协方差矩阵满足：
[0230]
[0231] 其中，σw为所述水下航行器对水速度观测不确定性的标准差；σc为海流不确定性的标准差；
[0232] S2.观测模型离散化；
[0233] 所述水下航行器在k-1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：
[0234]
[0235] 其中，νt,k为观测噪声，假设其满足方差为Rt,k的Gauss分布；考虑到有效声速ve,k的时变未知性，将ve,k也看作随机变量；离散形式的观测方程写作：
[0236] mk＝hk(xk,ve,k)，
[0237]
[0238] 由于海流观测
采样频率较高，假设在每一个离散时间点k处均可以得到海流观测，故离散后的海流观测方程为：
[0239] mvc，k＝Hkxk+νvc,k
[0240] 其中，Hk为k时刻海流速度观测矩阵，满足：
[0241]
[0242] νvc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：
[0243]
[0244] 其中，σvc,m为海流速度观测噪声的标准差；
[0245] D.建立有效声速及其不确定性统计参数的随机模型以及函数模型；
[0246] 有效声速及其不确定性统计参数的随机模型以及函数模型建立方法为：
[0247] 将有效声速初始先验分布建模为Gauss分布：
[0248]
[0249] 其中：N(x|μ,Σ)表示以μ为均值，Σ为协方差矩阵，满足Gauss分布的随机向量x；为初始声速估计均值；Pe,0为初始声速不确定性方差；
[0250] 将k-1时刻有效声速的后验分布也建模为Gauss分布：
[0251]
[0252] 分别为k-1时刻有效声速的后验均值与方差；
[0253] 有效声速的动态扩散的函数模型记作：
[0254] ve,k＝ve,k-1+ωe,k-1
[0255] 其中：ωe,k-1为有效声速的不确定性，将其建模为均值及方差均未知的Gauss分布：
[0256]
[0257] 其中：μk-1与σe,k-1分别为声速不确定性的均值与标准差；
[0258] 据此，可以将有效声速的概率扩散方程记作：
[0259]
[0260] 结合上式，获得有效声速的预测模型为：
[0261]
[0262] 其中 分别为k时刻有效声速的先验均值与方差，其计算公式为：
[0263]
[0264]
[0265] 由于真实的有效声速的不确定性均值和方差未知，以其名义值 与 进行声速预测，预测方程为：
[0266]
[0267]
[0268] 其中： 与 表示有效声速先验统计参数(均值与方差)的名义值；
[0269] 通过在线估计Pe,k|k-1来补偿σe,k-1设置误差的影响；将μk-1与Pe,k|k-1的先验分布建模为其共轭先验，即Gauss-inverse Gama(GIG)分布：
[0270]
[0271] 其中： 表示μk-1与Pe,k|k-1的先验统计参数，GIG(a,A|τ,α,λ,ν) 表示以τ,α,λ,ν为参数的GIG分布，可以被分解为：
[0272] GIG(a,A|τ,α,λ,ν)＝N(a|τ,αA)IG(A|λ,ν)
[0273] 其中：IG(A|λ,ν)表示以λ,ν为参数的逆Gama分布；
[0274] μk-1与Pe,k|k-1的先验分布可以被分解为：
[0275]
[0276]
[0277] μk-1与Pe,k|k-1的先验估计需要与其名义值匹配，即μk-1与Pe,k|k-1的先验统计参数满足：
[0278]
[0279]
[0280] 可设计μk-1与Pe,k|k-1统计参数预测方程为：
[0281]
[0282]
[0283]
[0284]
[0285] 其中：ρα与ρλ为调制参数；
[0286] E.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知；所述水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，同时进行有效声速随机模型参数预测；所述水下航行器在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取所述螺旋桨转速信息及所述电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；
[0287] 所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：
[0288] 根据Kalman滤波的预测环节，将系统状态的先验分布近似为Gauss分布：
[0289]
[0290] 其中： 与Pk|k-1分别为k时刻的先验状态和先验方差，其计算方法为：
[0291]
[0292]
[0293] 其中： 与Pk-1|k-1分别为k-1时刻的后验状态和后验方差；
[0294] 所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：
[0295]
[0296]
[0297] Pk|k＝Pk|k-1-KkHkPk|k-1
[0298] 其中：Kk为海流更新Kalman增益；
[0299] F.所述水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速及其不确定性统计参数的未知性，以此基于扩展Kalman 滤波算法及变分贝叶斯近似，以水声信号传递时间为观测变量，进行所述水下航行器的位置更新；
[0300] 通过扩展Kalman滤波及变分贝叶斯近似进行水声信号传递时间校正的方法为：
[0301] S1.建立观测似然函数、有效声速后验分布及其不确定性统计参数后验概率密度分布模型；
[0302] 根据离散形式的水声信号传递时间观测模型，得到水声信号传递时间的观测似然密度为：
[0303] p(mk|ve,k,xk)＝N(mk|hk(xk,ve,k),Rk)
[0304] 将有效声速后验分布建模为Gauss分布，即：
[0305]
[0306] μk-1与Pe,k|k-1的后验分布同样建模为GIG分布，分解为：
[0307]
[0308]
[0309] S2.定义状态变量、有效声速及声速不确定性统计参数后验估计近似值；
[0310] 通过变分贝叶斯近似，将状态变量、有效声速及声速不确定性统计参数的联合后验估计近似为：
[0311] p(xk,ve,k,μk-1,Pe,k|k-1|m1:k)≈q(xk)q(ve,k)q(μk-1)q(Pe,k|k-1)
[0312] 以最小化近似前与近似后两个概率密度函数之间的Kullback-Leibler散度(KLD)最小化为目标，得到近似解为：
[0313]
[0314] 其中：log(·)表示对数运算；θ表示xk,ve,k,μk-1,Pe,k|k-1中的任意元素；Ex[·]表示相对于x的期望；上标(-θ)表示整个集合当中除了θ以外的其它元素；cθ表示与θ无关的常数；采用固定点迭代来求解q(θ)；
[0315] S3.求解状态变量、有效声速及声速不确定性统计参数后验估计近似值；
[0316] S3.1求解联合概率密度对数值；
[0317]
[0318] 其对数形式表示为：
[0319]
[0320] S3.2求解声速不确定性统计参数Pe,k|k-1估计值；
[0321] 令θ＝Pe,k|k-1，得到：
[0322]
[0323] 其中：
[0324]
[0325]
[0326] 变量加上标(i)表示该变量在第i次迭代中的估计值；进而得到：
[0327]
[0328]
[0329] 取Pe,k|k-1在第i+1次迭代中的估计值为：
[0330]
[0331] S3.3求解声速不确定性统计参数μk-1估计值；
[0332] 令θ＝μk-1，得到：
[0333]
[0334] 进而得到：
[0335]
[0336]
[0337] 取μk-1在第i+1次迭代中的估计值为其期望值，即：
[0338]
[0339] S3.4求解有效声速ve,k估计值；
[0340] 记 选择 为展开点，对非线性的水声信号传递时间观测方程mk＝hk(xk,ve,k)进行线性化，只保留一阶项，得到：
[0341]
[0342] 其中：
[0343]
[0344] 为观测方程的雅克比矩阵；
[0345] 由此得到线性化后的水声信号传递时间观测方程为：
[0346]
[0347] 令θ＝ve,k，得到：
[0348]
[0349] 其中：
[0350]
[0351] 根据Kalman滤波更新式，得到有效声速更新方程为：
[0352]
[0353]
[0354]
[0355] 其中： 为第i+1次迭代中有效声速更新的Kalman增益；
[0356] S3.5求解系统状态xk估计值；
[0357] 以 为展开点，对非线性的水声信号传递时间观测方程mk＝hk(xk,ve,k)进行线性化，只保留一阶项，得到：
[0358]
[0359] 其中：
[0360]
[0361] 为观测方程的雅克比矩阵；
[0362] 由此得到线性化后的水声信号传递时间观测方程为：
[0363]
[0364] 令θ＝xk，得到：
[0365]
[0366] 其中：
[0367]
[0368] 根据Kalman滤波更新式，得到系统状态更新方程为：
[0369]
[0370]
[0371]
[0372] 其中： 为第i+1次迭代中系统状态更新的Kalman增益；
[0373] 记总迭代次数为N，则最终有效声速均值及方差、系统状态均值及协方差矩阵的估计值分别为：
[0374]
[0375]
[0376]
[0377]
[0378] 参数 需要用于k+1时刻的预测过程，记其估计值为
[0379] 实施例2，利用实施例1所述的方法通过试验数据进行验证。
[0380] 作为比较，本实施例同时展示了基于声速不确定性统计参数完全已知以及基于已知定常水声声速的水下单信标定位方法定位结果(分别记作传统方法1与传统方法2，传统方法1 参考文献Z.Zhu and S.L.J.Hu“, Model and algorithm improvement on single beacon underwater tracking,”IEEE Journal of Oceanic Engineering,vol.PP,no.99,pp.1–18,2017.)。试验数据的搜集方法为：水面船搭载GPS、水听器以及罗盘，在水面上进行二维运动。GPS观测到的水面船运动轨迹作为真实参考，水听器接收固定在水底的水声信标所发射的水声信号，并以此获得水声信号传递时间。由于水面船未安装多普勒测速仪，采用GPS轨迹进行差分结合电子罗盘测得的艏向角来仿真多普勒测速仪所观测的航行器对地速度。试验当中水声信号发射周期约为30秒(少数几个信号出现观测丢包)，海流观测周期为1秒，离散时间间隔Δt同样设置为1秒。
[0381] 在数值验证的过程中，各种方法调制参数设置为：(1)x与y两个方向位置初始误差均为10米；(2)x与y两个方向海流初始误差均为0.05米/秒；(3)名义声速 为1540米/秒； (4)海流不确定性标准差σc为0.01米/秒；(5)航行器对水速度观测不确定性标准差σw为 0.1米/秒；(6)传统方法1与本发明方法采用的水声声速不确定性的标准差为1米/秒；(7) 水声信号传递时间观测噪声标准差σt,m为0.001秒；(8)海流观测噪声标准差σvc,m为0.01米 /秒；(9)传统方法2中斜距观测噪声标准差σr,m为5米；(10)本发明所提出方法的迭代次数N设置为15，调制参数ρα与ρλ分别为3与2，初始参数α0设置为2。
[0382] 附图2为试验中水面船的真实运动轨迹、本发明的方法与两种传统水下单信标定位方法所估计出的运动轨迹。附图3表示三种方法的水平定位误差随时间变化的曲线，定位误差计算公式为 附图4表示真实声速值、基于声速不确定性统计参数完全已知的水下单信标定位方法(传统方法1)与本发明提出方法的水声声速估计结果。两种传统方法与本发明所提出方法的平均均方定位误差分别为7.19米、13.09米与5.18米。而传统方法1与本发明方法的声速平均均方误差分别为6.87米/秒与3.03米/秒。平均均方定位误差与声速平均均方误差计算公式分别为：
[0383]
[0384]
[0385] 其中T和K分别表示总离散间隔数及总水声信号传递时间采样数。根据图2、3、4以及三种方法的平均均方定位误差、声速平均均方误差，可以看出本发明所提出的方法可以获得比传统水下单信标定位方法更好的结果。相比于基于声速不确定性统计参数完全已知的水下单信标定位方法(传统方法1)，本发明所提出方法可以更好的跟踪水声声速变化的趋势，进而也可以得到更好的定位结果。
[0386] 实施例3，本发明的算法伪代码总结为：
[0387]
[0388] 虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些
修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。
