CN107677272A - A kind of AUV collaborative navigation methods based on nonlinear transformations filtering - Google Patents

A kind of AUV collaborative navigation methods based on nonlinear transformations filtering 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
Chinese (zh)
Other versions
CN107677272B (en
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/en
Publication of CN107677272A publication Critical patent/CN107677272A/en
Application granted granted Critical
Publication of CN107677272B publication Critical patent/CN107677272B/en
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

The present invention discloses a kind of AUV collaborative navigation methods based on nonlinear transformations filtering.In this method, using distributed frame without the location tasks during mark information filter completion collaborative navigation.During co-located, the state equation and measurement equation of AUV navigation system are initially set up；Then using the status information that main AUV is obtained without mark information filter, the expansion of moment progress status information is transmitted in packet, and by being completed without mark information filter to the estimation from AUV states, packet information is handled in the packet time of reception；Finally recover the navigation information that master and slave AUV is obtained by information filter.The invention solves the problems, such as that AUV positioning precisions are low caused by message delay in underwater sound communication, the information correlativity problem that information transmission is brought between AUV is taken into full account, and solves the problem using information marginalisation method, avoid navigation information diverging, the target positioned during the high-precision real for realizing collaborative navigation.

Description

A kind of AUV collaborative navigation methods based on nonlinear transformations filtering
Technical field
The present invention relates to nonlinear filtering to involve collaborative navigation technical field, and in particular to one kind is filtered based on nonlinear transformations AUV collaborative navigation methods.
Background technology
In AUV collaborative navigation technical fields, high accuracy navigation is matter of utmost importance urgently to be resolved hurrily.Centralized collaborative navigation side The original measurement informations of each AUV are delivered to fusion center processing by method, and data fusion process is carried out in fusion center, structure Very flexible, real-time operation can not be carried out to navigation data, practicality is not strong, once fusion center failure, whole system all can Paralysis.In order to solve the problems, such as centralized collaborative navigation, researcher proposes distributed collaboration navigational structure.Distributed collaboration Air navigation aid is handled each AUV measurement information in real time on respective platform, and takes full advantage of the distance between AUV Measurement information, it is preferably to select for real-time navigation.But distributed navigation mode is also faced with several main ask Topic：Compared with ground based navigational system, underwater sound navigation is limited by serious message delay.The underwater spread speed of sound About 1500m/s, the delay of number of seconds level will be caused by carrying out propagation data bag as yardstick using the length of the kilometer order of magnitude.It is this to prolong It is inevitable in communicating under water late, the performance that can be positioned to collaborative navigation produces a very large impact；Secondly, before each AUV Information transmission make its internal information that there is correlation, have to consider this problem in data processing.
Now the research in general Nonlinear Filtering Problem is quite active, it is conventional have " EKF EKF, no Quick Kalman filtering UKF, particle filter PF etc..The non-linear optimal filter of in general can be attributed to the problem of seeking conditional expectation.It is right In the situation of limited multiple observations, conditional expectation can be calculated with Bayesian formula in principle.Even in fairly simple Occasion, the result so drawn be also it is quite numerous and diverse, it is all very inconvenient to practical application or theoretical research.With karr Graceful filtering is similar, can desirably provide certain recursive algorithm of nonlinear filtering or stochastic differential equation that it is met. But it is general they and be not present, it is therefore necessary to appropriate limitation is subject to the process X and Y discussed.The research of nonlinear filtering Work is quite active, and it is related to many modern age achievements of Stochastic Process Theory, such as random process general theory, halter strap, stochastic differential side Journey, point process etc..The problem of one of them is particularly significant, it is to study under what conditions, a halter strap M is present so that any Moment, M and Y include same information；Such M is referred to as Y innovation process.At present for a kind of so-called " condition normal state mistake Journey ", have been presented for the recursion formula that can strictly realize of non-linear optimal filter.In practical application, to nonlinear filtering The method that problem often uses various linear approximations.
At present, people are in the exploratory stage in the research of distributed collaboration field of navigation technology, when carrying out information transmission, High latency signal causes AUV can not receive accurate collaborative navigation information, has a significant impact to navigation accuracy；And ignore navigation Correlation between information, it will cause positioning precision degradation in the case of navigating for a long time.
In order to solve the above problems, the invention provides it is a kind of based on nonlinear transformations filtering AUV collaborative navigation methods, This method considers in AUV information transmissions that caused information correlativity, real-time operation are strong, it is ensured that AUV is in message delay Environment in keep high position precision.
The content of the invention
The present invention initially sets up AUV collaborative navigations system for a kind of AUV collaborative navigation methods based on nonlinear transformations filtering The state model and measurement model of system；Then main AUV state is estimated using nonlinear transformations filtering, in AUV data Bag expands state vector at the time of transmission, the status information at current time is added, in the number of subsequent time transmission According to the packet information that in package informatin, first remove last moment transmission, underwater acoustic channel can be thus avoided to the full extent The problem of narrow bandwidth is brought；Then to carrying out the state estimation based on nonlinear transformations filtering method from AUV, reached in packet At the time of to packet information carry out reception processing, improve from AUV navigation and positioning accuracies；Finally, master and slave AUV information is filtered Ripple result carries out data recovery, obtains high-precision navigator fix information.
Specifically include following steps：
(1) state equation and measurement equation of description AUV collaborative navigation systems are established；
(2) state estimation based on nonlinear transformations filtering is carried out to main AUV, and is having packet to pass to from AUV's Current time information is saved in state vector by the moment, enters row information marginalisation to state vector after packet transmission terminates Process；
(3) to carrying out the state estimation based on nonlinear transformations filtering from AUV, and arrived in the packet for thering is main AUV to transmit Up at the time of carry out data receiver and processing, enter row information marginalisation to state vector after the packet received is processed Journey；
(4) master and slave AUV information filter state is recovered, obtains AUV navigation information.
Characterized in that, described step (1) is specially：
It is as follows to establish nonlinear system model：
Wherein, state equation xk=f (xk-1)+nk-1, observational equation zk=h (xk)+vk, xkShape is tieed up for the n at kth moment State vector；zkVector is measured for the m dimensions at kth moment；F () and h () is known nonlinear function；nk-1For the moment of kth -1 N ties up system noise；vkObservation noise is tieed up for kth moment m, it is assumed that stochastic system noise nk-1~N (0, Qk-1), q~N (μ, Σ) table It is the Gaussian Profile that μ variances are Σ to show that random vector q obeys average；Stochastic Measurement Noises vk~N (0, Rk) and nk-1With vkNot phase Close.
Characterized in that, described step (2) is specially：
(2.1) one-step prediction renewal is carried out：When current time main AUV transmits without packet, one-step prediction without State expands the state for being added without current time, it is assumed that current state is as follows：
Wherein,K moment united state vector is represented, it is made up of two parts,For k moment states,For history Moment state；
Information filter redefines state as follows：
Wherein,K moment evaluated error covariances are represented,For k time information matrixes, useRepresent,For k when Information vector is carved, is usedRepresent, k moment united state matrixes and state vector represent as follows：
WhereinK moment united information matrixes are represented,The information square of k moment and historical juncture are represented respectively Battle array,The related information matrix of k moment and historical juncture are represented,K moment united information vector is represented,Represent K time informations vector,Represent historical juncture information vector；
One-step prediction result is as follows：
Wherein,Represent stochastic system noiseCovariance,Represent that nonlinear function f () is pseudo- Sytem matrix, it can be defined as follows：
Wherein,RepresentWithCross covariance, can be adopted in Unscented kalman filtering algorithm with sigma Sampling point represents as follows：
Wherein,It is sampled point, 2n is total number of samples；
When current time main AUV carries out packet transmission, one-step prediction carries out state expansion, adds current state, during k The packet transmitted is carved to be expressed as：
Wherein, ΛTRepresent that a upper packet transmits the information matrix at moment main AUV, ηTRepresent that a upper packet transmits the moment Main AUV information vector；
After information transmission terminates, to ΛT、ηTUpgrade in time：
K moment status informations are extended in state vector, it is as a result as follows：
Corresponding information matrix and information vector are as follows：
(2.2) renewal is measured：
Wherein,Represent to measure noise vkVariance,The measurement vector at k+1 moment is represented,Represent non-linear The pseudo- measurement matrixes of function h (), represent as follows：
Wherein,Represent one-step prediction estimation and measure the cross covariance of prediction,Represent to filter using Unscented kalman The one-step prediction error covariance that ripple fundamental equation is tried to achieve；
(2.3) marginalisation is handled：After the completion of renewal is measured, row information marginalisation processing is entered to state vector.
Characterized in that, described step (3) is specially：
(3.1) one-step prediction updates：
WhereinK moment united information matrixes are represented,The information matrix and historical information at k moment are represented respectively Matrix,K moment related information matrixes and historical context information matrix are represented,Represent k moment united information to Amount,K time informations vector is represented,Represent historical information vector；
One-step prediction result represents as follows：
Wherein,One-step prediction information matrix is represented,One-step prediction information vector is represented,Represent non-linear letter Number f () antiforge system matrix,Represent stochastic system noiseVariance；
(3.2) renewal is measured：When current time does not receive the packet that main AUV is transmitted from AUV, do not enter after one-step prediction Row processing data packets, directly carry out local update：
Wherein,The pseudo- measurement matrixes of nonlinear function h () are represented,Represent to measure noise vkVariance,Represent The measurement vector at k+1 moment；Current time from AUV receive from the packet that main AUV is transmitted when, carry out data after one-step prediction Bag processing updates again；ΛΔIt is added after zero padding：
Distance measuring renewal is as follows：
Local measurement information renewal is as follows：
(3.3) marginalisation is handled：After the completion of renewal is measured, row information marginalisation processing is entered to state vector, it is specific to calculate Method is the same as main AUV information marginalisation process.
The advantage of the invention is that：
(1) AUV collaborative navigation system models are established, using the distance between AUV measurement informations, have taken into full account delay The presence of problem, give a kind of high-precision collaborative navigation method based on nonlinear transformations filtering.
(2) the information correlativity problem that information transmission is brought between AUV has been taken into full account, and side has been carried out to relevant information Edgeization processing, while computation complexity is simplified, ensure the accuracy of navigator fix information.
Brief description of the drawings
Fig. 1 is flow chart of the method for the present invention；
Fig. 2 is the main AUV navigation system provided by the invention based on nonlinear transformations filtering method to x coordinate direction of principal axis position Put the mean square error curve of estimation；
Fig. 3 is the main AUV navigation system provided by the invention based on nonlinear transformations filtering method to y-coordinate direction of principal axis position Put the mean square error curve of estimation；
Fig. 4 is the main AUV navigation system provided by the invention based on nonlinear transformations filtering method to x coordinate direction of principal axis speed Spend the mean square error curve of estimation；
Fig. 5 is the main AUV navigation system provided by the invention based on nonlinear transformations filtering method to y-coordinate direction of principal axis speed Spend the mean square error curve of estimation；
Fig. 6 be it is provided by the invention based on nonlinear transformations filtering method from AUV navigation system to x coordinate direction of principal axis position Put the mean square error curve of estimation；
Fig. 7 be it is provided by the invention based on nonlinear transformations filtering method from AUV navigation system to y-coordinate direction of principal axis position Put the mean square error curve of estimation；
Fig. 8 is provided by the invention fast to x coordinate direction of principal axis from AUV navigation system based on nonlinear transformations filtering method Spend the mean square error curve of estimation；
Fig. 9 is provided by the invention fast to y-coordinate direction of principal axis from AUV navigation system based on nonlinear transformations filtering method Spend the mean square error curve of estimation.
Embodiment
Below in conjunction with drawings and examples, the present invention is described in further detail.
The present invention is a kind of AUV collaborative navigation methods based on nonlinear transformations filtering, including following steps：
(1) state equation and measurement equation of description AUV collaborative navigation systems are established.Specifically, establish nonlinear system Model of uniting is as follows：
Wherein, state equation xk=f (xk-1)+nk-1, observational equation zk=h (xk)+vk, xkShape is tieed up for the n at kth moment State vector, characterize AUV positional information and velocity information, zkVector is measured for the m dimensions at kth moment, characterizes the side to AUV Position observation information, f () and h () nonlinear function, n for known tok-1System noise, v are tieed up for the moment of kth -1 nkFor kth when Carve m dimension observation noises, it is assumed that stochastic system noise nk-1~N (0, Qk-1) (q~N (μ, Σ) represents that random vector q obeys average and is μ variances are Σ Gaussian Profile), Stochastic Measurement Noises vk~N (0, Rk), and nk-1With vkIt is uncorrelated.
(2) state estimation based on nonlinear transformations filtering is carried out to main AUV systems.
(2.1) one-step prediction updates
When current time main AUV transmits without packet, it is when being added without current that one-step prediction expands without state The state at quarter, specific algorithm are as follows：
Assuming that current state is as follows：
Wherein,K moment united state vector is represented, it is made up of two parts,For k moment states,For history when Quarter state.
Information filter redefines state as follows：
Wherein,K moment evaluated error covariances are represented,For k time information matrixes, useRepresent,For k when Information vector is carved, is usedRepresent.Then it is expressed as below for k moment united state matrixes and state vector：
WhereinK moment united information matrixes are represented,The information square of k moment and historical juncture are represented respectively Battle array,The related information matrix of k moment and historical juncture are represented,K moment united information vector is represented,Represent K time informations vector,Represent historical juncture information vector.
One-step prediction result represents as follows：
Wherein,Represent stochastic system noiseCovariance,Represent that nonlinear function f () is pseudo- Sytem matrix, it can be defined as follows：
Wherein,RepresentWithCross covariance, can be adopted in Unscented kalman filtering algorithm with sigma Sampling point represents as follows：
Wherein,It is sampled point, 2n is total number of samples.
When current time main AUV carries out packet transmission, one-step prediction carries out state expansion, adds current state, has Body algorithm is as follows：
Now first have to carry out packet transmission, k time data package informatins are passed to from AUV.When packet transmits, nothing The full detail at k moment need to be transmitted, need to only transmit from a upper packet and transmit the moment to the increment information at k moment, so both protect The integrality of transmission information has been demonstrate,proved, but it is relatively low to bandwidth requirement.
The packet that the k moment transmits can be expressed as：
Wherein, ΛTRepresent that a upper packet transmits the information matrix at moment main AUV, ηTRepresent that a upper packet transmits the moment Main AUV information vector.
Information transmission terminates rear, it is necessary to ΛT、ηTUpgrade in time：
K moment status informations are extended in state vector, it is as a result as follows：
Then its corresponding information matrix and information vector are as follows：
(2.2) renewal is measured
Wherein,Represent to measure noise vkVariance,The measurement vector at k+1 moment is represented,Represent non-linear The pseudo- measurement matrixes of function h (), can be represented as follows：
Wherein,Represent one-step prediction estimation and measure the cross covariance of prediction,Represent one-step prediction error association side Difference, available Unscented kalman filtering fundamental equation are tried to achieve.
(2.3) marginalisation is handled
In order to ensure that the dimension of main AUV state vectors is unlikely to too high, dyscalculia is caused, after the completion of renewal is measured, Enter row information marginalisation processing to state vector, specific algorithm is as follows：
Situation one：When needing the status information of marginalisation to be located at information vector bottom position：
It is by β information progress marginalisation result：
η (α)=ηα-ΛαβΛββ -1ηβ (23)
Situation two：When needing the status information of marginalisation to be located at information vector medium position：
It is by β information progress marginalisation result：
(3) to carrying out the state estimation based on information filter from AUV.
(3.1) one-step prediction updates
When carrying out one-step prediction from AUV, it is not necessary to packet is transmitted to main AUV, so information expansion need not be carried out, Specific algorithm is as follows：
WhereinK moment united information matrixes are represented,The information matrix and historical information at k moment are represented respectively Matrix,K moment related information matrixes and historical context information matrix are represented,Represent k moment united information to Amount,K time informations vector is represented,Represent historical information vector.Pay attention to：Now historical information passes for main AUV packets The information passed, due to the addition of distance measuring information so that master and slave AUV information has correlation.Herein, information is as considered Algorithm after correlation.
One-step prediction result represents as follows：
Wherein,One-step prediction information matrix is represented,One-step prediction information vector is represented,Represent non-linear letter Number f () antiforge system matrix,Represent stochastic system noiseVariance.
(3.2) renewal is measured
When current time does not receive the packet that main AUV is transmitted from AUV, without processing data packets after one-step prediction, Local update is directly carried out, specific algorithm is as follows：
Wherein,The pseudo- measurement matrixes of nonlinear function h () are represented,Represent to measure noise vkVariance,Represent The measurement vector at k+1 moment.
Current time from AUV receive from the packet that main AUV is transmitted when, carry out processing data packets after one-step prediction, have Body algorithm is as follows:
Pay attention to nowΛΔNot directly it is added, it is necessary to matrix zero padding is added again, because transmitting in packet not Information comprising the newest moment from AUV, it is equally, unrelated from the information and date package informatin at AUV current times.
Distance measuring updates：
Local measurement information renewal
(3.3) marginalisation is handled
In order to ensure that the dimension from AUV state vectors is unlikely to too high, dyscalculia is caused, after the completion of renewal is measured, Enter row information marginalisation processing to state vector, specific algorithm is the same as main AUV information marginalisation process.
(4) master and slave AUV information filter state is recovered, obtains AUV navigation information.
Embodiment：When AUV collaborative navigations position, underwater sound communication condition is the limiting factor that must take into consideration.Due to underwater Environment is complicated, and underwater sound communication is limited, so with reference to actual conditions, distributed AUV collaborative navigations method more meets actual requirement.But It is that existing distributed method is also faced with problems.Method provided by the invention, which is aimed to solve the problem that in distributed frame, to communicate Delay and information correlativity problem, high-precision navigation information is provided for AUV.Below with specific embodiment come illustrate the present invention Superiority.It is specific as follows：
We illustrate by taking two AUV collaborative navigation systems as an example in this example, one of them main AUV, one from AUV, Main AUV can give self information and distance measuring data transfer from AUV, there was only packet receiving ability from AUV, do not transmit letter Breath.
Under water in navigation system, AUV posture and depth can be measured with corresponding sensor respectively, and obtain It is the navigation information of error bounded.We only consider AUV position and velocity information in modeling, reduce state vector dimension Number, so it is easier to meet that underwater bandwidth limitation requires that state vector is chosen as follows：
X=[x y vx vy]T (43)
Then establish state model and distance measuring model is as follows：
xk+1=Fkxk+nk (44)
Wherein,Δ T is the discrete model sampling interval.nkFor the system noise at kth moment, nk~N (0,Qk), Qk=diag ([10m 10m 0.02m/s 0.02m/s]), QkCharacterize the uncertainty of system model.
Wherein, zkRepresent the distance measuring information at kth moment；WithPositional information of the current time from AUV is represented,WithRepresent current time from the AUV main AUV received navigation information, vkFor the measurement noise at kth moment, vk~N (0, Rk), Rk=9m, RkCharacterize the uncertainty of distance measuring.
Initial time of day value and initial covariance matrix set as follows：
Wherein xsAnd xcMain AUV and the original state from AUV are represented respectively,WithMain AUV is represented respectively and from AUV's Initial error covariance value,WithCharacterize the uncertainty of target initial position.
Then set according to original state and covariance, master and slave AUV initial information matrix and information can be calculated respectively Vector, concrete outcome are as follows：
Implementation process：The mean square error performance indications being defined as below are used in simulation process, compare the error of filtering method：
Wherein N is Carlo number of Monte.It is smaller to the square mean error amount of AUV navigation informations estimation, characterize positioning accurate Degree is higher, and effect is better.
Simulation time is 1000 seconds, carries out 500 Monte Carlo emulation, verifies that the present invention provides high accuracy positioning letter Breath.

Claims (4)

  1. A kind of 1. AUV collaborative navigation methods based on nonlinear transformations filtering, it is characterised in that specifically include following step Suddenly：
    (1) state equation and measurement equation of description AUV collaborative navigation systems are established；
    (2) state estimation based on nonlinear transformations filtering is carried out to main AUV, and at the time of thering is packet to pass to from AUV Current time information is saved in state vector, enters row information marginalisation to state vector after packet transmission terminates Journey；
    (3) to carrying out the state estimation based on nonlinear transformations filtering from AUV, and reached in the packet for thering is main AUV to transmit Moment carries out data receiver and processing, enters row information marginalisation process to state vector after the packet received is processed；
    (4) master and slave AUV information filter state is recovered, obtains AUV navigation information.
  2. A kind of 2. AUV collaborative navigation methods based on nonlinear transformations filtering according to claim 1, it is characterised in that Described step (1) is specially：
    It is as follows to establish nonlinear system model：
    <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>
    Wherein, state equation xk=f (xk-1)+nk-1, observational equation zk=h (xk)+vk, xkFor the kth moment n tie up state to Amount；zkVector is measured for the m dimensions at kth moment；F () and h () is known nonlinear function；nk-1Tieed up for the moment of kth -1 n System noise；vkObservation noise is tieed up for kth moment m, it is assumed that stochastic system noise nk-1~N (0, Qk-1), q~N (μ, Σ) is represented It is the Gaussian Profile that μ variances are Σ that random vector q, which obeys average,；Stochastic Measurement Noises vk~N (0, Rk) and nk-1With vkIt is uncorrelated.
  3. A kind of 3. AUV collaborative navigation methods based on nonlinear transformations filtering according to claim 1, it is characterised in that Described step (2) is specially：
    (2.1) one-step prediction renewal is carried out：
    When current time main AUV transmits without packet, one-step prediction expands without state is added without current time State, it is assumed that current state is as follows：
    <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>
    Wherein,K moment united state vector is represented,For k moment states,For historical juncture state；
    Information filter redefines state as follows：
    <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>
    Wherein,K moment evaluated error covariances are represented,For k time information matrixes, useRepresent,Believe for the k moment Breath vector, is usedRepresent, k moment united state matrixes and state vector represent as follows：
    <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>
    WhereinK moment united information matrixes are represented,The information matrix of k moment and historical juncture are represented respectively,The related information matrix of k moment and historical juncture are represented,K moment united information vector is represented,When representing k Carve information vector,Represent historical juncture information vector；
    One-step prediction result is as follows：
    <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>
    Wherein,Represent stochastic system noiseCovariance,Represent nonlinear function f () antiforge system square Battle array, can be defined as follows：
    <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>
    Wherein,RepresentWithCross covariance, sigma sampled points can be used in Unscented kalman filtering algorithm Represent as follows：
    <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>
    Wherein,It is sampled point, 2n is total number of samples；
    When current time main AUV carries out packet transmission, one-step prediction carries out state expansion, adds current state, the k moment passes The packet passed is expressed as：
    <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>
    Wherein, ΛTRepresent that a upper packet transmits the information matrix at moment main AUV, ηTRepresent that a upper packet transmits moment master AUV information vector；
    After information transmission terminates, to ΛT、ηTUpgrade in time：
    <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 moment status informations are extended in state vector, it is as a result as follows：
    <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>
    Corresponding information matrix and information vector are as follows：
    <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) renewal is measured：
    <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>
    Wherein,Represent to measure noise vkVariance,The measurement vector at k+1 moment is represented,Represent nonlinear function h () pseudo- measurement matrix, represent as follows：
    <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>
    Wherein,Represent one-step prediction estimation and measure the cross covariance of prediction,Expression utilizes Unscented kalman filtering base The one-step prediction error covariance that this equation is tried to achieve；
    (2.3) marginalisation is handled：After the completion of renewal is measured, row information marginalisation processing is entered to state vector.
  4. A kind of 4. AUV collaborative navigation methods based on nonlinear transformations filtering according to claim 1, it is characterised in that Described step (3) is specially：
    (3.1) one-step prediction updates：
    <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>
    WhereinK moment united information matrixes are represented,The information matrix and historical information square at k moment are represented respectively Battle array,K moment related information matrixes and historical context information matrix are represented,K moment united information vector is represented,K time informations vector is represented,Represent historical information vector；
    One-step prediction result represents as follows：
    <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>
    Wherein,One-step prediction information matrix is represented,One-step prediction information vector is represented,Represent nonlinear function f () antiforge system matrix,Represent stochastic system noiseVariance；
    (3.2) renewal is measured：
    When current time does not receive the packet that main AUV is transmitted from AUV, without processing data packets after one-step prediction, directly Carry out local update：
    <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>
    Wherein,The pseudo- measurement matrixes of nonlinear function h () are represented,Represent to measure noise vkVariance,Represent k+1 The measurement vector at moment；Current time from AUV receive from the packet that main AUV is transmitted when, carry out packet after one-step prediction Processing updates again；ΛΔIt is added after zero padding：
    <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>
    Distance measuring renewal is as follows：
    <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>
    Local measurement information renewal is as follows：
    <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) marginalisation is handled：After the completion of renewal is measured, row information marginalisation processing is entered to state vector, specific algorithm is same Main AUV information marginalisation process.
