CN107677272A - 一种基于非线性信息滤波的auv协同导航方法 - Google Patents

一种基于非线性信息滤波的auv协同导航方法 Download PDF

Info

Publication number
CN107677272A
CN107677272A CN201710805228.1A CN201710805228A CN107677272A CN 107677272 A CN107677272 A CN 107677272A CN 201710805228 A CN201710805228 A CN 201710805228A CN 107677272 A CN107677272 A CN 107677272A
Authority
CN
China
Prior art keywords
msub
mrow
msup
mtd
mtr
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
Application number
CN201710805228.1A
Other languages
English (en)
Other versions
CN107677272B (zh
Inventor
李宁
张滋
***
张勇刚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201710805228.1A priority Critical patent/CN107677272B/zh
Publication of CN107677272A publication Critical patent/CN107677272A/zh
Application granted granted Critical
Publication of CN107677272B publication Critical patent/CN107677272B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)

Abstract

本发明公开一种基于非线性信息滤波的AUV协同导航方法。该方法中，采用分布式结构的无迹信息滤波器完成协同导航过程中的定位任务。在协同定位过程中，首先建立AUV导航***的状态方程和量测方程；然后采用无迹信息滤波获得主AUV的状态信息，在数据包传递时刻进行状态信息的扩充，并通过无迹信息滤波完成对从AUV状态的估计，在数据包接收时刻对数据包信息进行处理；最后恢复主、从AUV通过信息滤波得到的导航信息。该发明解决了水声通信中信息延迟造成的AUV定位精度低的问题，充分考虑了AUV之间信息传递带来的信息相关性问题，并利用信息边缘化方法解决了该问题，避免了导航信息发散，实现了协同导航的高精度实时定位的目标。

Description

