CN113093092A - 一种水下鲁棒自适应单信标定位方法 - Google Patents
一种水下鲁棒自适应单信标定位方法 Download PDFInfo
- Publication number
- CN113093092A CN113093092A CN202110358029.7A CN202110358029A CN113093092A CN 113093092 A CN113093092 A CN 113093092A CN 202110358029 A CN202110358029 A CN 202110358029A CN 113093092 A CN113093092 A CN 113093092A
- Authority
- CN
- China
- Prior art keywords
- distribution
- parameter
- state
- underwater
- prior
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 102
- 238000009826 distribution Methods 0.000 claims abstract description 239
- 239000011159 matrix material Substances 0.000 claims abstract description 137
- 230000005236 sound signal Effects 0.000 claims abstract description 61
- 230000005540 biological transmission Effects 0.000 claims abstract description 38
- 230000008569 process Effects 0.000 claims abstract description 35
- 230000008054 signal transmission Effects 0.000 claims abstract description 25
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 23
- 238000004364 calculation method Methods 0.000 claims description 21
- BULVZWIRKLYCBC-UHFFFAOYSA-N phorate Chemical compound CCOP(=S)(OCC)SCSCC BULVZWIRKLYCBC-UHFFFAOYSA-N 0.000 claims description 15
- OKTJSMMVPCPJKN-UHFFFAOYSA-N Carbon Chemical compound [C] OKTJSMMVPCPJKN-UHFFFAOYSA-N 0.000 claims description 12
- 230000003044 adaptive effect Effects 0.000 claims description 7
- 238000000342 Monte Carlo simulation Methods 0.000 description 8
- 238000004088 simulation Methods 0.000 description 6
- 230000003416 augmentation Effects 0.000 description 5
- 238000012795 verification Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002238 attenuated effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S1/00—Beacons or beacon systems transmitting signals having a characteristic or characteristics capable of being detected by non-directional receivers and defining directions, positions, or position lines fixed relatively to the beacon transmitters; Receivers co-operating therewith
- G01S1/72—Beacons or beacon systems transmitting signals having a characteristic or characteristics capable of being detected by non-directional receivers and defining directions, positions, or position lines fixed relatively to the beacon transmitters; Receivers co-operating therewith using ultrasonic, sonic or infrasonic waves
- G01S1/76—Systems for determining direction or position line
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Acoustics & Sound (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明涉及一种单信标定位方法，首先建立运动学模型及观测模型；将位置状态及参数状态先验分布建模为Student’s t分布与高斯分布；将海流观测噪声与水声信号传递时间观测噪声建模为Student’s t分布；将所有的高斯分布与Student’s t分布的方差矩阵、尺度矩阵和自由度参数看作随机变量，先验分布建模为其对应的共轭先验；将所有的Student’s t分布分解为分层高斯的形式。每一个时间历元采用序贯更新的方式：得到位置状态的预测值；得到参数状态的预测值；对噪声参数的超参数进行预测；进行海流速度观测相关变量的更新；进行水声信号传递时间相关变量的更新。本发明可以同时处理实际水下单信标定位系统当中常见的观测野值以及噪声统计参数的时变未知性。
Description
技术领域
本发明属于水下定位领域，特别涉及一种单信标定位方法。
背景技术
精确的位置反馈是水下航行器完成既定水下任务的基础。由于水下电磁波信号衰减较快，广泛应用于陆地与天空定位的GNSS系统在水下无法应用。目前常用于水下的定位方式包括惯性导航、航位推算、长基线定位系统以及超短基线定位系统。其中惯性导航以及航位推算的定位误差会随时间累计，无法用于长时间水下定位。而长基线定位系统以及超短基线定位系统成本通常较高，这限制了其在水下航行器中的应用。水下单信标定位系统融合了航位推算数据与单个水声信标提供的水声信号传递时间信息，可以得到比常规的惯性导航以及航位推算更好的定位精度，且成本低于长基线定位以及超短基线定位。目前的单信标定位系统多基于非线性Kalman滤波器来融合水声信号传递时间信息以及航位推算信息，进行航行器的位置解算。但常规的非线性Kalman滤波器仅仅在状态空间模型的过程噪声及观测噪声为高斯分布且噪声统计参数完全已知时才可保证其性能。实际的水下定位应用中，多普勒测速仪(DVL) 以及电子罗盘会存在观测野值，这会导致航行器测得的自身与大地的相对速度以及自身与水的相对速度存在野值，也会进一步导致海流的观测出现野值。由于水下环境的恶劣性，水声信号传递时间也会存在观测野值。这些观测野值会使得单信标定位系统的位置状态过程噪声、海流观测噪声以及水声信号传递时间观测噪声呈现非高斯性，具体来说，噪声会呈现厚尾特性。此外，在实际的应用当中，模型过程噪声以及观测噪声的统计参数难以准确获得，且航行器运动特性以及水下环境的改变还会使得噪声的统计参数随时间变化，这进一步增加了准确获得噪声统计参数的难度。传感器的观测野值以及噪声统计参数设置的不准确会恶化定位系统的性能，甚至会导致滤波发散，这会影响基于非线性Kalman滤波器的单信标定位系统在实际水下定位当中的应用。
发明内容
本发明的目的是：针对于实际水下单信标定位系统当中出现的DVL、电子罗盘、水声信号传递时间观测野值以及模型噪声统计参数的时变未知性，将位置状态过程噪声、海流观测噪声以及水声信号传递时间观测噪声建模为Student’s t分布，且将所有的模型噪声方差矩阵、尺度矩阵、自由度参数均看作未知的。在变分贝叶斯近似的框架下对未知变量及参数进行迭代求解，提出相应的水下鲁棒自适应单信标定位方法。
本发明的技术方案是：首先根据航行器位置、海流速度以及有效声速不确定性噪声特性的不同，将系统状态分为位置状态以及参数状态，建立离散形式的运动学模型以及观测模型；进而，分别将位置状态以及参数状态先验分布建模为Student’s t分布与高斯分布；同时考虑到观测野值，将海流观测噪声与水声信号传递时间观测噪声建模为Student’st 分布；将所有的高斯分布与Student’s t分布的方差矩阵、尺度矩阵和自由度参数看作随机变量，其先验分布建模为其对应的共轭先验；引入满足Gamma分布的辅助参数，将所有的Student’s t分布分解为分层高斯的形式。水声信标周期性广播水声信号，其位置坐标以及信号发射时间已知。对于每一个时间历元，采用序贯更新的方式，当航行器接收到自身螺旋桨转速提供的航行器与水相对速度以及罗盘提供的航行器艏向角时，进行航位推算，根据位置状态的运动学模型得到位置状态的预测值；根据参数状态的运动学模型得到参数状态的预测值；根据噪声统计参数的慢时变特性，对噪声参数的超参数进行预测；当航行器接收到海流观测时，基于VB近似进行海流速度相关变量的更新；当航行器接收到信标发射的水声信号时，根据信号接收的时刻以及已知的水声信号发射时刻，计算水声信号传递时间，基于VB近似进行水声信号传递时间相关变量的更新。
本发明包括以下步骤：
A.根据航行器位置、海流速度以及有效声速不确定性噪声特性的不同，将系统状态分为位置状态以及参数状态，建立离散形式的运动学模型以及观测模型。
B.分别将位置状态以及参数状态先验分布建模为Student’s t分布与高斯分布；同时考虑到观测野值，将海流观测噪声与水声信号传递时间观测噪声建模为Student’s t分布；将所有的高斯分布与Student’s t分布的方差矩阵、尺度矩阵和自由度参数看作随机变量，其先验分布建模为其对应的共轭先验；引入满足Gamma分布的辅助参数，将所有的Student’s t分布分解为分层高斯的形式。
C.水声信标周期性广播水声信号，其位置坐标以及信号发射时间已知；采用序贯更新的方式，对于每一个时间历元，执行如下步骤：
C1.当航行器接收到自身螺旋桨转速提供的航行器对水速度以及罗盘提供的航行器艏向角时，进行航位推算，根据位置状态的运动学模型得到位置状态的预测值；根据参数状态的运动学模型得到参数状态的预测值；根据噪声统计参数的慢时变特性，对噪声参数的超参数进行预测。
C2.当航行器接收到海流观测时，基于变分贝叶斯，即Variational Bayes，VB近似进行海流速度相关变量的更新。
C3.当航行器接收到信标发射的水声信号时，根据信号接收的时刻以及已知的水声信号发射时刻，计算水声信号传递时间，基于VB近似进行水声信号传递时间相关变量的更新。
上述方案中，所述步骤A采用以下方法：
以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；定义位置状态：
其中：下标k表示变量在第k个时间历元；xk，yk为第k个时间历元水下航行器在水下局部惯性坐标系中的水平位置；
定义参数状态：
其中：vcx,k，vcy,k为第k个时间历元x与y方向的海流速度；ve,k表示第k个时间历元的水声声速；
考虑水下航行器运动过程中模型噪声影响，建立位置状态的离散形式运动学模型为：
pk+1＝pk+Δt(vw,k+Hcxk)+ωp,k
其中：Δt为离散时间间隔，vw,k＝[vwx,k vwy,k]T为航行器与水的相对速度矢量，vwx,k与vwy,k分别表示航行器与水的相对速度在x与y方向的投影值，
为位置状态过程噪声矢量，Hc表达式为：
将航行器与水的相对速度矢量vw,k看作已知的输入，可由下式计算得出：
其中：
根据参数状态的慢时变特性，建立参数状态的离散形式运动学模型为：
xk+1＝xk+ωx,k
其中：
为参数状态过程噪声矢量；
根据多普勒测速仪，即Doppler Velocity Log,DVL测得的随体坐标系下水下航行器的绝对速度
结合电子罗盘测得的艏向角
计算得到水下航行器绝对速度在局部惯性坐标系下的表示
进一步，计算得到海流观测值为：
vcm,k＝vg,k-vw,k
海流观测方程为：
vcm,k＝Hcxk+νc,k；
其中：
为海流观测噪声；
水声信标周期性广播水声信号，水下航行器接收到水声信号后根据信号接收时刻以及水声信号发射时刻计算水声信号传递时间为：
Ttm,k＝Ta,k-Te,k
其中：Ttm,k为水声信号传递时间，Ta,k为水下航行器测得的水声信号接收时刻，Te,k为已知的水声信号发射时刻；
水声信号传递时间的观测方程为：
Ttm,k＝hk(pk,xk)+νt,k
其中：
其中：bk＝[bx,k by,k]T为已知的水声信标位置坐标，zk与bz,k分别为水下航行器与水声信标的深度坐标，假设其均由深度计精确测得，为已知量，xk(3)表示矢量xk的第三个维度值，νt,k为水声信号传递时间的观测噪声。
上述方案中，所述步骤B采用以下方法：
由于实际应用中电子罗盘的观测会出现野值，导致位置过程噪声矢量ωp,k呈现厚尾特性，进而会导致状态估计的过程中位置状态先验分布呈现厚尾特性，故将位置状态先验分布建模为Student’s t分布：
其中：St(x；μ,Σ,ω)表示以μ为均值向量、Σ为尺度矩阵、ω为自由度的满足Student’t分布的随机向量x；m1:k表示从第1个到第k个时间历元的观测值集合；下标i|j代表以第j时刻及第j时刻之前的系统观测为条件的第i时刻的变量估计值；
表示k时刻位置状态的先验均值；Pp,k|k-1表示k时刻位置状态的先验尺度矩阵；ηp,k表示k时刻位置状态的先验自由度；
将参数状态的先验分布建模为高斯分布，即：
其中：N(x；a,A)表示随机向量x满足以a为均值，以A为方差矩阵的高斯分布；
表示k时刻参数状态的先验均值；Px,k|k-1示k时刻参数状态的先验方差矩阵；
考虑到DVL及电子罗盘的观测野值，海流观测噪声通常为厚尾的，因此将海流观测噪声建模为均值为0的Student’s t分布：
p(νc,k)＝St(νc,k；0,Rc,k,ηc,k)
其中：Rc,k与ηc,k为对应的尺度矩阵与自由度；
考虑到水下环境的恶劣性，水声信号传递时间观测噪声通常也为厚尾的，因此将水声信号传递时间观测噪声建模为均值为0的Student’s t分布：
p(νt,k)＝St(νt,k；0,Rt,k,ηt,k)
其中：Rt,k与ηt,k为对应的尺度矩阵与自由度；
考虑到实际应用过程中噪声参数的未知性，将先验分布与观测噪声的方差矩阵、尺度矩阵以及自由度参数均看作随机变量，其先验分布建模为对应的共轭先验，即k时刻位置状态的先验尺度矩阵Pp,k|k-1的先验分布建模为逆Wishart分布：
其中：IW(X；a,B)表示随机矩阵X满足以a为自由度，以B为逆尺度矩阵的逆Wishart分布；
与
为对应的超参数；
第k个时间历元位置状态的先验自由度ηp,k的先验分布建模为Gamma分布：
其中：Gamma(x；a,b)表示以a为形状参数，b为速率参数的满足Gamma分布的随机变量x；
与
为对应的超参数；
第k个时间历元参数状态的先验方差矩阵Px,k|k-1的先验分布建模为逆Wishart分布：
其中：
与
为对应的超参数；
第k个时间历元海流观测噪声尺度矩阵Rc,k与水声信号传递时间观测噪声尺度矩阵Rt,k先验分布均建模为逆Wishart分布：
其中：
与
为对应的超参数；
第k个时间历元海流观测噪声自由度ηc,k与水声信号传递时间观测噪声自由度ηt,k先验分布均建模为Gamma分布：
其中：
与
为对应的超参数；
基于Student’t分布的分层高斯性质，引入满足Gamma分布的辅助参数将其分解为高斯分布与Gamma分布的分层组合，即：
p(λp,k)＝Gamma(λp,k；ηp,k/2,ηp,k/2)
p(νc,k|λc,k)＝N(νc,k；0,Rc,k/λc,k)
p(λc,k)＝Gamma(λc,k；ηc,k/2,ηc,k/2)
p(νt,k|λt,k)＝N(νt,k；0,Rt,k/λt,k)
p(λt,k)＝Gamma(λt,k；ηt,k/2,ηt,k/2)
其中：λp,k,λc,k与λt,k为辅助参数。
上述方案中，所述步骤C1采用以下方法：
根据位置状态的运动学方程，在航行器随体坐标系下与水相对速度
以及艏向角
已知时，计算得到航行器局部惯性坐标系下的与水的相对速度vw,k，进而根据下式进行航位推算：
得到位置状态的先验估计；其中：
为第k个时间历元位置状态的先验估计，
为第k-1个时间历元位置状态的后验估计，
为第k-1个时间历元参数状态的后验估计；定义位置状态的名义过程噪声方差矩阵为
则位置状态的先验方差根据下式计算：
其中：
为第k个时间历元位置状态先验方差的名义值，Pp,k-1|k-1为第k-1个时间历元位置状态的后验方差，Px,k-1|k-1为第k-1个时间历元参数状态的后验方差；根据参数状态的运动学方程，得到参数状态先验估计为：
其中：
为第k个时间历元参数状态的先验估计；定义参数状态的名义过程噪声方差矩阵为
则参数状态的先验方差矩阵根据下式计算：
其中：
为第k个时间历元参数状态先验方差的名义值；
根据先验方差矩阵和尺度矩阵的先验建模，用位置状态的先验尺度矩阵Pp,k|k-1逆的期望值来匹配其方差矩阵名义值的逆，用参数状态的先验方差矩阵Px,k|k-1逆的期望值来匹配其方差矩阵名义值的逆，即：
得出：
其中：ρx及ρp为调制参数；
定义观测噪声尺度矩阵的名义值为
及
根据观测噪声尺度矩阵的先验建模，用尺度矩阵逆的期望值来匹配其名义值的逆，即：
得到：
其中：ρc及ρt为调制参数；
定义三个自由度参数名义值为
根据自由度参数的先验分布，令其期望值等于对应的名义值，即：
得到：
其中：
为调制参数。
上述方案中，所述步骤C2采用以下方法：
需要更新的变量包括参数状态xk，参数状态的先验方差矩阵Px,k|k-1，海流观测噪声尺度矩阵Rc,k，海流观测噪声自由度参数ηc,k，海流观测辅助参数λc,k；由于各个变量相互耦合，需要采用固定点迭代进行近似，记上标(i)表示第i个迭代周期的参数；
S1.更新参数状态xk；
根据VB近似，计算得到第i+1个迭代周期参数状态xk的近似后验分布满足高斯分布，即：
其中：
其中：In表示n维单位矩阵；
S2.更新参数状态的先验方差矩阵Px,k|k-1；
根据VB近似，计算得到第i+1个迭代周期参数状态先验方差矩阵Px,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S3.更新海流观测噪声尺度矩阵Rc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测噪声尺度矩阵Rc,k的近似后验分布满足逆Wishart分布，即：
其中：
进而得到：
S4.更新海流观测噪声自由度参数ηc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测噪声自由度参数ηc,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S5.更新海流观测辅助参数λc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测辅助参数λc,k的近似后验分布满足 Gamma分布，即：
其中：
进而得到：
其中：ψ(·)为digamma函数；
S6.迭代初始化及终止；
在VB迭代开始前需要对参数进行初始化，具体初始化值设置为：
设定迭代次数N，在进行N次迭代后，得到最终的参数状态后验估计及后验方差为：
上述方案中，所述步骤C3采用以下方法：
需要更新的变量包括位置状态pk、参数状态xk、参数状态的先验方差矩阵Px,k|k-1、位置状态的先验分布尺度矩阵Pp,k|k-1、水声信号传递时间观测噪声尺度矩阵Rt,k、位置状态先验分布自由度参数ηp,k、水声信号传递时间观测噪声自由度参数ηt,k、位置状态先验分布辅助参数λp,k、水声信号传递时间观测辅助参数λt,k；由于各个变量相互耦合，需要采用固定点迭代进行近似，同样采用上标(i)表示第i个迭代周期的参数；
S1.更新位置状态pk；
根据VB近似，计算得到第i+1个迭代周期位置状态pk的近似后验分布满足高斯分布，即：
其中：
S2.更新参数状态xk；
根据VB近似，计算得到第i+1个迭代周期参数状态xk的近似后验分布满足高斯分布，即：
其中：
S3.更新参数状态的先验方差矩阵Px,k|k-1；
根据VB近似，计算得到第i+1个迭代周期参数状态先验方差矩阵Px,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S4.位置状态的先验尺度矩阵Pp,k|k-1；
根据VB近似，计算得到第i+1个迭代周期位置状态先验尺度矩阵Pp,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S5.更新海流观测噪声尺度矩阵Rt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声尺度矩阵Rt,k的近似后验分布满足逆Wishart分布，即：
其中：
进而得到：
S6.更新位置状态先验分布自由度参数ηp,k；
根据VB近似，计算得到第i+1个迭代周期位置状态先验分布自由度参数ηp,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S7.更新水声信号传递时间观测噪声自由度参数ηt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声自由度参数ηt,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S8.更新位置状态先验分布辅助参数λp,k；
根据VB近似，计算得到第i+1个迭代周期位置状态先验分布辅助参数λp,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S9.更新水声信号传递时间观测噪声辅助参数λt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声辅助参数λt,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S10.迭代初始化及终止；
在VB迭代开始前需要对参数进行初始化，具体初始化值设置为：
设定迭代次数N，在进行N次迭代后，得到最终的参数状态及位置状态后验估计、后验方差矩阵及后验尺度矩阵为：
有益效果：本发明通过将单信标定位模型中的位置状态过程噪声、海流观测噪声以及水声信号传递时间观测噪声建模为Student’s t分布来处理实际单信标定位系统当中经常出现的观测野值；通过将模型噪声统计参数均看作未知的随机变量来处理单信标定位系统中模型噪声统计参数时变未知的问题；通过VB近似来迭代求解所有的未知变量及参数的近似后验分布；得到相应的水下鲁棒自适应单信标定位方法。所提出的方法可以同时处理实际水下单信标定位系统当中常见的观测野值以及噪声统计参数的时变未知性，具有很好的实际应用潜力。
附图说明
图1为本发明的流程图；
图2为仿真验证所采用的轨迹；
图3为本发明所提出的方法与状态增广法、期望最大化方法、变分贝叶斯近似方法四种方法的位置估计均方根误差比较；
图4为本发明所提出的方法与状态增广法、期望最大化方法、变分贝叶斯近似方法四种方法的海流速度估计均方根误差比较；
图5为本发明所提出的方法与状态增广法、期望最大化方法、变分贝叶斯近似方法四种方法的水声声速估计均方根误差比较。
具体实施方式
实施例1，参见附图1，一种水下鲁棒自适应单信标定位方法，包括以下步骤：
A.根据航行器位置、海流速度以及有效声速不确定性噪声特性的不同，将系统状态分为位置状态以及参数状态，建立离散形式的运动学模型以及观测模型。
以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；定义位置状态：
其中：下标k表示变量在第k个时间历元；xk，yk为第k个时间历元水下航行器在水下局部惯性坐标系中的水平位置；
定义参数状态：
其中：vcx,k，vcy,k为第k个时间历元x与y方向的海流速度；ve,k表示第k个时间历元的水声声速；
考虑水下航行器运动过程中模型噪声影响，建立位置状态的离散形式运动学模型为：
pk+1＝pk+Δt(vw,k+Hcxk)+ωp,k
其中：Δt为离散时间间隔，vw,k＝[vwx,k vwy,k]T为航行器与水的相对速度矢量，vwx,k与vwy,k分别表示航行器与水的相对速度在x与y方向的投影值，
为位置状态过程噪声矢量，Hc表达式为：
将航行器与水的相对速度矢量vw,k看作已知的输入，可由下式计算得出：
其中：
根据参数状态的慢时变特性，建立参数状态的离散形式运动学模型为：
xk+1＝xk+ωx,k
其中：
为参数状态过程噪声矢量；
根据多普勒测速仪，即Doppler Velocity Log,DVL测得的随体坐标系下水下航行器的绝对速度
结合电子罗盘测得的艏向角
计算得到水下航行器绝对速度在局部惯性坐标系下的表示
进一步，计算得到海流观测值为：
vcm,k＝vg,k-vw,k
海流观测方程为：
vcm,k＝Hcxk+νc,k；
其中：
为海流观测噪声；
水声信标周期性广播水声信号，水下航行器接收到水声信号后根据信号接收时刻以及水声信号发射时刻计算水声信号传递时间为：
Ttm,k＝Ta,k-Te,k
其中：Ttm,k为水声信号传递时间，Ta,k为水下航行器测得的水声信号接收时刻，Te,k为已知的水声信号发射时刻；
水声信号传递时间的观测方程为：
Ttm,k＝hk(pk,xk)+νt,k
其中：
其中：bk＝[bx,k by,k]T为已知的水声信标位置坐标，zk与bz,k分别为水下航行器与水声信标的深度坐标，假设其均由深度计精确测得，为已知量，xk(3)表示矢量xk的第三个维度值，νt,k为水声信号传递时间的观测噪声。
B.分别将位置状态以及参数状态先验分布建模为Student’s t分布与高斯分布；同时考虑到观测野值，将海流观测噪声与水声信号传递时间观测噪声建模为Student’s t分布；将所有的高斯分布与Student’s t分布的方差矩阵、尺度矩阵和自由度参数看作随机变量，其先验分布建模为其对应的共轭先验；引入满足Gamma分布的辅助参数，将所有的Student’s t分布分解为分层高斯的形式。
由于实际应用中电子罗盘的观测会出现野值，导致位置过程噪声矢量ωp,k呈现厚尾特性，进而会导致状态估计的过程中位置状态先验分布呈现厚尾特性，故将位置状态先验分布建模为Student’s t分布：
其中：St(x；μ,Σ,ω)表示以μ为均值向量、Σ为尺度矩阵、ω为自由度的满足Student’t分布的随机向量x；m1:k表示从第1个到第k个时间历元的观测值集合；下标i|j代表以第j时刻及第j时刻之前的系统观测为条件的第i时刻的变量估计值；
表示k时刻位置状态的先验均值；Pp,k|k-1表示k时刻位置状态的先验尺度矩阵；ηp,k表示k时刻位置状态的先验自由度；
将参数状态的先验分布建模为高斯分布，即：
其中：N(x；a,A)表示随机向量x满足以a为均值，以A为方差矩阵的高斯分布；
表示k时刻参数状态的先验均值；Px,k|k-1示k时刻参数状态的先验方差矩阵；
考虑到DVL及电子罗盘的观测野值，海流观测噪声通常为厚尾的，因此将海流观测噪声建模为均值为0的Student’s t分布：
p(νc,k)＝St(νc,k；0,Rc,k,ηc,k)
其中：Rc,k与ηc,k为对应的尺度矩阵与自由度；
考虑到水下环境的恶劣性，水声信号传递时间观测噪声通常也为厚尾的，因此将水声信号传递时间观测噪声建模为均值为0的Student’s t分布：
p(νt,k)＝St(νt,k；0,Rt,k,ηt,k)
其中：Rt,k与ηt,k为对应的尺度矩阵与自由度；
考虑到实际应用过程中噪声参数的未知性，将先验分布与观测噪声的方差矩阵、尺度矩阵以及自由度参数均看作随机变量，其先验分布建模为对应的共轭先验，即k时刻位置状态的先验尺度矩阵Pp,k|k-1的先验分布建模为逆Wishart分布：
其中：IW(X；a,B)表示随机矩阵X满足以a为自由度，以B为逆尺度矩阵的逆Wishart分布；
与
为对应的超参数；
第k个时间历元位置状态的先验自由度ηp,k的先验分布建模为Gamma分布：
其中：Gamma(x；a,b)表示以a为形状参数，b为速率参数的满足Gamma分布的随机变量x；
与
为对应的超参数；
第k个时间历元参数状态的先验方差矩阵Px,k|k-1的先验分布建模为逆Wishart分布：
其中：
与
为对应的超参数；
第k个时间历元海流观测噪声尺度矩阵Rc,k与水声信号传递时间观测噪声尺度矩阵Rt,k先验分布均建模为逆Wishart分布：
其中：
与
为对应的超参数；
第k个时间历元海流观测噪声自由度ηc,k与水声信号传递时间观测噪声自由度ηt,k先验分布均建模为Gamma分布：
其中：
与
为对应的超参数；
基于Student’t分布的分层高斯性质，引入满足Gamma分布的辅助参数将其分解为高斯分布与Gamma分布的分层组合，即：
p(λp,k)＝Gamma(λp,k；ηp,k/2,ηp,k/2)
p(νc,k|λc,k)＝N(νc,k；0,Rc,k/λc,k)
p(λc,k)＝Gamma(λc,k；ηc,k/2,ηc,k/2)
p(νt,k|λt,k)＝N(νt,k；0,Rt,k/λt,k)
p(λt,k)＝Gamma(λt,k；ηt,k/2,ηt,k/2)
其中：λp,k,λc,k与λt,k为辅助参数。
C.水声信标周期性广播水声信号，其位置坐标以及信号发射时间已知；采用序贯更新的方式，对于每一个时间历元，执行如下步骤：
C1.当航行器接收到自身螺旋桨转速提供的航行器对水速度以及罗盘提供的航行器艏向角时，进行航位推算，根据位置状态的运动学模型得到位置状态的预测值；根据参数状态的运动学模型得到参数状态的预测值；根据噪声统计参数的慢时变特性，对噪声参数的超参数进行预测。
根据位置状态的运动学方程，在航行器随体坐标系下与水相对速度
以及艏向角
已知时，计算得到航行器局部惯性坐标系下的与水的相对速度vw,k，进而根据下式进行航位推算：
得到位置状态的先验估计；其中：
为第k个时间历元位置状态的先验估计，
为第k-1个时间历元位置状态的后验估计，
为第k-1个时间历元参数状态的后验估计；定义位置状态的名义过程噪声方差矩阵为
则位置状态的先验方差根据下式计算：
其中：
为第k个时间历元位置状态先验方差的名义值，Pp,k-1|k-1为第k-1个时间历元位置状态的后验方差，Px,k-1|k-1为第k-1个时间历元参数状态的后验方差；根据参数状态的运动学方程，得到参数状态先验估计为：
其中：
为第k个时间历元参数状态的先验估计；定义参数状态的名义过程噪声方差矩阵为
则参数状态的先验方差矩阵根据下式计算：
其中：
为第k个时间历元参数状态先验方差的名义值；
根据先验方差矩阵和尺度矩阵的先验建模，用位置状态的先验尺度矩阵Pp,k|k-1逆的期望值来匹配其方差矩阵名义值的逆，用参数状态的先验方差矩阵Px,k|k-1逆的期望值来匹配其方差矩阵名义值的逆，即：
得出：
其中：ρx及ρp为调制参数；
定义观测噪声尺度矩阵的名义值为
及
根据观测噪声尺度矩阵的先验建模，用尺度矩阵逆的期望值来匹配其名义值的逆，即：
得到：
其中：ρc及ρt为调制参数；
定义三个自由度参数名义值为
根据自由度参数的先验分布，令其期望值等于对应的名义值，即：
得到：
其中：
为调制参数。
C2.当航行器接收到海流观测时，基于变分贝叶斯，即Variational Bayes，VB近似进行海流速度相关变量的更新。
需要更新的变量包括参数状态xk，参数状态的先验方差矩阵Px,k|k-1，海流观测噪声尺度矩阵Rc,k，海流观测噪声自由度参数ηc,k，海流观测辅助参数λc,k；由于各个变量相互耦合，需要采用固定点迭代进行近似，记上标(i)表示第i个迭代周期的参数；
S1.更新参数状态xk；
根据VB近似，计算得到第i+1个迭代周期参数状态xk的近似后验分布满足高斯分布，即：
其中：
其中：In表示n维单位矩阵；
S2.更新参数状态的先验方差矩阵Px,k|k-1；
根据VB近似，计算得到第i+1个迭代周期参数状态先验方差矩阵Px,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S3.更新海流观测噪声尺度矩阵Rc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测噪声尺度矩阵Rc,k的近似后验分布满足逆Wishart分布，即：
其中：
进而得到：
S4.更新海流观测噪声自由度参数ηc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测噪声自由度参数ηc,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S5.更新海流观测辅助参数λc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测辅助参数λc,k的近似后验分布满足 Gamma分布，即：
其中：
进而得到：
其中：ψ(·)为digamma函数；
S6.迭代初始化及终止；
在VB迭代开始前需要对参数进行初始化，具体初始化值设置为：
设定迭代次数N，在进行N次迭代后，得到最终的参数状态后验估计及后验方差为：
C3.当航行器接收到信标发射的水声信号时，根据信号接收的时刻以及已知的水声信号发射时刻，计算水声信号传递时间，基于VB近似进行水声信号传递时间相关变量的更新。
需要更新的变量包括位置状态pk、参数状态xk、参数状态的先验方差矩阵Px,k|k-1、位置状态的先验分布尺度矩阵Pp,k|k-1、水声信号传递时间观测噪声尺度矩阵Rt,k、位置状态先验分布自由度参数ηp,k、水声信号传递时间观测噪声自由度参数ηt,k、位置状态先验分布辅助参数λp,k、水声信号传递时间观测辅助参数λt,k；由于各个变量相互耦合，需要采用固定点迭代进行近似，同样采用上标(i)表示第i个迭代周期的参数；
S1.更新位置状态pk；
根据VB近似，计算得到第i+1个迭代周期位置状态pk的近似后验分布满足高斯分布，即：
其中：
S2.更新参数状态xk；
根据VB近似，计算得到第i+1个迭代周期参数状态xk的近似后验分布满足高斯分布，即：
其中：
S3.更新参数状态的先验方差矩阵Px,k|k-1；
根据VB近似，计算得到第i+1个迭代周期参数状态先验方差矩阵Px,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S4.位置状态的先验尺度矩阵Pp,k|k-1；
根据VB近似，计算得到第i+1个迭代周期位置状态先验尺度矩阵Pp,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S5.更新海流观测噪声尺度矩阵Rt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声尺度矩阵Rt,k的近似后验分布满足逆Wishart分布，即：
其中：
进而得到：
S6.更新位置状态先验分布自由度参数ηp,k；
根据VB近似，计算得到第i+1个迭代周期位置状态先验分布自由度参数ηp,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S7.更新水声信号传递时间观测噪声自由度参数ηt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声自由度参数ηt,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S8.更新位置状态先验分布辅助参数λp,k；
根据VB近似，计算得到第i+1个迭代周期位置状态先验分布辅助参数λp,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S9.更新水声信号传递时间观测噪声辅助参数λt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声辅助参数λt,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S10.迭代初始化及终止；
在VB迭代开始前需要对参数进行初始化，具体初始化值设置为：
设定迭代次数N，在进行N次迭代后，得到最终的参数状态及位置状态后验估计、后验方差矩阵及后验尺度矩阵为：
实施例2，实施例1所实现的伪代码为：
实施例3，利用实施例1所述的方法通过仿真数据进行验证。
作为比较，本实施例同时展示了采用状态增广法估计未知有效声速的单信标定位方法(简记作“状态增广法”)、基于期望最大化方法估计未知有效声速的单信标定位方法(简记作“期望最大化方法”)、基于变分贝叶斯近似估计未知有效声速及声速不确定性噪声参数的单信标定位方法(简记作“变分贝叶斯近似方法”)的估计性能。仿真轨迹如图2所示，整个仿真时长为2800秒。在整个仿真过程中，有效声速设置为恒定的1530米/秒，x与y方向的海流速度设置为恒定的0.3米/秒。仿真的传感器采样频率及噪声参数如表1所示。
表1
在数值验证的过程中，滤波器初始参数设置为：(1)x与y两个方向位置初始误差均为 10米；(2)x与y两个方向海流初始误差均为0.5米/秒；(3)有效声速的初始值为1500米/秒；(4)名义过程噪声参数
(5)名义过程噪声参数
(6)名义水声信号传递时间观测噪声参数
(7)名义海流速度观测噪声参数
(8)名义自由度参数
(9)调制参数
ρc＝1，ρt＝1，ρx＝1，ρp＝1；(10)迭代次数N＝15。
20独立的次Monte Carlo仿真的结果被用来验证实施例1所提出的方法。位置、海流速度以及有效声速的均方根误差(RMSE)和平均均方根误差(ARMSE)被用来评价不同方法的性能。评价指标的计算方式如下：
其中：
与
为第i次Monte Carlo仿真中航行器在k时刻真实位置坐标；
与
为第 i次Monte Carlo仿真中航行器在k时刻的位置坐标估计值；
与
为第i次MonteCarlo 仿真中k时刻真实海流速度；
与
为第i次Monte Carlo仿真中k时刻海流速度估计值；
为第i次Monte Carlo仿真中k时刻真实水声声速；
为第i次Monte Carlo仿真中k时刻水声声速估计值；M＝20表示总的Monte Carlo仿真次数；T＝2800秒表示总的仿真时长。
附图3、附图4与附图5分别表示四种定位方法的位置、海流速度以及水声声速均方根误差比较。四种方法的位置、海流速度以及水声声速平均均方根误差比较如表2所示。
表2
根据图3、4、5以及表2，可以看出存在观测野值且模型名义噪声参数设置不准确的情况下，实施例1所提出的方法可以获得比现有的水下单信标定位方法更好的结果，实施例1 所提方法的位置、海流流速以及水声声速估计性能远优于现有的水下单信标定位方法。这表明实施例1所提出的方法对于各种传感器的观测野值均具有较好的鲁棒性，对于未知的模型噪声参数具有较好的自适应能力。
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。
Claims (6)
1.一种水下鲁棒自适应单信标定位方法，其特征在于，包括以下步骤：
A.根据航行器位置、海流速度以及有效声速不确定性噪声特性的不同，将系统状态分为位置状态以及参数状态，建立离散形式的运动学模型以及观测模型；
B.分别将位置状态以及参数状态先验分布建模为Student’s t分布与高斯分布；同时考虑到观测野值，将海流观测噪声与水声信号传递时间观测噪声建模为Student’s t分布；将所有的高斯分布与Student’s t分布的方差矩阵、尺度矩阵和自由度参数看作随机变量，其先验分布建模为其对应的共轭先验；引入满足Gamma分布的辅助参数，将所有的Student’s t分布分解为分层高斯的形式；
C.水声信标周期性广播水声信号，其位置坐标以及信号发射时间已知；采用序贯更新的方式，对于每一个时间历元，执行如下步骤：
C1.当航行器接收到自身螺旋桨转速提供的航行器对水速度以及罗盘提供的航行器艏向角时，进行航位推算，根据位置状态的运动学模型得到位置状态的预测值；根据参数状态的运动学模型得到参数状态的预测值；根据噪声统计参数的慢时变特性，对噪声参数的超参数进行预测；
C2.当航行器接收到海流观测时，基于变分贝叶斯，即Variational Bayes，VB近似进行海流速度相关变量的更新；
C3.当航行器接收到信标发射的水声信号时，根据信号接收的时刻以及已知的水声信号发射时刻，计算水声信号传递时间，基于VB近似进行水声信号传递时间相关变量的更新。
2.如权利要求1所述的一种水下鲁棒自适应单信标定位方法，其特征在于，所述步骤A采用以下方法：
以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；定义位置状态：
其中：下标k表示变量在第k个时间历元；xk，yk为第k个时间历元水下航行器在水下局部惯性坐标系中的水平位置；
定义参数状态：
其中：vcx,k，vcy,k为第k个时间历元x与y方向的海流速度；ve,k表示第k个时间历元的水声声速；
考虑水下航行器运动过程中模型噪声影响，建立位置状态的离散形式运动学模型为：
pk+1＝pk+Δt(vw,k+Hcxk)+ωp,k
其中：Δt为离散时间间隔，vw,k＝[vwx,k vwy,k]T为航行器与水的相对速度矢量，vwx,k与vwy,k分别表示航行器与水的相对速度在x与y方向的投影值，
为位置状态过程噪声矢量，Hc表达式为：
将航行器与水的相对速度矢量vw,k看作已知的输入，可由下式计算得出：
其中：
根据参数状态的慢时变特性，建立参数状态的离散形式运动学模型为：
xk+1＝xk+ωx,k
其中：
为参数状态过程噪声矢量；
根据多普勒测速仪，即Doppler Velocity Log,DVL测得的随体坐标系下水下航行器的绝对速度
结合电子罗盘测得的艏向角
计算得到水下航行器绝对速度在局部惯性坐标系下的表示
进一步，计算得到海流观测值为：
vcm,k＝vg,k-vw,k
海流观测方程为：
vcm,k＝Hcxk+νc,k；
其中：
为海流观测噪声；
水声信标周期性广播水声信号，水下航行器接收到水声信号后根据信号接收时刻以及水声信号发射时刻计算水声信号传递时间为：
Ttm,k＝Ta,k-Te,k
其中：Ttm,k为水声信号传递时间，Ta,k为水下航行器测得的水声信号接收时刻，Te,k为已知的水声信号发射时刻；
水声信号传递时间的观测方程为：
Ttm,k＝hk(pk,xk)+νt,k
其中：
其中：bk＝[bx,k by,k]T为已知的水声信标位置坐标，zk与bz,k分别为水下航行器与水声信标的深度坐标，假设其均由深度计精确测得，为已知量，xk(3)表示矢量xk的第三个维度值，νt,k为水声信号传递时间的观测噪声。
3.如权利要求2所述的一种水下鲁棒自适应单信标定位方法，其特征在于，所述步骤B采用以下方法：
由于实际应用中电子罗盘的观测会出现野值，导致位置过程噪声矢量ωp,k呈现厚尾特性，进而会导致状态估计的过程中位置状态先验分布呈现厚尾特性，故将位置状态先验分布建模为Student’s t分布：
其中：St(x；μ,Σ,ω)表示以μ为均值向量、Σ为尺度矩阵、ω为自由度的满足Student’t分布的随机向量x；m1:k表示从第1个到第k个时间历元的观测值集合；下标i|j代表以第j时刻及第j时刻之前的系统观测为条件的第i时刻的变量估计值；
表示k时刻位置状态的先验均值；Pp,k|k-1表示k时刻位置状态的先验尺度矩阵；ηp,k表示k时刻位置状态的先验自由度；
将参数状态的先验分布建模为高斯分布，即：
其中：N(x；a,A)表示随机向量x满足以a为均值，以A为方差矩阵的高斯分布；
表示k时刻参数状态的先验均值；Px,k|k-1示k时刻参数状态的先验方差矩阵；
考虑到DVL及电子罗盘的观测野值，海流观测噪声通常为厚尾的，因此将海流观测噪声建模为均值为0的Student’s t分布：
p(νc,k)＝St(νc,k；0,Rc,k,ηc,k)
其中：Rc,k与ηc,k为对应的尺度矩阵与自由度；
考虑到水下环境的恶劣性，水声信号传递时间观测噪声通常也为厚尾的，因此将水声信号传递时间观测噪声建模为均值为0的Student’s t分布：
p(νt,k)＝St(νt,k；0,Rt,k,ηt,k)
其中：Rt,k与ηt,k为对应的尺度矩阵与自由度；
考虑到实际应用过程中噪声参数的未知性，将先验分布与观测噪声的方差矩阵、尺度矩阵以及自由度参数均看作随机变量，其先验分布建模为对应的共轭先验，即k时刻位置状态的先验尺度矩阵Pp,k|k-1的先验分布建模为逆Wishart分布：
其中：IW(X；a,B)表示随机矩阵X满足以a为自由度，以B为逆尺度矩阵的逆Wishart分布；
与
为对应的超参数；
第k个时间历元位置状态的先验自由度ηp,k的先验分布建模为Gamma分布：
其中：Gamma(x；a,b)表示以a为形状参数，b为速率参数的满足Gamma分布的随机变量x；
与
为对应的超参数；
第k个时间历元参数状态的先验方差矩阵Px,k|k-1的先验分布建模为逆Wishart分布：
其中：
与
为对应的超参数；
第k个时间历元海流观测噪声尺度矩阵Rc,k与水声信号传递时间观测噪声尺度矩阵Rt,k先验分布均建模为逆Wishart分布：
其中：
与
为对应的超参数；
第k个时间历元海流观测噪声自由度ηc,k与水声信号传递时间观测噪声自由度ηt,k先验分布均建模为Gamma分布：
其中：
与
为对应的超参数；
基于Student’t分布的分层高斯性质，引入满足Gamma分布的辅助参数将其分解为高斯分布与Gamma分布的分层组合，即：
p(λp,k)＝Gamma(λp,k；ηp,k/2,ηp,k/2)
p(νc,k|λc,k)＝N(νc,k；0,Rc,k/λc,k)
p(λc,k)＝Gamma(λc,k；ηc,k/2,ηc,k/2)
p(νt,k|λt,k)＝N(νt,k；0,Rt,k/λt,k)
p(λt,k)＝Gamma(λt,k；ηt,k/2,ηt,k/2)
其中：λp,k,λc,k与λt,k为辅助参数。
4.如权利要求3所述的一种水下鲁棒自适应单信标定位方法，其特征在于，所述步骤C1采用以下方法：
根据位置状态的运动学方程，在航行器随体坐标系下与水相对速度
以及艏向角
已知时，计算得到航行器局部惯性坐标系下的与水的相对速度vw,k，进而根据下式进行航位推算：
得到位置状态的先验估计；其中：
为第k个时间历元位置状态的先验估计，
为第k-1个时间历元位置状态的后验估计，
为第k-1个时间历元参数状态的后验估计；定义位置状态的名义过程噪声方差矩阵为
则位置状态的先验方差根据下式计算：
其中：
为第k个时间历元位置状态先验方差的名义值，Pp,k-1|k-1为第k-1个时间历元位置状态的后验方差，Px,k-1|k-1为第k-1个时间历元参数状态的后验方差；根据参数状态的运动学方程，得到参数状态先验估计为：
其中：
为第k个时间历元参数状态的先验估计；定义参数状态的名义过程噪声方差矩阵为
则参数状态的先验方差矩阵根据下式计算：
其中：
为第k个时间历元参数状态先验方差的名义值；
根据先验方差矩阵和尺度矩阵的先验建模，用位置状态的先验尺度矩阵Pp,k|k-1逆的期望值来匹配其方差矩阵名义值的逆，用参数状态的先验方差矩阵Px,k|k-1逆的期望值来匹配其方差矩阵名义值的逆，即：
得出：
其中：ρx及ρp为调制参数；
定义观测噪声尺度矩阵的名义值为
及
根据观测噪声尺度矩阵的先验建模，用尺度矩阵逆的期望值来匹配其名义值的逆，即：
得到：
其中：ρc及ρt为调制参数；
定义三个自由度参数名义值为
根据自由度参数的先验分布，令其期望值等于对应的名义值，即：
得到：
其中：
为调制参数。
5.如权利要求4所述的一种水下鲁棒自适应单信标定位方法，其特征在于，所述步骤C2采用以下方法：
需要更新的变量包括参数状态xk，参数状态的先验方差矩阵Px,k|k-1，海流观测噪声尺度矩阵Rc,k，海流观测噪声自由度参数ηc,k，海流观测辅助参数λc,k；由于各个变量相互耦合，需要采用固定点迭代进行近似，记上标(i)表示第i个迭代周期的参数；
S1.更新参数状态xk；
根据VB近似，计算得到第i+1个迭代周期参数状态xk的近似后验分布满足高斯分布，即：
其中：
其中：In表示n维单位矩阵；
S2.更新参数状态的先验方差矩阵Px,k|k-1；
根据VB近似，计算得到第i+1个迭代周期参数状态先验方差矩阵Px,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S3.更新海流观测噪声尺度矩阵Rc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测噪声尺度矩阵Rc,k的近似后验分布满足逆Wishart分布，即：
其中：
进而得到：
S4.更新海流观测噪声自由度参数ηc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测噪声自由度参数ηc,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S5.更新海流观测辅助参数λc,k；
根据VB近似，计算得到第i+1个迭代周期海流观测辅助参数λc,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
其中：ψ(·)为digamma函数；
S6.迭代初始化及终止；
在VB迭代开始前需要对参数进行初始化，具体初始化值设置为：
设定迭代次数N，在进行N次迭代后，得到最终的参数状态后验估计及后验方差为：
6.如权利要求5所述的一种水下鲁棒自适应单信标定位方法，其特征在于，所述步骤C3采用以下方法：
需要更新的变量包括位置状态pk、参数状态xk、参数状态的先验方差矩阵Px,k|k-1、位置状态的先验分布尺度矩阵Pp,k|k-1、水声信号传递时间观测噪声尺度矩阵Rt,k、位置状态先验分布自由度参数ηp,k、水声信号传递时间观测噪声自由度参数ηt,k、位置状态先验分布辅助参数λp,k、水声信号传递时间观测辅助参数λt,k；由于各个变量相互耦合，需要采用固定点迭代进行近似，同样采用上标(i)表示第i个迭代周期的参数；
S1.更新位置状态pk；
根据VB近似，计算得到第i+1个迭代周期位置状态pk的近似后验分布满足高斯分布，即：
其中：
S2.更新参数状态xk；
根据VB近似，计算得到第i+1个迭代周期参数状态xk的近似后验分布满足高斯分布，即：
其中：
S3.更新参数状态的先验方差矩阵Px,k|k-1；
根据VB近似，计算得到第i+1个迭代周期参数状态先验方差矩阵Px,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S4.位置状态的先验尺度矩阵Pp,k|k-1；
根据VB近似，计算得到第i+1个迭代周期位置状态先验尺度矩阵Pp,k|k-1的近似后验分布满足逆Wishart分布，即：
其中：
得到：
S5.更新海流观测噪声尺度矩阵Rt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声尺度矩阵Rt,k的近似后验分布满足逆Wishart分布，即：
其中：
进而得到：
S6.更新位置状态先验分布自由度参数ηp,k；
根据VB近似，计算得到第i+1个迭代周期位置状态先验分布自由度参数ηp,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S7.更新水声信号传递时间观测噪声自由度参数ηt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声自由度参数ηt,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S8.更新位置状态先验分布辅助参数λp,k；
根据VB近似，计算得到第i+1个迭代周期位置状态先验分布辅助参数λp,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S9.更新水声信号传递时间观测噪声辅助参数λt,k；
根据VB近似，计算得到第i+1个迭代周期水声信号传递时间观测噪声辅助参数λt,k的近似后验分布满足Gamma分布，即：
其中：
进而得到：
S10.迭代初始化及终止；
在VB迭代开始前需要对参数进行初始化，具体初始化值设置为：
设定迭代次数N，在进行N次迭代后，得到最终的参数状态及位置状态后验估计、后验方差矩阵及后验尺度矩阵为：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110358029.7A CN113093092B (zh) | 2021-04-01 | 2021-04-01 | 一种水下鲁棒自适应单信标定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110358029.7A CN113093092B (zh) | 2021-04-01 | 2021-04-01 | 一种水下鲁棒自适应单信标定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113093092A true CN113093092A (zh) | 2021-07-09 |
| CN113093092B CN113093092B (zh) | 2022-06-14 |
Family
ID=76673204
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110358029.7A Expired - Fee Related CN113093092B (zh) | 2021-04-01 | 2021-04-01 | 一种水下鲁棒自适应单信标定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113093092B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114430525A (zh) * | 2022-03-15 | 2022-05-03 | 中国矿业大学 | 一种封闭空间基于传感器网络的分布式定位方法 |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1435555A2 (en) * | 2002-12-30 | 2004-07-07 | Samsung Electronics Co., Ltd. | Robot localization system |
| US20110306360A1 (en) * | 2010-06-11 | 2011-12-15 | Skyhook Wireless, Inc. | Systems for and methods of determining likelihood of mobility of reference points in a positioning system |
| US20150248577A1 (en) * | 2012-09-21 | 2015-09-03 | Umwelt (Australia) Pty. Limited | On-ground or near-ground discrete object detection method and system |
| CN106646435A (zh) * | 2017-01-25 | 2017-05-10 | 大连理工大学 | 一种用于实验教学的水下声源系统 |
| US20170245104A1 (en) * | 2015-09-02 | 2017-08-24 | Estimote Polska Sp. Z O. O. | System and method for beacon fleet management |
| CN110646783A (zh) * | 2019-09-30 | 2020-01-03 | 哈尔滨工程大学 | 一种水下航行器的水下信标定位方法 |
| CN110749891A (zh) * | 2019-10-21 | 2020-02-04 | 哈尔滨工程大学 | 一种可估计未知有效声速的自适应水下单信标定位方法 |
| CN110779519A (zh) * | 2019-11-18 | 2020-02-11 | 哈尔滨工程大学 | 一种具有全局收敛性的水下航行器单信标定位方法 |
| CN110779518A (zh) * | 2019-11-18 | 2020-02-11 | 哈尔滨工程大学 | 一种具有全局收敛性的水下航行器单信标定位方法 |
| CN110794409A (zh) * | 2019-10-21 | 2020-02-14 | 哈尔滨工程大学 | 一种可估计未知有效声速的水下单信标定位方法 |
| CN111854760A (zh) * | 2020-07-21 | 2020-10-30 | 广州道源信息科技有限公司 | 一种基于滤波的移动终端位置检测方法 |
| CN111947651A (zh) * | 2020-07-17 | 2020-11-17 | 中国人民解放军海军工程大学 | 水下组合导航信息融合方法、系统及自主式水下航行器 |
-
2021
- 2021-04-01 CN CN202110358029.7A patent/CN113093092B/zh not_active Expired - Fee Related
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1435555A2 (en) * | 2002-12-30 | 2004-07-07 | Samsung Electronics Co., Ltd. | Robot localization system |
| US20110306360A1 (en) * | 2010-06-11 | 2011-12-15 | Skyhook Wireless, Inc. | Systems for and methods of determining likelihood of mobility of reference points in a positioning system |
| US20140045525A1 (en) * | 2010-06-11 | 2014-02-13 | Skyhook Wireless, Inc. | Methods of and systems for measuring beacon stability of wireless access points |
| US20150248577A1 (en) * | 2012-09-21 | 2015-09-03 | Umwelt (Australia) Pty. Limited | On-ground or near-ground discrete object detection method and system |
| US20170245104A1 (en) * | 2015-09-02 | 2017-08-24 | Estimote Polska Sp. Z O. O. | System and method for beacon fleet management |
| CN106646435A (zh) * | 2017-01-25 | 2017-05-10 | 大连理工大学 | 一种用于实验教学的水下声源系统 |
| CN110646783A (zh) * | 2019-09-30 | 2020-01-03 | 哈尔滨工程大学 | 一种水下航行器的水下信标定位方法 |
| CN110749891A (zh) * | 2019-10-21 | 2020-02-04 | 哈尔滨工程大学 | 一种可估计未知有效声速的自适应水下单信标定位方法 |
| CN110794409A (zh) * | 2019-10-21 | 2020-02-14 | 哈尔滨工程大学 | 一种可估计未知有效声速的水下单信标定位方法 |
| CN110779519A (zh) * | 2019-11-18 | 2020-02-11 | 哈尔滨工程大学 | 一种具有全局收敛性的水下航行器单信标定位方法 |
| CN110779518A (zh) * | 2019-11-18 | 2020-02-11 | 哈尔滨工程大学 | 一种具有全局收敛性的水下航行器单信标定位方法 |
| CN111947651A (zh) * | 2020-07-17 | 2020-11-17 | 中国人民解放军海军工程大学 | 水下组合导航信息融合方法、系统及自主式水下航行器 |
| CN111854760A (zh) * | 2020-07-21 | 2020-10-30 | 广州道源信息科技有限公司 | 一种基于滤波的移动终端位置检测方法 |
Non-Patent Citations (4)
| Title |
|---|
| JIAN WANG: "Student’s t-Based Robust Kalman Filter for a SINS/USBL Integration Navigation Strategy", 《IEEE SENSORS JOURNAL》 * |
| QIAN LI: "Robust Student"s t-Based Cooperative Navigation for Autonomous Underwater Vehicles", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》 * |
| ZHONGCHAO DENG: "Adaptive Kalman Filter Based Single Beacon Underwater Tracking With Unknown Effective Sound Velocity", 《2018 IEEE 8TH INTERNATIONAL CONFERENCE ON UNDERWATER SYSTEM TECHNOLOGY: THEORY AND APPLICATIONS (USYS)》 * |
| 刘湘衡: "基于单信标测距辅助 SINS 的中层水域导航算法", 《水下无人系统学报》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114430525A (zh) * | 2022-03-15 | 2022-05-03 | 中国矿业大学 | 一种封闭空间基于传感器网络的分布式定位方法 |
| CN114430525B (zh) * | 2022-03-15 | 2022-10-18 | 中国矿业大学 | 一种封闭空间基于传感器网络的分布式定位方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113093092B (zh) | 2022-06-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110779518B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
| CN110794409B (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
| Hue et al. | Sequential Monte Carlo methods for multiple target tracking and data fusion | |
| CN110749891B (zh) | 一种可估计未知有效声速的自适应水下单信标定位方法 | |
| Yang et al. | Comparison of unscented and extended Kalman filters with application in vehicle navigation | |
| CN110779519B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
| CN110231636B (zh) | Gps与bds双模卫星导航系统的自适应无迹卡尔曼滤波方法 | |
| CN110646783B (zh) | 一种水下航行器的水下信标定位方法 | |
| Jwo et al. | Critical remarks on the linearised and extended Kalman filters with geodetic navigation examples | |
| Nguyen et al. | Algebraic solution for stationary emitter geolocation by a LEO satellite using Doppler frequency measurements | |
| CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
| Yoo et al. | Improvement of terrain referenced navigation using a point mass filter with grid adaptation | |
| CN114777812B (zh) | 一种水下组合导航系统行进间对准与姿态估计方法 | |
| CN113093092B (zh) | 一种水下鲁棒自适应单信标定位方法 | |
| Bai et al. | A Robust Generalized $ t $ Distribution-Based Kalman Filter | |
| Liu et al. | A SINS aided correct method for USBL range based on maximum correntropy criterion adaptive filter | |
| KR101502721B1 (ko) | 적응형 상호작용 다중모델 추정기를 이용한 정밀 위치정보 제공 방법 및 장치 | |
| Xia et al. | Extended object tracking using hierarchical truncation measurement model with automotive radar | |
| CN112034445B (zh) | 基于毫米波雷达的车辆运动轨迹跟踪方法和系统 | |
| Dektor et al. | Robust adaptive terrain-relative navigation | |
| CN107886058B (zh) | 噪声相关的两阶段容积Kalman滤波估计方法及系统 | |
| Fernandes et al. | Gnss/mems-ins integration for drone navigation using ekf on lie groups | |
| CN115371705A (zh) | 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法 | |
| CN110716219A (zh) | 一种提高定位解算精度的方法 | |
| Wang et al. | Particle Filter with Hybrid Proposal Distribution for Nonlinear State Estimation. |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20220614 |