CN116680500B - 水下航行器在非高斯噪声干扰下的位置估计方法及*** - Google Patents
水下航行器在非高斯噪声干扰下的位置估计方法及*** Download PDFInfo
- Publication number
- CN116680500B CN116680500B CN202310690292.5A CN202310690292A CN116680500B CN 116680500 B CN116680500 B CN 116680500B CN 202310690292 A CN202310690292 A CN 202310690292A CN 116680500 B CN116680500 B CN 116680500B
- Authority
- CN
- China
- Prior art keywords
- state
- error
- estimation
- underwater vehicle
- expression
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 70
- 238000005259 measurement Methods 0.000 claims abstract description 50
- 239000011159 matrix material Substances 0.000 claims abstract description 46
- 230000003190 augmentative effect Effects 0.000 claims abstract description 19
- 230000003416 augmentation Effects 0.000 claims abstract description 8
- 238000010276 construction Methods 0.000 claims description 25
- 230000008569 process Effects 0.000 claims description 24
- 238000005457 optimization Methods 0.000 claims description 15
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 6
- 230000007423 decrease Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000008045 co-localization Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012938 design process Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000012417 linear regression Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 230000003313 weakening effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Computing Systems (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公开了水下航行器在非高斯噪声干扰下的位置估计方法及***，所述方法包括以下步骤：根据获取的水下航行器的位置数据、速度数据与航向数据，构建协同定位***状态空间模型；在得到基于线性化误差补偿的优化量测方程的基础上，构建增广状态模型；利用增广状态模型，基于student'st核函数和最小误差熵准则，构建代价函数；基于代价函数，求得后验状态估计的加权最小二乘表达形式；基于后验状态估计的加权最小二乘表达形式，使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式，完成水下航行器的位置估计。本发明能够有效削弱非高斯噪声对位置估计的影响，提高水下航行器在非理想作业环境中的定位精度。
Description
技术领域
本发明属于水下航行器协同定位技术领域，具体涉及水下航行器在非高斯噪声干扰下的位置估计方法及***。
背景技术
复杂的水下环境加剧了量测噪声的不确定性。在实际的协同定位中，声源与接收机之间的多径传播会导致距离测量中出现离群值。此外，声速随深度和盐度的变化而变化，DVL水锁效应也会导致测得速度为对水速度而不是对地速度，由此产生的速度不确定性也可能导致异常值的出现。异常值的出现在时间上通常是具有间歇性的，测量异常值可能会引起厚尾分布的非高斯噪声。
由于传统滤波算法是在高斯噪声假设下推导完成的，因此对非高斯噪声不具有鲁棒性。目前，一系列基于优化准则的改进方法被提出，如基于Huber准则的鲁棒优化算法、基于最大互相关熵准则的鲁棒优化算法等，这些改进算法都在一定程度上削弱了非高斯噪声的影响。然而，目前在该类优化算法设计过程中使用的Huber核函数由于缓慢下降，对大异常值的响应不够快；而高斯核函数由于下降速度过快可能导致有效量测数据的损失，且容易导致数值稳定性下降。这些问题的存在使得现有的方法无法更好的应对非高斯噪声的干扰，导致在非高斯噪声下定位精度下降。
发明内容
针对现有技术的不足，本发明提出了水下航行器在非高斯噪声干扰下的位置估计方法及***，能够有效削弱非高斯噪声对位置估计的影响，提高水下航行器在非理想作业环境中的定位精度。
为实现上述目的，本发明提供了如下方案：
水下航行器在非高斯噪声干扰下的位置估计方法，包括以下步骤：
S1：根据获取的位置数据、速度数据与航向数据，构建协同定位***状态空间模型；
S2：基于所述协同定位***状态空间模型，得到线性化误差补偿的优化量测方程，基于所述线性化误差补偿的优化量测方程，构建增广状态模型；
S3：利用所述增广状态模型，基于student's t核函数和最小误差熵准则，构建代价函数；
S4：基于所述代价函数，求得后验状态估计的加权最小二乘表达形式；
S5：基于所述后验状态估计的加权最小二乘表达形式，使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式，完成水下航行器的位置估计。
优选的，所述S1中，所述协同定位***状态空间模型的表达式为：
其中，xk∈Rn×1，zk∈Rm×1，f(·)为状态转移函数，h(·)为量测函数，且E(ωkδl T)＝0；
离散时间模型表达式为：
其中，领航者与跟随者在k时刻的位置坐标分别为xk＝[ak,bk]T，zk为领航者与跟随者之间的相对距离，(·)T为转置操作，Δt为采样周期，ωk-1＝[qx,k-1,qy,k-1]T和δk分别为过程噪声和量测噪声，/>为速度向量，其中/>和/>分别为前向和右向速度，/>为航向。
优选的，所述S2中，基于所述线性化误差补偿的优化量测方程，构建增广状态模型的方法包括：
其中，为状态的一步预测值，Hk为观测矩阵，/>为量测一步预测值，/>为增广噪声向量，包含状态误差向量和量测误向量，即/>其中，vk为线性化过程带来的误差；
增广噪声向量的协方差矩阵表示为：
其中，Σp/k,k-1，Σr/k和Σk分别通过对Υk+Rk，和/>进行Cholesky分解后得到，统计线性化误差vk视为递归模型中的补偿因子，用于补偿线性化过程所带来的误差，
进一步得到：
式中三个量满足其中τk＝[τ1,k,τ2,k,...,τn+m,k]T，ζk＝[ζ1,k,ζ2,k,...,ζn+m,k]T。
优选的，所述S3中，基于student's t核函数和最小误差熵准则，构建代价函数的方法包括：
student's t核函数的表达式具体为：
其中，v和σ是核函数的两个重要参数，σ决定了核带宽度，v用于灵活调节核函数形状，ζi和ζj分别表示第i和第j个误差量；
基于最小误差熵理论和student'st核函数，得到优化代价函数为：
其中，N＝n+m为状态量与观测量的维数之和，n为状态维数，m为量测维数；
优化估计的表达式为：
优选的，所述S4中，基于所述代价函数，求得后验状态估计的加权最小二乘表达形式的方法包括：
对代价函数求偏导并令代价函数的偏导为0，将代价函数的偏导表示为矩阵形式：
通过定点迭代得到状态估计结果，具体表达式为：
其中，
优选的，所述S5中，所述使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式的方法包括：
其中，增益矩阵预测误差协方差矩阵/>和量测误差方差阵/>的表达形式为：
相应的后验状态估计误差协方差矩阵为：
其中，
本发明还提供了水下航行器在非高斯噪声干扰下的位置估计***，包括：第一构建模块、第二构建模块、第三构建模块、加权模块和位置估计模块；
所述第一构建模块用于根据获取的位置数据、速度数据与航向数据，构建协同定位***状态空间模型；
所述第二构建模块用于基于所述协同定位***状态空间模型，得到线性化误差补偿的优化量测方程，基于所述线性化误差补偿的优化量测方程，构建增广状态模型；
所述第三构建模块用于利用所述增广状态模型，基于student's t核函数和最小误差熵准则，构建代价函数；
所述加权模块用于基于所述代价函数，求得后验状态估计的加权最小二乘表达形式；
所述位置估计模块用于基于所述后验状态估计的加权最小二乘表达形式，使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式，完成水下航行器的位置估计。
优选的，所述第一构建模块中，所述协同定位***状态空间模型的表达式为：
其中，xk∈Rn×1，zk∈Rm×1，f(·)为状态转移函数，h(·)为量测函数，且E(ωkδl T)＝0；
离散时间模型表达式为：
其中，领航者与跟随者在k时刻的位置坐标分别为xk＝[ak,bk]T，zk为领航者与跟随者之间的相对距离，(·)T为转置操作，Δt为采样周期，ωk-1＝[qx,k-1,qy,k-1]T和δk分别为过程噪声和量测噪声，/>为速度向量，其中/>和/>分别为前向和右向速度，/>为航向。
优选的，所述第二构建模块中，基于所述线性化误差补偿的优化量测方程，构建增广状态模型的过程包括：
其中，为状态的一步预测值，Hk为观测矩阵，/>为量测一步预测值，/>为增广噪声向量，包含状态误差向量和量测误向量，即/>其中，/>vk为线性化过程带来的误差；
增广噪声向量的协方差矩阵表示为：
其中，Σp/k,k-1，Σr/k和Σk分别通过对Υk+Rk，和/>进行Cholesky分解后得到，统计线性化误差vk视为递归模型中的补偿因子，用于补偿线性化过程所带来的误差，
进一步得到：
式中三个量满足其中τk＝[τ1,k,τ2,k,...,τn+m,k]T，ζk＝[ζ1,k,ζ2,k,...,ζn+m,k]T。
优选的，所述第三构建模块中，基于student's t核函数和最小误差熵准则，构建代价函数的方法包括：
student's t核函数的表达式具体为：
其中，v和σ是核函数的两个重要参数，σ决定了核带宽度，v用于灵活调节核函数形状，ζi和ζj分别表示第i和第j个误差量；
基于最小误差熵理论和student'st核函数，得到优化代价函数为：
其中，N＝n+m为状态量与观测量的维数之和，n为状态维数，m为量测维数；
优化估计的表达式为：
与现有技术相比，本发明的有益效果为：
本发明考虑复杂水下环境中非高斯噪声干扰下的协同定位问题，对非高斯噪声下一种基于student’s t核函数的鲁棒容积卡尔曼滤波方法进行了研究。对于削弱非高斯噪声干扰的问题，现有的基于Huber准则的鲁棒优化算法、基于最大互相关熵准则的鲁棒优化算法等均取得了一定成效，但受核函数下降过于缓慢或下降至0的速度过快的影响，致使此类基于优化准则的改进方法无法更灵活地应对非高斯噪声的干扰，导致在非高斯噪声下定位精度下降。
针对目前现有的鲁棒优化算法受所使用的核函数下降缓慢影响而对大异常值的响应不够快的问题，或核函数下降速度过快可能导致有效量测数据的损失，且容易导致数值稳定性下降的问题，本发明在代价函数构建过程中引入student’s t核函数，结合误差数据使用更充分的最小误差熵准则构建代价函数，有效改善了核函数应对非高斯噪声的灵敏程度，提高了***在非高斯噪声下的位置估计精度。本发明可用于非理想条件下的多潜航器协同定位领域。
附图说明
为了更清楚地说明本发明的技术方案，下面对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例中水下航行器在非高斯噪声干扰下的位置估计方法流程示意图；
图2为本发明实施例中协同定位***实际航行示意图；
图3为本发明实施例中不同方法在非高斯噪声下的定位误差比较图；
图4为本发明实施例中不同方法下的平均定位误差比较图；
图5为本发明实施例中不同参数下的核函数对比图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例一
如图1所示，本发明公开了水下航行器在非高斯噪声干扰下的位置估计方法，包括以下步骤：
S1：根据获取的位置数据、速度数据与航向数据，构建协同定位***状态空间模型；
S2：基于协同定位***状态空间模型，得到线性化误差补偿的优化量测方程，基于线性化误差补偿的优化量测方程，构建增广状态模型；
S3：利用增广状态模型，基于student's t核函数和最小误差熵准则，构建代价函数；
S4：基于代价函数，求得后验状态估计的加权最小二乘表达形式；
S5：基于后验状态估计的加权最小二乘表达形式，使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式，完成水下航行器的位置估计。
在本实施例中，所述S1中，根据获取的位置数据、速度数据与航向数据，构建协同定位***状态空间模型的方法包括：
离散时间模型表达形式如下：
其中领航者与跟随者在k时刻的位置坐标分别为xk＝[ak,bk]T。zk为领航者与跟随者之间的相对距离，(·)T为转置操作，Δt为采样周期，ωk-1＝[qx,k-1,qy,k-1]T和δk分别为过程噪声和量测噪声。/>为速度向量，其中/>和/>分别为前向和右向速度，/>为航向。
***状态空间模型表达形式为：
其中xk∈Rn×1,zk∈Rm×1,f(·)为状态转移函数，h(·)为量测函数,且E(ωkδl T)＝0。
在本实施例中，所述S2中，在得到基于线性化误差补偿的优化量测方程的基础上，构建增广状态模型；
其中为状态的一步预测值，Hk为观测矩阵，/>为量测一步预测值，/>为增广噪声向量，它包含状态误差向量和量测误向量，即/>其中/>vk为线性化过程带来的误差。
增广噪声向量的协方差矩阵表示为
其中Σp/k,k-1，Σr/k和Σk可分别通过对Υk+Rk，和/>进行Cholesky分解后得到。统计线性化误差vk可视为递归模型中的补偿因子，用于补偿线性化过程所带来的误差。
进一步得到：
式中三个量满足其中τk＝[τ1,k,τ2,k,...,τn+m,k]T，ζk＝[ζ1,k,ζ2,k,...,ζn+m,k]T。
在本实施例中，所述S3中，基于student's t核函数和最小误差熵准则完成代价函数构建；
student's t核函数的表达形式具体为：
其中v和σ是核函数的两个重要参数,σ决定了核带宽度，v用于灵活调节核函数形状，ζi和ζj分别表示第i和第j个误差量。
基于最小误差熵理论和student'st核函数，得到优化代价函数为：
其中N＝n+m为状态量与观测量的维数之和，n为状态维数，m为量测维数。
优化估计的表达形式如下：
在本实施例中，所述S4中，求解后验状态估计的加权最小二乘表达形式；
为了获得优化解，对代价函数求偏导并令其为0，且为了简化双重求和的表达形式，将其表示为矩阵形式：
其中
可通过定点迭代得到状态估计结果，具体表现形式如下:
其中
在本实施例中，所述S5中，使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式。
其中增益矩阵预测误差协方差矩阵/>和量测误差方差阵/>的表达形式如下：
相应的后验状态估计误差协方差矩阵为：
其中
图2为协同定位***多潜航器实际航行轨迹图，此处我们考虑2个领航者1个跟随者的经典场景，在协同定位过程中，通过水声设备每隔一段时间测量领航者与跟随者之间的相对距离。两个领航者分别位于随从者两侧，这种队形可以提高***的可观测性。图3为本发明中提出的StKCKF与现有算法的定位误差对比图，可以看出，当初始核带宽选取不合适的情形下，现有算法在测量异常值出现时误差较大，而StKCKF方法由于辅助参数v的使用，可以更加灵活地应对非高斯噪声干扰，保证定位精度。图4为多次蒙特卡洛实验下的平均定位误差对比，可见，由于高斯核函数收敛至0的速度过快，可能造成数值稳定性问题，导致大偏差出现，而StKCKF方法执行稳定，误差始终保持在合理范围内。图5为不同参数下的核函数和不同参数下的定位误差比较图，可以看出，student’s t核函数变化灵活，因此可以在非高斯噪声环境下做出迅速且合理的反应，使得位置估计精度得到有力保证。
实施例二
本发明还提供了水下航行器在非高斯噪声干扰下的位置估计***，包括：第一构建模块、第二构建模块、第三构建模块、加权模块和位置估计模块；
第一构建模块用于根据获取的位置数据、速度数据与航向数据，构建协同定位***状态空间模型；
第二构建模块用于基于协同定位***状态空间模型，得到线性化误差补偿的优化量测方程，基于线性化误差补偿的优化量测方程，构建增广状态模型；
第三构建模块用于利用增广状态模型，基于student's t核函数和最小误差熵准则，构建代价函数；
加权模块用于基于代价函数，求得后验状态估计的加权最小二乘表达形式；
位置估计模块用于基于后验状态估计的加权最小二乘表达形式，使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式，完成水下航行器的位置估计。
在本实施例中，所述第一构建模块中，所述协同定位***状态空间模型的表达式为：
其中，xk∈Rn×1，zk∈Rm×1，f(·)为状态转移函数，h(·)为量测函数，且E(ωkδl T)＝0；
离散时间模型表达式为：
其中，领航者与跟随者在k时刻的位置坐标分别为xk＝[ak,bk]T，zk为领航者与跟随者之间的相对距离，(·)T为转置操作，Δt为采样周期，ωk-1＝[qx,k-1,qy,k-1]T和δk分别为过程噪声和量测噪声，/>为速度向量，其中/>和/>分别为前向和右向速度，/>为航向。
在本实施例中，所述第二构建模块中，基于所述线性化误差补偿的优化量测方程，构建增广状态模型的过程包括：
其中，为状态的一步预测值，Hk为观测矩阵，/>为量测一步预测值，/>为增广噪声向量，包含状态误差向量和量测误向量，即/>其中，vk为线性化过程带来的误差；
增广噪声向量的协方差矩阵表示为：
其中，Σp/k,k-1，Σr/k和Σk分别通过对Υk+Rk，和/>进行Cholesky分解后得到，统计线性化误差vk视为递归模型中的补偿因子，用于补偿线性化过程所带来的误差，
进一步得到：
式中三个量满足其中τk＝[τ1,k,τ2,k,...,τn+m,k]T，ζk＝[ζ1,k,ζ2,k,...,ζn+m,k]T。
在本实施例中，所述第三构建模块中，基于student's t核函数和最小误差熵准则，构建代价函数的方法包括：
student's t核函数的表达式具体为：
其中，v和σ是核函数的两个重要参数，σ决定了核带宽度，v用于灵活调节核函数形状，ζi和ζj分别表示第i和第j个误差量；
基于最小误差熵理论和student'st核函数，得到优化代价函数为：
其中，N＝n+m为状态量与观测量的维数之和，n为状态维数，m为量测维数；
优化估计的表达式为：
在本实施例中，所述加权模块中，基于所述代价函数，求得后验状态估计的加权最小二乘表达形式的过程包括：
对代价函数求偏导并令代价函数的偏导为0，将代价函数的偏导表示为矩阵形式：
通过定点迭代得到状态估计结果，具体表达式为：
其中，/>
在本实施例中，所述位置估计模块中，所述使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式的方法包括：
其中，增益矩阵预测误差协方差矩阵/>和量测误差方差阵/>的表达形式为：
相应的后验状态估计误差协方差矩阵为：
其中，
本发明的主要优点体现在：
本发明采用统计线性回归方法对协同定位***的非线性量测方程进行线性化，并推导出统计线性化误差协方差，在此基础上构造增广模型，削弱了线性化误差的影响；
本发明引入student’s t核函数，使目标函数能够更合理地处理更复杂的非高斯噪声，从而更好地捕捉噪声的厚尾和多峰特性，使得该方法在复杂的实际应用场景中更具适应性；
本发明使用定点迭代法来更新后验状态估计，其收敛性已得到证明，有效的保证了定点迭代输出结果的有效性，从而保证了后验状态估计结果的精度。
以上所述的实施例仅是对本发明优选方式进行的描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。
Claims (6)
1.水下航行器在非高斯噪声干扰下的位置估计方法，其特征在于，包括以下步骤：
S1：根据获取的水下航行器的位置数据、速度数据与航向数据，构建协同定位***状态空间模型；
S2：基于所述协同定位***状态空间模型，得到线性化误差补偿的优化量测方程，基于所述线性化误差补偿的优化量测方程，构建增广状态模型；
S3：利用所述增广状态模型，基于student's t核函数和最小误差熵准则，构建代价函数；
S4：基于所述代价函数，求得后验状态估计的加权最小二乘表达形式；
S5：基于所述后验状态估计的加权最小二乘表达形式，使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式，完成水下航行器的位置估计；
所述S1中，所述协同定位***状态空间模型的表达式为：
其中，xk∈Rn×1，zk∈Rm×1，f(·)为状态转移函数，h(·)为量测函数，且E(ωkδl T)＝0；
离散时间模型表达式为：
其中，领航者与跟随者在k时刻的位置坐标分别为xk＝[ak,bk]T，zk为领航者与跟随者之间的相对距离，(·)T为转置操作，Δt为采样周期，ωk-1＝[qx,k-1,qy,k-1]T和δk分别为过程噪声和量测噪声，/>为速度向量，其中/>和/>分别为前向和右向速度，/>为航向；
所述S3中，基于student's t核函数和最小误差熵准则，构建代价函数的方法包括：
student's t核函数的表达式具体为：
其中，v和σ是核函数的两个重要参数，σ决定了核带宽度，v用于灵活调节核函数形状，ζi和ζj分别表示第i和第j个误差量；
基于最小误差熵理论和student's t核函数，得到优化代价函数为：
其中，N＝n+m为状态量与观测量的维数之和，n为状态维数，m为量测维数；
优化估计的表达式为：
2.根据权利要求1所述的水下航行器在非高斯噪声干扰下的位置估计方法，其特征在于，所述S2中，基于所述线性化误差补偿的优化量测方程，构建增广状态模型的方法包括：
其中，为状态的一步预测值，Hk为观测矩阵，/>为量测一步预测值，/>为增广噪声向量，包含状态误差向量和量测误差向量，即/>其中，vk为线性化过程带来的误差；
增广噪声向量的协方差矩阵表示为：
其中，Σp/k,k-1，Σr/k和Σk分别通过对Υk+Rk，和/>进行Cholesky分解后得到，统计线性化误差vk视为递归模型中的补偿因子，用于补偿线性化过程所带来的误差，
进一步得到：
式中三个量满足其中/> ζk＝[ζ1,k,ζ2,k,...,ζn+m,k]T。
3.根据权利要求1所述的水下航行器在非高斯噪声干扰下的位置估计方法，其特征在于，所述S4中，基于所述代价函数，求得后验状态估计的加权最小二乘表达形式的方法包括：
对代价函数求偏导并令代价函数的偏导为0，将代价函数的偏导表示为矩阵形式：
通过定点迭代得到状态估计结果，具体表达式为：
其中，
4.根据权利要求1所述的水下航行器在非高斯噪声干扰下的位置估计方法，其特征在于，所述S5中，所述使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式的方法包括：
其中，增益矩阵预测误差协方差矩阵/>和量测误差方差矩阵/>的表达形式为：
相应的后验状态估计误差协方差矩阵为：
其中，
5.水下航行器在非高斯噪声干扰下的位置估计***，其特征在于，包括：第一构建模块、第二构建模块、第三构建模块、加权模块和位置估计模块；
所述第一构建模块用于根据获取的位置数据、速度数据与航向数据，构建协同定位***状态空间模型；
所述第二构建模块用于基于所述协同定位***状态空间模型，得到线性化误差补偿的优化量测方程，基于所述线性化误差补偿的优化量测方程，构建增广状态模型；
所述第三构建模块用于利用所述增广状态模型，基于student's t核函数和最小误差熵准则，构建代价函数；
所述加权模块用于基于所述代价函数，求得后验状态估计的加权最小二乘表达形式；
所述位置估计模块用于基于所述后验状态估计的加权最小二乘表达形式，使用矩阵求逆引理得到后验状态估计及相应的估计误差协方差矩阵的最终表达形式，完成水下航行器的位置估计；
所述第一构建模块中，所述协同定位***状态空间模型的表达式为：
其中，xk∈Rn×1，zk∈Rm×1，f(·)为状态转移函数，h(·)为量测函数，且E(ωkδl T)＝0；
离散时间模型表达式为：
其中，领航者与跟随者在k时刻的位置坐标分别为xk＝[ak,bk]T，zk为领航者与跟随者之间的相对距离，(·)T为转置操作，Δt为采样周期，ωk-1＝[qx,k-1,qy,k-1]T和δk分别为过程噪声和量测噪声，/>为速度向量，其中/>和/>分别为前向和右向速度，/>为航向；
所述第三构建模块中，基于student's t核函数和最小误差熵准则，构建代价函数的方法包括：
student's t核函数的表达式具体为：
其中，v和σ是核函数的两个重要参数，σ决定了核带宽度，v用于灵活调节核函数形状，ζi和ζj分别表示第i和第j个误差量；
基于最小误差熵理论和student's t核函数，得到优化代价函数为：
其中，N＝n+m为状态量与观测量的维数之和，n为状态维数，m为量测维数；
优化估计的表达式为：
6.根据权利要求5所述的水下航行器在非高斯噪声干扰下的位置估计***，其特征在于，所述第二构建模块中，基于所述线性化误差补偿的优化量测方程，构建增广状态模型的过程包括：
其中，为状态的一步预测值，Hk为观测矩阵，/>为量测一步预测值，/>为增广噪声向量，包含状态误差向量和量测误差向量，即/>其中，/>vk为线性化过程带来的误差；
增广噪声向量的协方差矩阵表示为：
其中，Σp/k,k-1，Σr/k和Σk分别通过对Υk+Rk，和/>进行Cholesky分解后得到，统计线性化误差vk视为递归模型中的补偿因子，用于补偿线性化过程所带来的误差，
进一步得到：
式中三个量满足其中/> ζk＝[ζ1,k,ζ2,k,...,ζn+m,k]T。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310690292.5A CN116680500B (zh) | 2023-06-12 | 2023-06-12 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310690292.5A CN116680500B (zh) | 2023-06-12 | 2023-06-12 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116680500A CN116680500A (zh) | 2023-09-01 |
| CN116680500B true CN116680500B (zh) | 2024-03-22 |
Family
ID=87780652
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310690292.5A Active CN116680500B (zh) | 2023-06-12 | 2023-06-12 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116680500B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117973429B (zh) * | 2024-04-01 | 2024-06-07 | 南京信息工程大学 | 一种应用于非高斯噪声滤波的模型参数比估计方法 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109459040A (zh) * | 2019-01-14 | 2019-03-12 | 哈尔滨工程大学 | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 |
| CN109974706A (zh) * | 2019-03-08 | 2019-07-05 | 哈尔滨工程大学 | 一种基于双运动模型的主从式多auv协同导航方法 |
| CN110749891A (zh) * | 2019-10-21 | 2020-02-04 | 哈尔滨工程大学 | 一种可估计未知有效声速的自适应水下单信标定位方法 |
| CN113324546A (zh) * | 2021-05-24 | 2021-08-31 | 哈尔滨工程大学 | 罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法 |
| WO2022088797A1 (zh) * | 2020-10-26 | 2022-05-05 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
| JP2022123401A (ja) * | 2021-02-12 | 2022-08-24 | 三菱重工業株式会社 | 経路計画装置、移動体、経路計画方法及びプログラム |
| CN115711622A (zh) * | 2022-11-21 | 2023-02-24 | 电子科技大学 | 一种广义最小误差熵卡尔曼的水下无人航行器定位方法 |
| WO2023045357A1 (zh) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
-
2023
- 2023-06-12 CN CN202310690292.5A patent/CN116680500B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109459040A (zh) * | 2019-01-14 | 2019-03-12 | 哈尔滨工程大学 | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 |
| CN109974706A (zh) * | 2019-03-08 | 2019-07-05 | 哈尔滨工程大学 | 一种基于双运动模型的主从式多auv协同导航方法 |
| CN110749891A (zh) * | 2019-10-21 | 2020-02-04 | 哈尔滨工程大学 | 一种可估计未知有效声速的自适应水下单信标定位方法 |
| WO2022088797A1 (zh) * | 2020-10-26 | 2022-05-05 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
| JP2022123401A (ja) * | 2021-02-12 | 2022-08-24 | 三菱重工業株式会社 | 経路計画装置、移動体、経路計画方法及びプログラム |
| CN113324546A (zh) * | 2021-05-24 | 2021-08-31 | 哈尔滨工程大学 | 罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法 |
| WO2023045357A1 (zh) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
| CN115711622A (zh) * | 2022-11-21 | 2023-02-24 | 电子科技大学 | 一种广义最小误差熵卡尔曼的水下无人航行器定位方法 |
Non-Patent Citations (3)
| Title |
|---|
| Maximum Correntropy Extend Kalman Filter for Multiple AUVs Cooperative Localization;Bo Xu 等;《2019 Chinese control and decision conference(CCDC)》;第4670-4675页 * |
| Student’s t-Kernel-Based Maximum Correntropy Kalman Filter;Hongliang Huang 等;《Sensors》;第1-18页 * |
| 基于四阵元基站的水下航行器自定位方法;翟京生 等;《天津大学学报(自然科学与工程技术版)》;第56卷(第3期);第252-259页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116680500A (zh) | 2023-09-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109459040B (zh) | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 | |
| CN107677272B (zh) | 一种基于非线性信息滤波的auv协同导航方法 | |
| CN116680500B (zh) | 水下航行器在非高斯噪声干扰下的位置估计方法及*** | |
| CN104112079A (zh) | 一种模糊自适应变分贝叶斯无迹卡尔曼滤波方法 | |
| Xu et al. | Cooperative localization in harsh underwater environment based on the MC-ANFIS | |
| CN103217175A (zh) | 一种自适应容积卡尔曼滤波方法 | |
| Jwo et al. | Adaptive Kalman filter for navigation sensor fusion | |
| Bai et al. | A novel mixture distributions-based robust Kalman filter for cooperative localization | |
| CN110794409A (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
| Xu et al. | A novel adaptive filter for cooperative localization under time-varying delay and non-Gaussian noise | |
| CN108226887B (zh) | 一种观测量短暂丢失情况下的水面目标救援状态估计方法 | |
| CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
| CN110555398A (zh) | 一种基于滤波最优平滑确定故障首达时刻的故障诊断方法 | |
| CN111291319B (zh) | 一种应用于非高斯噪声环境下的移动机器人状态估计方法 | |
| Bai et al. | A Robust Generalized $ t $ Distribution-Based Kalman Filter | |
| Han et al. | Precise positioning with machine learning based Kalman filter using GNSS/IMU measurements from android smartphone | |
| JP2002181926A (ja) | 目標追尾方法およびその装置 | |
| CN116772867A (zh) | 一种基于因子图的节点优选的多auv自适应协同定位方法及*** | |
| CN109582914B (zh) | 噪声相关的带偏差***的并行式融合估计方法 | |
| CN116088303B (zh) | 一种不确定复杂动态网络状态时变递推估计方法 | |
| CN115047505B (zh) | 基于载波相位差分辅助的gnss定位方法及导航方法 | |
| Hou et al. | Improved exponential weighted moving average based measurement noise estimation for strapdown inertial navigation system/Doppler velocity log integrated system | |
| Dubois et al. | Performance evaluation of a moving horizon estimator for multi-rate sensor fusion with time-delayed measurements | |
| CN114061592B (zh) | 基于多模型的自适应鲁棒auv导航方法 | |
| CN112946697B (zh) | 一种基于深度学习的卫星信号周跳探测与修复方法 |
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 |