一种基于非线性信息滤波的AUV协同导航方法
技术领域
本发明涉及非线性滤波及协同导航技术领域，具体涉及一种基于非线性信息滤波的AUV协同导航方法。
背景技术
在AUV协同导航技术领域，高精度导航是亟待解决的首要问题。集中式协同导航方法将各个AUV原始的量测信息输送至融合中心处理，在融合中心进行数据融合过程，结构的灵活性差，无法对导航数据进行实时操作，实用性不强，一旦融合中心故障，整个***都会瘫痪。为了解决集中式协同导航的问题，研究人员提出了分布式协同导航结构。分布式协同导航方法将各个AUV的量测信息在各自平台上进行实时处理，并充分利用了AUV之间的距离量测信息，对于实时导航来说是更好的选择。但是，分布式导航方式也面临着几个主要的问题：和陆基导航***相比，水下声学导航受到严重的信息延迟限制。声音在水下的传播速度大约是1500m/s，以公里数量级的长度为尺度来传播数据包便会导致秒数级的延迟。这种延迟在水下通信中是不可避免的，会对协同导航定位的性能产生很大影响；其次，各AUV之前的信息传递使其内部信息具有相关性，在数据处理过程中必须要考虑这个问题。
现对一般的非线性滤波问题的研究相当活跃，常用的有“扩展卡尔曼滤波EKF，不敏卡尔曼滤波UKF，粒子滤波PF等。一般的非线性最优滤波可归结为求条件期望的问题。对于有限多个观测值的情形，条件期望原则上可以用贝叶斯公式来计算。但即使在比较简单的场合，这样得出的结果也是相当繁杂的，无论对实际应用或理论研究都很不方便。与卡尔曼滤波类似，人们也希望能给出非线性滤波的某种递推算法或它所满足的随机微分方程。但一般它们并不存在，因此必须对所讨论的过程X与Y加以适当的限制。非线性滤波的研究工作相当活跃，它涉及随机过程论的许多近代成果，如随机过程一般理论、鞅、随机微分方程、点过程等。其中一个十分重要的问题，是研究在什么条件下，存在一个鞅M，使得在任何时刻,M和Y都包含同样的信息；这样的M称为Y的新息过程。目前对于一类所谓“条件正态过程”，已经给出了非线性最优滤波的可严格实现的递推算式。在实际应用上，对非线性滤波问题往往采用各种线性近似的方法。
目前，人们在分布式协同导航技术领域的研究处于探索阶段，在进行信息传递时，高延迟信号使得AUV无法接收到精确的协同导航信息，对导航精度有很大影响；而忽略导航信息之间的相关性，将会导致长时间导航情况下定位精度严重下降。
为了解决上述问题，本发明提供了一种基于非线性信息滤波的AUV协同导航方法，该方法考虑了AUV信息传递中产生的信息相关性，实时操作性强，可以保证AUV在信息延迟的环境中保持高定位精度。
发明内容
本发明为一种基于非线性信息滤波的AUV协同导航方法首先建立AUV协同导航***的状态模型和量测模型；然后应用非线性信息滤波对主AUV的状态进行估计，在AUV数据包进行传递的时刻对状态向量进行扩充，加入当前时刻的状态信息，在下一时刻传递的数据包信息中，先要去除上一时刻传递的数据包信息，这样就可以最大程度上避免水声信道带宽窄带来的问题；接着对从AUV进行基于非线性信息滤波方法的状态估计，在数据包到达的时刻对数据包信息进行接收处理，提高从AUV导航定位精度；最后，对主、从AUV的信息滤波结果进行数据恢复，得到高精度的导航定位信息。
具体包括以下几个步骤：
(1)建立描述AUV协同导航***的状态方程和量测方程；
(2)对主AUV进行基于非线性信息滤波的状态估计，并在有数据包传递给从AUV的时刻将当前时刻信息保存到状态向量中，在数据包传递结束后对状态向量进行信息边缘化过程；
(3)对从AUV进行基于非线性信息滤波的状态估计，并在有主AUV传递的数据包到达的时刻进行数据接收和处理，在接收到的数据包被处理后对状态向量进行信息边缘化过程；
(4)对主、从AUV的信息滤波状态恢复，得到AUV的导航信息。
其特征在于，所述的步骤(1)具体为：
建立非线性的***模型如下：
其中，状态方程为xk＝f(xk-1)+nk-1，观测方程为zk＝h(xk)+vk，xk为第k时刻的n维状态向量；zk为第k时刻的m维量测向量；f(·)和h(·)为已知的非线性函数；nk-1为第k-1时刻n维***噪声；vk为第k时刻m维观测噪声，假设随机***噪声nk-1～N(0,Qk-1)，q～N(μ,Σ)表示随机向量q服从均值为μ方差为Σ的高斯分布；随机量测噪声vk～N(0,Rk)且nk-1与vk不相关。
其特征在于，所述的步骤(2)具体为：
(2.1)进行一步预测更新：当前时刻主AUV不进行数据包传递时，一步预测不进行状态扩充即不加入当前时刻的状态，假设当前状态如下：
其中，表示k时刻联合状态向量，它有两部分组成，为k时刻状态，为历史时刻状态；
信息滤波将状态重新定义如下：
其中，表示k时刻估计误差协方差，为k时刻信息矩阵，用表示，为k时刻信息向量，用表示，k时刻联合状态矩阵和状态向量表示如下：
其中表示k时刻联合信息矩阵，分别表示k时刻和历史时刻的信息矩阵，均表示k时刻与历史时刻的关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史时刻信息向量；
一步预测结果如下：
其中，表示随机***噪声的协方差，表示非线性函数f(·)伪***矩阵，可定义如下：
其中，表示和的互协方差，在无迹卡尔曼滤波算法中可以用sigma采样点表示如下：
其中，均为采样点，2n为总采样个数；
当前时刻主AUV进行数据包传递时，一步预测进行状态扩充，加入当前的状态，k时刻传递的数据包表示为：
其中，ΛT表示上一数据包传递时刻主AUV的信息矩阵，ηT表示上一数据包传递时刻主AUV的信息向量；
信息传递结束后，对ΛT、ηT及时更新：
将k时刻状态信息扩充到状态向量中，结果如下：
对应的信息矩阵和信息向量如下：
(2.2)量测更新：
其中，表示量测噪声vk的方差，表示k+1时刻的量测向量，代表非线性函数h(·)伪量测矩阵，表示如下：
其中，表示一步预测估计和量测预测的互协方差，表示利用无迹卡尔曼滤波基本方程求得的一步预测误差协方差；
(2.3)边缘化处理：在量测更新完成后，对状态向量进行信息边缘化处理。
其特征在于，所述的步骤(3)具体为：
(3.1)一步预测更新：
其中表示k时刻联合信息矩阵，分别表示k时刻的信息矩阵和历史信息矩阵，均表示k时刻关联信息矩阵与历史关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史信息向量；
一步预测结果表示如下：
其中，表示一步预测信息矩阵，表示一步预测信息向量，表示非线性函数f(·)伪***矩阵，表示随机***噪声的方差；
(3.2)量测更新：当前时刻从AUV未接收到主AUV传递的数据包时，一步预测后不进行数据包处理，直接进行本地更新：
其中，代表非线性函数h(·)伪量测矩阵，表示量测噪声vk的方差，表示k+1时刻的量测向量；当前时刻从AUV接收到从主AUV传递的数据包时，一步预测后进行数据包处理再更新；ΛΔ补零后相加：
距离量测更新如下：
本地量测信息更新如下：
(3.3)边缘化处理：在量测更新完成后，对状态向量进行信息边缘化处理，具体算法同主AUV信息边缘化过程。
本发明的优点在于：
(1)建立了AUV协同导航***模型，利用AUV之间的距离量测信息，充分考虑了延迟问题的存在，给出了一种高精度的基于非线性信息滤波的协同导航方法。
(2)充分考虑了AUV之间信息传递带来的信息相关性问题，并对相关信息进行了边缘化处理，在简化计算复杂度的同时，保证导航定位信息的精确度。
附图说明
图1是本发明的方法流程图；
图2是本发明提供的基于非线性信息滤波方法的主AUV导航***对x坐标轴方向位置估计的均方误差曲线；
图3是本发明提供的基于非线性信息滤波方法的主AUV导航***对y坐标轴方向位置估计的均方误差曲线；
图4是本发明提供的基于非线性信息滤波方法的主AUV导航***对x坐标轴方向速度估计的均方误差曲线；
图5是本发明提供的基于非线性信息滤波方法的主AUV导航***对y坐标轴方向速度估计的均方误差曲线；
图6是本发明提供的基于非线性信息滤波方法的从AUV导航***对x坐标轴方向位置估计的均方误差曲线；
图7是本发明提供的基于非线性信息滤波方法的从AUV导航***对y坐标轴方向位置估计的均方误差曲线；
图8是本发明提供的基于非线性信息滤波方法的从AUV导航***对x坐标轴方向速度估计的均方误差曲线；
图9是本发明提供的基于非线性信息滤波方法的从AUV导航***对y坐标轴方向速度估计的均方误差曲线。
具体实施方式
下面将结合附图和实施例对本发明作进一步的详细说明。
本发明为一种基于非线性信息滤波的AUV协同导航方法，包括以下几个步骤：
(1)建立描述AUV协同导航***的状态方程和量测方程。具体为，建立非线性的***模型如下：
其中，状态方程为xk＝f(xk-1)+nk-1，观测方程为zk＝h(xk)+vk，xk为第k时刻的n维状态向量，表征了AUV的位置信息和速度信息，zk为第k时刻的m维量测向量，表征了对AUV的方位观测信息，f(·)和h(·)为已知的非线性函数，nk-1为第k-1时刻n维***噪声，vk为第k时刻m维观测噪声，假设随机***噪声nk-1～N(0,Qk-1)(q～N(μ,Σ)表示随机向量q服从均值为μ方差为Σ的高斯分布)，随机量测噪声vk～N(0,Rk)，而且nk-1与vk不相关。
(2)对主AUV***进行基于非线性信息滤波的状态估计。
(2.1)一步预测更新
当前时刻主AUV不进行数据包传递时，一步预测不进行状态扩充即不加入当前时刻的状态，具体算法如下：
假设当前状态如下：
其中，表示k时刻联合状态向量，它有两部分组成，为k时刻状态，为历史时刻状态。
信息滤波将状态重新定义如下：
其中，表示k时刻估计误差协方差，为k时刻信息矩阵，用表示，为k时刻信息向量，用表示。则对于k时刻联合状态矩阵和状态向量有如下表示：
其中表示k时刻联合信息矩阵，分别表示k时刻和历史时刻的信息矩阵，均表示k时刻与历史时刻的关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史时刻信息向量。
一步预测结果表示如下：
其中，表示随机***噪声的协方差，表示非线性函数f(·)伪***矩阵，可定义如下：
其中，表示和的互协方差，在无迹卡尔曼滤波算法中可以用sigma采样点表示如下：
其中，均为采样点，2n为总采样个数。
当前时刻主AUV进行数据包传递时，一步预测进行状态扩充，加入当前的状态，具体算法如下：
此时首先要进行数据包传递，将k时刻数据包信息传递给从AUV。数据包传递时，无需传递k时刻的全部信息，只需传递从上一数据包传递时刻到k时刻的增量信息，这样既保证了传递信息的完整性，又对带宽要求较低。
k时刻传递的数据包可以表示为：
其中，ΛT表示上一数据包传递时刻主AUV的信息矩阵，ηT表示上一数据包传递时刻主AUV的信息向量。
信息传递结束后，需要对ΛT、ηT及时更新：
将k时刻状态信息扩充到状态向量中，结果如下：
则其对应的信息矩阵和信息向量如下：
(2.2)量测更新
其中，表示量测噪声vk的方差，表示k+1时刻的量测向量，代表非线性函数h(·)伪量测矩阵，可表示如下：
其中，表示一步预测估计和量测预测的互协方差，表示一步预测误差协方差，均可用无迹卡尔曼滤波基本方程求得。
(2.3)边缘化处理
为了保证主AUV状态向量的维数不至于过高，造成计算困难，在量测更新完成后，对状态向量进行信息边缘化处理，具体算法如下：
情况一：当需要边缘化的状态信息位于信息向量底部位置时：
将β信息进行边缘化结果为：
η(α)＝ηα-ΛαβΛββ -1ηβ (23)
情况二：当需要边缘化的状态信息位于信息向量中部位置时：
将β信息进行边缘化结果为：
(3)对从AUV进行基于信息滤波的状态估计。
(3.1)一步预测更新
当从AUV进行一步预测时，不需要向主AUV传递数据包，所以不需要进行信息扩充，具体算法如下：
其中表示k时刻联合信息矩阵，分别表示k时刻的信息矩阵和历史信息矩阵，均表示k时刻关联信息矩阵与历史关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史信息向量。注意：此时历史信息即为主AUV数据包传递的信息，由于距离量测信息的加入，使得主、从AUV信息具有相关性。此处，即为考虑信息相关性后的算法。
一步预测结果表示如下：
其中，表示一步预测信息矩阵，表示一步预测信息向量，表示非线性函数f(·)伪***矩阵，表示随机***噪声的方差。
(3.2)量测更新
当前时刻从AUV未接收到主AUV传递的数据包时，一步预测后不进行数据包处理，直接进行本地更新，具体算法如下：
其中，代表非线性函数h(·)伪量测矩阵，表示量测噪声vk的方差，表示k+1时刻的量测向量。
当前时刻从AUV接收到从主AUV传递的数据包时，一步预测后进行数据包处理，具体算法如下:
注意此时ΛΔ不可直接相加，需要将矩阵补零再相加，因为传递数据包中不包含最新时刻从AUV的信息，同样，从AUV当前时刻的信息与数据包信息无关。
距离量测更新：
本地量测信息更新
(3.3)边缘化处理
为了保证从AUV状态向量的维数不至于过高，造成计算困难，在量测更新完成后，对状态向量进行信息边缘化处理，具体算法同主AUV信息边缘化过程。
(4)对主、从AUV的信息滤波状态恢复，得到AUV的导航信息。
实施例：在AUV协同导航定位时，水声通信条件是必须考虑的限制因素。由于水下环境复杂，水声通信受限，所以结合实际情况，分布式AUV协同导航方法更满足实际要求。但是，现有的分布式方法也面临着诸多问题。本发明提供的方法旨在解决分布式结构中通信延迟和信息相关性问题，为AUV提供高精度导航信息。下面以具体实施例子来说明本发明的优越性。具体如下：
本例中我们以两个AUV协同导航***为例进行说明，其中一个主AUV，一个从AUV，主AUV可以把自身信息和距离量测数据传递给从AUV，从AUV只有数据包接收能力，不传递信息。
在水下导航***中，AUV的姿态和深度可以分别用相应的传感器测得，并且得到的均为误差有界的导航信息。我们在建模时仅考虑AUV的位置和速度信息，减小状态向量维数，这样更容易满足水下带宽限制要求，状态向量选取如下：
x＝[x y vx vy]T (43)
则建立状态模型和距离量测模型如下：
xk+1＝Fkxk+nk (44)
其中，ΔT为离散模型采样间隔。nk为第k时刻的***噪声，nk～N(0,Qk)，Qk＝diag([10m 10m 0.02m/s 0.02m/s])，Qk表征了***模型的不确定性。
其中，zk表示第k时刻的距离量测信息；和表示当前时刻从AUV的位置信息，和表示当前时刻从AUV接收到的主AUV的导航信息，vk为第k时刻的量测噪声，vk～N(0,Rk)，Rk＝9m，Rk表征了距离量测的不确定性。
初始真实状态值和初始协方差阵设置如下：
其中xs和xc分别表示主AUV和从AUV的初始状态，和分别表示主AUV和从AUV的初始误差协方差值，和表征了目标初始位置的不确定性。
则根据初始状态和协方差设置，可分别计算得到主、从AUV的初始信息矩阵和信息向量，具体结果如下：
实施过程：仿真过程中采用如下定义的均方误差性能指标，比较滤波方法的误差：
其中N为Monte Carlo次数。对AUV导航信息估计的均方误差值越小则表征定位精度越高，效果越好。
仿真时间为1000秒，进行500次Monte Carlo仿真，验证本发明提供高精度定位信息。