CN201710805228.1A 2017-09-08 2017-09-08 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering Active CN107677272B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710805228.1A CN107677272B (en) 2017-09-08 2017-09-08 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710805228.1A CN107677272B (en) 2017-09-08 2017-09-08 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering

Publications (2)

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

Family

ID=61134575

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710805228.1A Active CN107677272B (en) 2017-09-08 2017-09-08 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering

Country Status (1)

Country Link
CN (1) CN107677272B (en)

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108489498A (en) * 2018-06-15 2018-09-04 哈尔滨工程大学 A kind of AUV collaborative navigation methods without mark particle filter based on maximum cross-correlation entropy
CN108594834A (en) * 2018-03-23 2018-09-28 哈尔滨工程大学 One kind is towards more AUV adaptive targets search and barrier-avoiding method under circumstances not known
CN108802692A (en) * 2018-05-25 2018-11-13 哈尔滨工程大学 A kind of method for tracking target based on maximum cross-correlation entropy volume particle filter
CN108827305A (en) * 2018-05-25 2018-11-16 哈尔滨工程大学 A kind of AUV collaborative navigation method based on robust information filtering
CN109084767A (en) * 2018-06-15 2018-12-25 哈尔滨工程大学 A kind of AUV collaborative navigation method of the adaptive volume particle filter of maximum cross-correlation entropy
CN109931936A (en) * 2019-03-18 2019-06-25 西北工业大学 A kind of weak connectedness AUV collaborative navigation method based on mobile-relay station
CN109974706A (en) * 2019-03-08 2019-07-05 哈尔滨工程大学 A kind of more AUV collaborative navigation methods of master-slave mode based on double motion models
CN111257913A (en) * 2019-11-29 2020-06-09 交通运输部长江通信管理局 Beidou satellite signal capturing method and device
CN111308576A (en) * 2019-11-29 2020-06-19 哈尔滨工程大学 Multi-AUV system and deep sea coral detection method
CN112857313A (en) * 2020-12-31 2021-05-28 哈尔滨工程大学 Sounding information transmission method facing low-bandwidth acoustic channel
CN112945245A (en) * 2021-02-05 2021-06-11 中国航天空气动力技术研究院 Observability analysis method in multi-AUV collaborative navigation system based on condition number theory
CN113074725A (en) * 2021-05-11 2021-07-06 哈尔滨工程大学 Small underwater multi-robot cooperative positioning method and system based on multi-source information fusion
CN113608169A (en) * 2021-05-20 2021-11-05 济南大学 Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sequential fusion algorithm

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103336267A (en) * 2013-07-02 2013-10-02 哈尔滨工程大学 Master-slave mode multi-UUV (Unmanned Underwater Vehicle) cooperative location method based on underwater acoustic communication delay
CN103616026A (en) * 2013-12-17 2014-03-05 哈尔滨工程大学 AUV (Autonomous Underwater Vehicle) manipulating model auxiliary strapdown inertial navigation combined navigation method based on H infinity filtering
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 (en) * 2015-11-09 2016-03-30 哈尔滨工程大学 Underwater acoustic two-way distance-measuring error compensation method applied in dynamic condition of multi-AUV coordinative navigation
CN106441300A (en) * 2016-09-08 2017-02-22 哈尔滨工程大学 Self-adaptive collaborative navigation and filtering method
CN106525042A (en) * 2016-09-27 2017-03-22 哈尔滨工程大学 Multi-AUV synthetic location method based on combination of ant colony and extended Kalman filtering

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 (en) * 2013-07-02 2013-10-02 哈尔滨工程大学 Master-slave mode multi-UUV (Unmanned Underwater Vehicle) cooperative location method based on underwater acoustic communication delay
CN103616026A (en) * 2013-12-17 2014-03-05 哈尔滨工程大学 AUV (Autonomous Underwater Vehicle) manipulating model auxiliary strapdown inertial navigation combined navigation method based on H infinity filtering
CN105445722A (en) * 2015-11-09 2016-03-30 哈尔滨工程大学 Underwater acoustic two-way distance-measuring error compensation method applied in dynamic condition of multi-AUV coordinative navigation
CN106441300A (en) * 2016-09-08 2017-02-22 哈尔滨工程大学 Self-adaptive collaborative navigation and filtering method
CN106525042A (en) * 2016-09-27 2017-03-22 哈尔滨工程大学 Multi-AUV synthetic location method based on combination of ant colony and extended Kalman filtering

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 (en) * 2018-03-23 2018-09-28 哈尔滨工程大学 One kind is towards more AUV adaptive targets search and barrier-avoiding method under circumstances not known
CN108594834B (en) * 2018-03-23 2020-12-22 哈尔滨工程大学 Multi-AUV self-adaptive target searching and obstacle avoiding method oriented to unknown environment
CN108802692A (en) * 2018-05-25 2018-11-13 哈尔滨工程大学 A kind of method for tracking target based on maximum cross-correlation entropy volume particle filter
CN108827305A (en) * 2018-05-25 2018-11-16 哈尔滨工程大学 A kind of AUV collaborative navigation method based on robust information filtering
CN108489498A (en) * 2018-06-15 2018-09-04 哈尔滨工程大学 A kind of AUV collaborative navigation methods without mark particle filter based on maximum cross-correlation entropy
CN109084767A (en) * 2018-06-15 2018-12-25 哈尔滨工程大学 A kind of AUV collaborative navigation method of the adaptive volume particle filter of maximum cross-correlation entropy
CN109974706A (en) * 2019-03-08 2019-07-05 哈尔滨工程大学 A kind of more AUV collaborative navigation methods of master-slave mode based on double motion models
CN109931936A (en) * 2019-03-18 2019-06-25 西北工业大学 A kind of weak connectedness AUV collaborative navigation method based on mobile-relay station
CN111308576A (en) * 2019-11-29 2020-06-19 哈尔滨工程大学 Multi-AUV system and deep sea coral detection method
CN111257913A (en) * 2019-11-29 2020-06-09 交通运输部长江通信管理局 Beidou satellite signal capturing method and device
CN111257913B (en) * 2019-11-29 2024-04-30 交通运输部长江通信管理局 Beidou satellite signal capturing method and device
CN112857313A (en) * 2020-12-31 2021-05-28 哈尔滨工程大学 Sounding information transmission method facing low-bandwidth acoustic channel
CN112945245A (en) * 2021-02-05 2021-06-11 中国航天空气动力技术研究院 Observability analysis method in multi-AUV collaborative navigation system based on condition number theory
CN113074725A (en) * 2021-05-11 2021-07-06 哈尔滨工程大学 Small underwater multi-robot cooperative positioning method and system based on multi-source information fusion
CN113608169A (en) * 2021-05-20 2021-11-05 济南大学 Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sequential fusion algorithm
CN113608169B (en) * 2021-05-20 2023-08-25 济南大学 Multi-AUV (autonomous Underwater vehicle) co-location method based on sequential fusion algorithm