Claims (4)

1.一种基于非线性信息滤波的AUV协同导航方法，其特征在于，具体包括以下几个步骤：
(1)建立描述AUV协同导航***的状态方程和量测方程；
(2)对主AUV进行基于非线性信息滤波的状态估计，并在有数据包传递给从AUV的时刻将当前时刻信息保存到状态向量中，在数据包传递结束后对状态向量进行信息边缘化过程；
(3)对从AUV进行基于非线性信息滤波的状态估计，并在有主AUV传递的数据包到达的时刻进行数据接收和处理，在接收到的数据包被处理后对状态向量进行信息边缘化过程；
(4)对主、从AUV的信息滤波状态恢复，得到AUV的导航信息。
2.根据权利要求1所述的一种基于非线性信息滤波的AUV协同导航方法，其特征在于，所述的步骤(1)具体为：
建立非线性的***模型如下：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>f</mi> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> <mo>+</mo> <msub> <mi>n</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>v</mi> <mi>k</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
其中，状态方程为xk＝f(xk-1)+nk-1，观测方程为zk＝h(xk)+vk，xk为第k时刻的n维状态向量；zk为第k时刻的m维量测向量；f(·)和h(·)为已知的非线性函数；nk-1为第k-1时刻n维***噪声；vk为第k时刻m维观测噪声，假设随机***噪声nk-1～N(0,Qk-1)，q～N(μ,Σ)表示随机向量q服从均值为μ方差为Σ的高斯分布；随机量测噪声vk～N(0,Rk)且nk-1与vk不相关。
3.根据权利要求1所述的一种基于非线性信息滤波的AUV协同导航方法，其特征在于，所述的步骤(2)具体为：
(2.1)进行一步预测更新：
当前时刻主AUV不进行数据包传递时，一步预测不进行状态扩充即不加入当前时刻的状态，假设当前状态如下：
<mrow> <msub> <mi>X</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>p</mi> </msub> </msub> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>;</mo> </mrow>
其中，表示k时刻联合状态向量，为k时刻状态，为历史时刻状态；
信息滤波将状态重新定义如下：
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msup> <msub> <mi>P</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msup> <msub> <mi>P</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> </mrow>
其中，表示k时刻估计误差协方差，为k时刻信息矩阵，用表示，为k时刻信息向量，用表示，k时刻联合状态矩阵和状态向量表示如下：
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mi>k</mi> </mrow> </msub> </msub> </mtd> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mi>p</mi> </mrow> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> </mtd> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>p</mi> <mi>p</mi> </mrow> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>p</mi> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> </mrow>
其中表示k时刻联合信息矩阵，分别表示k时刻和历史时刻的信息矩阵，均表示k时刻与历史时刻的关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史时刻信息向量；
一步预测结果如下：
<mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;psi;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> </mtd> <mtd> <mrow> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msup> <msub> <mi>&amp;Omega;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mi>p</mi> </mrow> </msub> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> <msup> <msub> <mi>&amp;Omega;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>p</mi> <mi>p</mi> </mrow> </msub> </msub> <mo>-</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> <msup> <msub> <mi>&amp;Omega;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mi>p</mi> </mrow> </msub> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msup> <msub> <mi>&amp;Omega;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>+</mo> <msub> <mi>&amp;psi;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>(</mo> <mi>f</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>p</mi> </msub> </msub> <mo>-</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> <msup> <msub> <mi>&amp;Omega;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>*</mo> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;psi;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>+</mo> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msup> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mi>k</mi> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;Omega;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>;</mo> </mrow>
<mrow> <msup> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>*</mo> </msup> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>-</mo> <msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>f</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> <mo>-</mo> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示随机***噪声的协方差，表示非线性函数f(·)伪***矩阵，可定义如下：
<mrow> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>;</mo> </mrow>
其中，表示和的互协方差，在无迹卡尔曼滤波算法中可以用sigma采样点表示如下：
<mrow> <msub> <mi>P</mi> <mrow> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> <mi>i</mi> </msubsup> <mo>-</mo> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mi>i</mi> </msubsup> <mo>-</mo> <msub> <mi>x</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>;</mo> </mrow>
其中，均为采样点，2n为总采样个数；
当前时刻主AUV进行数据包传递时，一步预测进行状态扩充，加入当前的状态，k时刻传递的数据包表示为：
<mrow> <msub> <mi>&amp;eta;</mi> <mi>&amp;Delta;</mi> </msub> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>-</mo> <msub> <mi>&amp;eta;</mi> <mi>T</mi> </msub> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;Lambda;</mi> <mi>&amp;Delta;</mi> </msub> <mo>=</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>-</mo> <msub> <mi>&amp;Lambda;</mi> <mi>T</mi> </msub> <mo>;</mo> </mrow>
其中，ΛT表示上一数据包传递时刻主AUV的信息矩阵，ηT表示上一数据包传递时刻主AUV的信息向量；
信息传递结束后，对ΛT、ηT及时更新：
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Lambda;</mi> <mi>T</mi> </msub> <mo>=</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;eta;</mi> <mi>T</mi> </msub> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> </mrow>
将k时刻状态信息扩充到状态向量中，结果如下：
<mrow> <msub> <mi>X</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>x</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>p</mi> </msub> </msub> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>;</mo> </mrow>
对应的信息矩阵和信息向量如下：
<mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mi>k</mi> </mrow> </msub> </msub> </mtd> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mi>p</mi> </mrow> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> </mtd> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>p</mi> <mi>p</mi> </mrow> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>(</mo> <mi>f</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> <mo>-</mo> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <msup> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>(</mo> <mi>f</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> <mo>-</mo> <msub> <mi>F</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>k</mi> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mi>p</mi> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
(2.2)量测更新：
<mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>H</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>H</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>H</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>-</mo> <mi>h</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> <mo>+</mo> <msub> <mi>H</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示量测噪声vk的方差，表示k+1时刻的量测向量，代表非线性函数h(·)伪量测矩阵，表示如下：
<mrow> <msub> <mi>H</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <msub> <mi>s</mi> <mrow> <mi>x</mi> <mi>z</mi> </mrow> </msub> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>;</mo> </mrow>
其中，表示一步预测估计和量测预测的互协方差，表示利用无迹卡尔曼滤波基本方程求得的一步预测误差协方差；
(2.3)边缘化处理：在量测更新完成后，对状态向量进行信息边缘化处理。
4.根据权利要求1所述的一种基于非线性信息滤波的AUV协同导航方法，其特征在于，所述的步骤(3)具体为：
(3.1)一步预测更新：
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mi>k</mi> </mrow> </msub> </msub> </mtd> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mi>p</mi> </mrow> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> </mtd> <mtd> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>p</mi> <mi>p</mi> </mrow> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mi>p</mi> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> </mrow>
其中表示k时刻联合信息矩阵，分别表示k时刻的信息矩阵和历史信息矩阵，均表示k时刻关联信息矩阵与历史关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史信息向量；
一步预测结果表示如下：
<mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;psi;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> </mtd> <mtd> <mrow> <msup> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <msup> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mi>p</mi> </mrow> </msub> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> <msup> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>p</mi> <mi>p</mi> </mrow> </msub> </msub> <mo>-</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> <msup> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mi>p</mi> </mrow> </msub> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <msup> <msub> <mi>&amp;Omega;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>+</mo> <msub> <mi>&amp;psi;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>(</mo> <mi>f</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mi>p</mi> </msub> </msub> <mo>-</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>p</mi> <mi>k</mi> </mrow> </msub> </msub> <msup> <msub> <mi>&amp;Omega;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>*</mo> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;psi;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>+</mo> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <msup> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mi>k</mi> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;Omega;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>;</mo> </mrow>
<mrow> <msup> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>*</mo> </msup> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>-</mo> <msup> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>Q</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>f</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> <mo>-</mo> <msub> <mi>F</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>c</mi> <mi>k</mi> </msub> </msub> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示一步预测信息矩阵，表示一步预测信息向量，表示非线性函数f(·)伪***矩阵，表示随机***噪声的方差；
(3.2)量测更新：
当前时刻从AUV未接收到主AUV传递的数据包时，一步预测后不进行数据包处理，直接进行本地更新：
<mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>H</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>H</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>H</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>-</mo> <mi>h</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> <mo>+</mo> <msub> <mi>H</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，代表非线性函数h(·)伪量测矩阵，表示量测噪声vk的方差，表示k+1时刻的量测向量；当前时刻从AUV接收到从主AUV传递的数据包时，一步预测后进行数据包处理再更新；ΛΔ补零后相加：
<mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msub> <mi>&amp;Lambda;</mi> <mi>&amp;Delta;</mi> </msub> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msub> <mi>&amp;eta;</mi> <mi>&amp;Delta;</mi> </msub> <mo>;</mo> </mrow>
距离量测更新如下：
<mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>n</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>H</mi> <msub> <mi>r</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>H</mi> <msub> <mi>r</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>H</mi> <msub> <mi>r</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <msub> <mi>r</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>-</mo> <mi>h</mi> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> <mo>+</mo> <msub> <mi>H</mi> <msub> <mi>r</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
本地量测信息更新如下：
<mrow> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;Lambda;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>H</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>H</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>;</mo> </mrow>
<mrow> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>=</mo> <msub> <mi>&amp;eta;</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>+</mo> <msup> <msub> <mi>H</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>-</mo> <mi>h</mi> <mo>(</mo> <msub> <mi>x</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> <mo>+</mo> <msub> <mi>H</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msub> <msub> <mi>x</mi> <msub> <mi>c</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </msub> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
(3.3)边缘化处理：在量测更新完成后，对状态向量进行信息边缘化处理，具体算法同主AUV信息边缘化过程。
CN201710805228.1A 2017-09-08 2017-09-08 一种基于非线性信息滤波的auv协同导航方法 Active CN107677272B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710805228.1A CN107677272B (zh) 2017-09-08 2017-09-08 一种基于非线性信息滤波的auv协同导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710805228.1A CN107677272B (zh) 2017-09-08 2017-09-08 一种基于非线性信息滤波的auv协同导航方法

Publications (2)

Publication Number Publication Date
CN107677272A true CN107677272A (zh) 2018-02-09
CN107677272B CN107677272B (zh) 2020-11-10

Family

ID=61134575

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710805228.1A Active CN107677272B (zh) 2017-09-08 2017-09-08 一种基于非线性信息滤波的auv协同导航方法

Country Status (1)

Country Link
CN (1) CN107677272B (zh)

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108489498A (zh) * 2018-06-15 2018-09-04 哈尔滨工程大学 一种基于最大互相关熵无迹粒子滤波的auv协同导航方法
CN108594834A (zh) * 2018-03-23 2018-09-28 哈尔滨工程大学 一种面向未知环境下多auv自适应目标搜索和避障方法
CN108802692A (zh) * 2018-05-25 2018-11-13 哈尔滨工程大学 一种基于最大互相关熵容积粒子滤波的目标跟踪方法
CN108827305A (zh) * 2018-05-25 2018-11-16 哈尔滨工程大学 一种基于鲁棒信息滤波的auv协同导航方法
CN109084767A (zh) * 2018-06-15 2018-12-25 哈尔滨工程大学 一种最大互相关熵自适应容积粒子滤波的auv协同导航方法
CN109931936A (zh) * 2019-03-18 2019-06-25 西北工业大学 一种基于移动中继站的弱连通auv协同导航方法
CN109974706A (zh) * 2019-03-08 2019-07-05 哈尔滨工程大学 一种基于双运动模型的主从式多auv协同导航方法
CN111257913A (zh) * 2019-11-29 2020-06-09 交通运输部长江通信管理局 北斗卫星信号捕获方法以及装置
CN111308576A (zh) * 2019-11-29 2020-06-19 哈尔滨工程大学 一种多auv***及深海珊瑚探测方法
CN112857313A (zh) * 2020-12-31 2021-05-28 哈尔滨工程大学 一种面向低带宽声学信道的测深信息传输方法
CN112945245A (zh) * 2021-02-05 2021-06-11 中国航天空气动力技术研究院 基于条件数理论在多auv协同导航***中可观测性分析方法
CN113074725A (zh) * 2021-05-11 2021-07-06 哈尔滨工程大学 一种基于多源信息融合的小型水下多机器人协同定位方法及***
CN113608169A (zh) * 2021-05-20 2021-11-05 济南大学 一种基于序贯融合算法的多auv协同定位方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103336267A (zh) * 2013-07-02 2013-10-02 哈尔滨工程大学 一种基于水声通信延迟的主从式多uuv协同定位方法
CN103616026A (zh) * 2013-12-17 2014-03-05 哈尔滨工程大学 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法
US8903641B2 (en) * 2012-04-26 2014-12-02 The United States Of America, As Represented By The Secretary Of The Navy Collaborative robot manifold tracker
CN105445722A (zh) * 2015-11-09 2016-03-30 哈尔滨工程大学 一种适用于多auv协同导航的动态条件下水声双程测距误差补偿方法
CN106441300A (zh) * 2016-09-08 2017-02-22 哈尔滨工程大学 一种具有自适应的协同导航滤波方法
CN106525042A (zh) * 2016-09-27 2017-03-22 哈尔滨工程大学 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8903641B2 (en) * 2012-04-26 2014-12-02 The United States Of America, As Represented By The Secretary Of The Navy Collaborative robot manifold tracker
CN103336267A (zh) * 2013-07-02 2013-10-02 哈尔滨工程大学 一种基于水声通信延迟的主从式多uuv协同定位方法
CN103616026A (zh) * 2013-12-17 2014-03-05 哈尔滨工程大学 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法
CN105445722A (zh) * 2015-11-09 2016-03-30 哈尔滨工程大学 一种适用于多auv协同导航的动态条件下水声双程测距误差补偿方法
CN106441300A (zh) * 2016-09-08 2017-02-22 哈尔滨工程大学 一种具有自适应的协同导航滤波方法
CN106525042A (zh) * 2016-09-27 2017-03-22 哈尔滨工程大学 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
YUNXIN ZHAO: "A collaborative control framework with multi-leaders", 《JOURNAL OF THE FRANKLIN INSTITUTE》 *
刘明雍等: "一种基于无迹卡尔曼滤波的UUV协同定位方法", 《鱼雷技术》 *
张福斌等: "一种考虑时钟同步问题的多AUV协同定位算法", 《鱼雷技术》 *
赵辉: "水下航行器组合导航定位***设计研究", 《南通大学学报(自然科学版)》 *

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108594834A (zh) * 2018-03-23 2018-09-28 哈尔滨工程大学 一种面向未知环境下多auv自适应目标搜索和避障方法
CN108594834B (zh) * 2018-03-23 2020-12-22 哈尔滨工程大学 一种面向未知环境下多auv自适应目标搜索和避障方法
CN108802692A (zh) * 2018-05-25 2018-11-13 哈尔滨工程大学 一种基于最大互相关熵容积粒子滤波的目标跟踪方法
CN108827305A (zh) * 2018-05-25 2018-11-16 哈尔滨工程大学 一种基于鲁棒信息滤波的auv协同导航方法
CN108489498A (zh) * 2018-06-15 2018-09-04 哈尔滨工程大学 一种基于最大互相关熵无迹粒子滤波的auv协同导航方法
CN109084767A (zh) * 2018-06-15 2018-12-25 哈尔滨工程大学 一种最大互相关熵自适应容积粒子滤波的auv协同导航方法
CN109974706A (zh) * 2019-03-08 2019-07-05 哈尔滨工程大学 一种基于双运动模型的主从式多auv协同导航方法
CN109931936A (zh) * 2019-03-18 2019-06-25 西北工业大学 一种基于移动中继站的弱连通auv协同导航方法
CN111308576A (zh) * 2019-11-29 2020-06-19 哈尔滨工程大学 一种多auv***及深海珊瑚探测方法
CN111257913A (zh) * 2019-11-29 2020-06-09 交通运输部长江通信管理局 北斗卫星信号捕获方法以及装置
CN111257913B (zh) * 2019-11-29 2024-04-30 交通运输部长江通信管理局 北斗卫星信号捕获方法以及装置
CN112857313A (zh) * 2020-12-31 2021-05-28 哈尔滨工程大学 一种面向低带宽声学信道的测深信息传输方法
CN112945245A (zh) * 2021-02-05 2021-06-11 中国航天空气动力技术研究院 基于条件数理论在多auv协同导航***中可观测性分析方法
CN113074725A (zh) * 2021-05-11 2021-07-06 哈尔滨工程大学 一种基于多源信息融合的小型水下多机器人协同定位方法及***
CN113608169A (zh) * 2021-05-20 2021-11-05 济南大学 一种基于序贯融合算法的多auv协同定位方法
CN113608169B (zh) * 2021-05-20 2023-08-25 济南大学 一种基于序贯融合算法的多auv协同定位方法

Also Published As

Publication number Publication date
CN107677272B (zh) 2020-11-10

Similar Documents

Publication Publication Date Title
CN107677272A (zh) 一种基于非线性信息滤波的auv协同导航方法
JP4789216B2 (ja) ナビゲーション用途のための、改良されたｇｐｓ累積デルタ距離処理方法
CN104075715A (zh) 一种结合地形和环境特征的水下导航定位方法
CN110057365B (zh) 一种大潜深auv下潜定位方法
Webster et al. Preliminary deep water results in single-beacon one-way-travel-time acoustic navigation for underwater vehicles
CN103336267B (zh) 一种基于水声通信延迟的主从式多uuv协同定位方法
CN107643534A (zh) 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法
US8560234B2 (en) System and method of navigation based on state estimation using a stepped filter
CN104656058B (zh) 分布式多移动节点协作定位***
CN111596333B (zh) 一种水下定位导航方法及***
CN108827305A (zh) 一种基于鲁棒信息滤波的auv协同导航方法
CN105319534A (zh) 一种基于水声双程测距的多auv协同定位方法
Webster et al. Preliminary Results in Decentralized Estimation for Single-Beacon Acoustic Underwater Navigation.
CN101949703A (zh) 一种捷联惯性/卫星组合导航滤波方法
CN106017467A (zh) 一种基于多水下应答器的惯性/水声组合导航方法
CN103744098A (zh) 基于sins/dvl/gps的auv组合导航***
CN103712625A (zh) 一种估计初始航向偏差滤波的方法
CN103323815A (zh) 一种基于等效声速的水下声学定位方法
CN103697892B (zh) 一种多无人艇协同导航条件下陀螺漂移的滤波方法
CN106707322A (zh) 基于rtk/sins的高动态定位定姿***及方法
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
Wiktor et al. Collaborative multi-robot localization in natural terrain
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法
Guo et al. A robust SINS/USBL integrated navigation algorithm based on earth frame and right group error definition
CN103675880B (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