Also Published As

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

Similar Documents

Publication Publication Date Title
CN107677272A (en) A kind of AUV collaborative navigation methods based on nonlinear transformations filtering
JP4789216B2 (en) Improved GPS cumulative delta distance processing method for navigation applications
Kepper et al. A navigation solution using a MEMS IMU, model-based dead-reckoning, and one-way-travel-time acoustic range measurements for autonomous underwater vehicles
CN104075715A (en) Underwater navigation and positioning method capable of combining terrain and environment characteristics
CN110057365B (en) Large-submergence-depth AUV submergence positioning method
Webster et al. Preliminary deep water results in single-beacon one-way-travel-time acoustic navigation for underwater vehicles
CN103336267B (en) Master-slave mode multi-UUV (Unmanned Underwater Vehicle) cooperative location method based on underwater acoustic communication delay
CN107643534A (en) A kind of dual rate kalman filter method based on GNSS/INS deep integrated navigations
CN101949703B (en) Strapdown inertial/satellite combined navigation filtering method
US8560234B2 (en) System and method of navigation based on state estimation using a stepped filter
CN111596333B (en) Underwater positioning navigation method and system
CN103344260B (en) Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF
CN108827305A (en) A kind of AUV collaborative navigation method based on robust information filtering
CN105319534A (en) Multiple AUV cooperative positioning method based on underwater sound double pass range finding
Webster et al. Preliminary Results in Decentralized Estimation for Single-Beacon Acoustic Underwater Navigation.
CN106017467A (en) Inertia/underwater sound combined navigation method based on multiple underwater transponders
CN103744098A (en) Ship&#39;s inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)-based autonomous underwater vehicle (AUV) combined navigation system
CN103712625A (en) Method for estimating initial heading deviation filtering
CN103323815A (en) Underwater acoustic locating method based on equivalent sound velocity
CN106707322A (en) RTK/SINS-based high dynamic positioning attitude-determining system and method
CN115307643A (en) Double-responder assisted SINS/USBL combined navigation method
CN104061930A (en) Navigation method based on strapdown inertial guidance and Doppler log
CN103697892A (en) Filtering method for gyroscopic drift under collaborative navigation condition of multiple unmanned surface vehicles
Wiktor et al. Collaborative multi-robot localization in natural terrain
Guo et al. A robust SINS/USBL integrated navigation algorithm based on earth frame and right group error definition

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