CN113324546B - Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure - Google Patents

Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure Download PDF

Info

Publication number
CN113324546B
CN113324546B CN202110563429.1A CN202110563429A CN113324546B CN 113324546 B CN113324546 B CN 113324546B CN 202110563429 A CN202110563429 A CN 202110563429A CN 113324546 B CN113324546 B CN 113324546B
Authority
CN
China
Prior art keywords
matrix
formula
noise
state
underwater vehicle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110563429.1A
Other languages
Chinese (zh)
Other versions
CN113324546A (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 CN202110563429.1A priority Critical patent/CN113324546B/en
Publication of CN113324546A publication Critical patent/CN113324546A/en
Application granted granted Critical
Publication of CN113324546B publication Critical patent/CN113324546B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • Computing Systems (AREA)
  • Algebra (AREA)
  • Automation & Control Theory (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

The invention provides a multi-underwater vehicle collaborative positioning self-adaptive regulation robust filtering method under compass failure, which comprises the following steps: the method comprises the following steps: establishing a multi-underwater vehicle co-location model with unknown course angle; step two: calculating a heading angle input unknown state estimation value to be corrected; step three: the influence of outlier noise on state estimation is weakened by adaptively updating a measurement noise variance matrix; step four: constructing a nuclear band width self-adaptive adjustment factor; step five: and calculating a course estimation value and a corrected position vector estimation value. The method realizes accurate positioning under the condition that the course angle is unknown along with non-Gaussian noise on the basis of ensuring that the state dimension is not expanded. The method can be used in the field of multi-underwater vehicle co-location under non-ideal conditions.

Description

Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure
Technical Field
The invention belongs to the field of multi-underwater-vehicle cooperative positioning under non-ideal conditions, and relates to a multi-underwater-vehicle cooperative positioning self-adaptive regulation robust filtering method under compass failure.
Background
The cooperative positioning is one of the most effective navigation methods of the multi-underwater vehicle in the middle layer area at present. However, the positioning performance of the cooperative system is often limited by various factors, and an important problem in the cooperative positioning process is how to estimate the position of the underwater vehicle under non-ideal conditions. The multi-underwater vehicle co-location system is a non-linear system, and when course information of the multi-underwater vehicle is absent and accompanied by noise and non-Gaussian distribution, the multi-underwater vehicle co-location system has great influence on location accuracy. The compass has a complex circuit structure, and the use of the low-cost compass can also cause the increase of the fault occurrence probability. Generally, when the compass fails, the course angle is estimated by extending the state dimension, however, the amount of calculation increases, and especially when the unknown fault vector dimension is comparable to the system state dimension, the calculation cost is larger. For the dimension problem, a second-order extended kalman filter and a second-order unscented kalman filter are proposed in succession, but the two-order extended kalman filter and the two-order unscented kalman filter are limited in that a statistical model of a fault vector is unknown and are not suitable for an environment in which noise is non-gaussian distributed.
Therefore, it is a technical problem to be solved for the cooperative positioning direction to discuss the influence mechanism of compass failure and non-gaussian noise on the positioning accuracy of the cooperative positioning system, and consider how to estimate the real-time changing course angle on the premise of not expanding the state dimension and overcoming the interference of the non-gaussian noise.
Disclosure of Invention
Aiming at the prior art, the technical problem to be solved by the invention is to provide a multi-underwater vehicle collaborative positioning adaptive regulation robust filtering method under compass failure for estimating a changed course angle in real time while overcoming non-Gaussian noise interference on the premise of not expanding a state dimension and unknown course noise sequence so as to improve the collaborative positioning precision.
In order to solve the technical problem, the invention provides a multi-underwater vehicle collaborative positioning adaptive adjustment robust filtering method under compass failure, which comprises the following steps:
the method comprises the following steps: establishing a multi-underwater vehicle co-location model with unknown course angle;
step two: calculating a heading angle input unknown state estimation value to be corrected;
step three: the influence of outlier noise on state estimation is weakened by adaptively updating a measurement noise variance matrix;
step four: constructing a nuclear band width self-adaptive adjustment factor;
step five: and calculating a course estimation value and a corrected position vector estimation value.
The invention also includes:
1. the establishment of the multi-underwater vehicle co-location model with unknown course angle in the first step specifically comprises the following steps:
the equation of state and the measurement equation are as follows:
Figure BDA0003079966230000021
Figure BDA0003079966230000022
in the formula a k And b k East and north positions of the slave underwater vehicle at the moment k, delta t is a sampling period, v a,k ,v b,k Is the right and forward speed, theta, at time k, of the slave vehicle k Is the angle between the forward direction and the north direction at time k, w a,k ,w b,k Is zero mean white Gaussian noise, Z k The relative distance measurement information between the pilot underwater vehicle and the slave underwater vehicle,
Figure BDA0003079966230000023
is the position of the pilot vehicle at time k, epsilon k To measure noise;
will theta k As an estimated variable, satisfy
Figure BDA0003079966230000024
In the formula
Figure BDA0003079966230000025
Is zero mean white noise;
the discrete time state space model establishment specifically comprises the following steps:
Figure BDA0003079966230000026
Figure BDA0003079966230000027
in the formula
Figure BDA0003079966230000028
x k ＝[a k b k ] T Is the position of the slave vehicle at time k, n a N +1, n is x k The dimension (c) of (a) is,
Figure BDA0003079966230000029
and
Figure BDA00030799662300000210
respectively nonlinear state function and measurement function, assuming
Figure BDA00030799662300000211
Is a sequence of white gaussian noise, and is,
Figure BDA00030799662300000212
and has the following components:
Figure BDA00030799662300000213
in the formula u k ＝[v a,k v b,k ] T The forward and right speed measured for the DVL,
Figure BDA00030799662300000214
the estimated value of the course angle at the moment of k-1;
in the formula
Figure BDA00030799662300000215
Figure BDA00030799662300000216
In the formula (I), the compound is shown in the specification,
Figure BDA00030799662300000217
is a nonlinear state function.
2. In the second step, the calculation of the unknown state estimation value to be corrected of the course angle input is specifically as follows:
error covariance of state estimate
Figure BDA0003079966230000031
Can be decomposed into:
Figure BDA0003079966230000032
in the formula S k-1/k-1 Is composed of
Figure BDA0003079966230000033
A lower triangular matrix after triangular decomposition;
the volume point calculation is specifically:
χ k-1,i ＝S k-1/k-1 ρ i +x k-1/k-1
in the formula
Figure BDA0003079966230000034
[I n ] i Representing an n-dimensional identity matrix I n The ith column;
volume point propagation is specifically:
Figure BDA0003079966230000035
Figure BDA0003079966230000036
Figure BDA0003079966230000037
Figure BDA0003079966230000038
y k/k-1,i ＝h k (χ k/k-1,i )
Figure BDA0003079966230000039
in the formula
Figure BDA00030799662300000310
As a function of the non-linear state f k-1 (c) the spread out sample points,
Figure BDA00030799662300000311
in order to be the state prediction value,
Figure BDA00030799662300000312
is composed of
Figure BDA00030799662300000313
Of the error covariance matrix χ k/k-1,i Is a secondary sampling point, y k/k-1,i As a function of the non-linear measurement h k (c) the spread out sample points,
Figure BDA00030799662300000314
for measuring the predicted value, Q k Is a process noise covariance matrix, ξ i And rho i The values are the same.
The measurement is updated as follows:
Figure BDA00030799662300000315
Figure BDA00030799662300000316
Figure BDA00030799662300000317
Figure BDA00030799662300000318
Figure BDA00030799662300000319
in the formula
Figure BDA00030799662300000320
In the form of a cross-covariance matrix,
Figure BDA00030799662300000321
to measure the auto-covariance matrix, R k To measure the noise covariance matrix, K k Is a gain matrix, y k In order to actually measure the information, the measurement device is provided with a sensor,
Figure BDA0003079966230000041
respectively, a state estimation value and an error covariance matrix when no course input exists.
3. The third step of attenuating the influence of the outlier noise on the state estimation by adaptively updating the measured noise variance matrix is specifically as follows:
adaptive updating of the metric noise variance matrix, namely:
Figure BDA0003079966230000042
and is provided with
Figure BDA0003079966230000043
Wherein T is P,k|k-1 And T r,k Are respectively as
Figure BDA0003079966230000044
And the variance matrix R of the measured noise k A lower triangular matrix after triangular decomposition; in the co-location model, the system noise is assumed to be gaussian noise, so there are:
Figure BDA0003079966230000045
where σ is the nuclear band width and has:
e k ＝ζ k -G k x k
in the formula
Figure BDA0003079966230000046
Figure BDA0003079966230000047
m is the dimension of the measurement.
4. The construction of the nuclear band width adaptive adjustment factor in the fourth step is specifically as follows:
an innovation vector is defined as
Figure BDA0003079966230000048
By an innovation matrix
Figure BDA0003079966230000049
And a measurement error covariance matrix
Figure BDA00030799662300000410
Constructing an adaptive adjustment factor:
Figure BDA00030799662300000411
in the formula
Figure BDA00030799662300000412
When the trace of the innovation matrix is less than or equal to the trace of the measurement error variance matrix, the value of the self-adaptive factor is 1, otherwise, the constructed self-adaptive factor is utilized to carry out real-time correction on the bandwidth sigma, namely sigma t ＝λ t σ t-1 And t is the number of iterations.
5. The fifth step of calculating the course estimation value and the corrected position vector estimation value specifically comprises the following steps:
the error covariance matrix of the course angle is:
Figure BDA00030799662300000413
in the formula
Figure BDA0003079966230000051
The gain matrix of the position and course estimation is respectively:
Figure BDA0003079966230000052
Figure BDA0003079966230000053
the course estimation value is as follows:
Figure BDA0003079966230000054
the covariance matrices of the position estimation and the position estimation error after the correction of the course angle estimation value are respectively as follows:
Figure BDA0003079966230000055
Figure BDA0003079966230000056
in the formula
Figure BDA0003079966230000057
The invention has the beneficial effects that: the co-location method based on the U-V conversion decoupling thought and the cross-correlation entropy theory simultaneously considers the compass failure and the non-Gaussian characteristics of noise, and researches the co-location method under the condition that the course angle is unknown and the non-Gaussian noise is accompanied. The existing second-order extended Kalman filtering, second-order unscented Kalman filtering and the like can realize decoupling of extended state quantities so as to reduce calculation cost, but are limited by unknown fault vector statistical models, and the former is not suitable for strong nonlinear models. In addition, when noise presents non-gaussian characteristics due to the occurrence of noise outliers, the existing algorithm cannot overcome the interference of the non-gaussian noise while reducing the calculation cost.
Aiming at the problems that the estimation of a course angle in the current cooperative positioning needs to expand a state estimation dimension, so that the calculation cost is increased, and an algorithm is not suitable for a non-Gaussian noise environment, the method introduces U-V transformation in a volume Kalman filtering (CKF) suitable for a strong nonlinear model to complete the decoupling of position estimation and course estimation, and simultaneously introduces a maximum cross-correlation entropy (MCC) theory to assist a measurement noise variance matrix to complete self-adaptive updating when an abnormal measurement value appears. And finally, on the basis of ensuring that the state dimension is not expanded, accurate positioning under the condition that the course angle is unknown and accompanied by non-Gaussian noise is realized. The method can be used in the field of multi-underwater vehicle co-location under non-ideal conditions.
The main advantages of the invention are as follows:
the method takes the course angle as an unknown input item to reconstruct a nonlinear system equation, introduces the CKF to process the nonlinear system equation and the measurement equation, and has higher practical value;
according to the invention, the course angle and the positioning information are estimated by using the robust two-stage CKF through U-V conversion, so that the decoupling of position estimation and course estimation is completed, and the calculation cost is effectively reduced;
the invention combines the MCC method to establish a recursion model based on an unknown input nonlinear equation, which is used for updating the posterior estimation and covariance matrix of the state sub-filter, can effectively weaken the interference of non-Gaussian noise, and has a better solution for problems possibly occurring in an actual scene;
the invention utilizes the innovation matrix and the prior measurement covariance matrix to construct the self-adaptive factor, utilizes the self-adaptive factor to adjust the bandwidth on line, and has higher practical value.
Drawings
FIG. 1 is a schematic diagram of acoustic communication of co-located relative distance information;
FIG. 2 is an actual navigation track diagram of a plurality of submersible vehicles of the co-location system;
FIG. 3 is a schematic diagram of the probability distribution of actual measurement noise and noise in the co-location process;
FIG. 4 is a comparison of positioning errors;
FIG. 5 is a comparison of course angle estimates;
FIG. 6 is a comparison of heading angle errors.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
The method comprises the following steps: establishing a multi-underwater vehicle co-location model with unknown course angle;
the method takes a master-slave cooperative positioning mode as an example, and considers a classical scene that two pilot submergible devices alternately provide measurement information for a slave submergible device. The communication diagram is shown in figure 1. The equation of state and the measurement equation are as follows
Figure BDA0003079966230000061
Figure BDA0003079966230000062
In the formula a k And b k East and north positions of the slave underwater vehicle at the moment k, delta t is a sampling period, v a,k ,v b,k Is the right and forward speed, theta, at time k, of the slave vehicle k Is the angle between the forward direction and the north direction at time k, w a,k ,w b,k Is zero-mean white gaussian noise. Z is a linear or branched member k The relative distance measurement information between the pilot underwater vehicle and the slave underwater vehicle,
Figure BDA0003079966230000063
is the position of the pilot vehicle at time k, epsilon k To measure noise.
The compass circuit structure is complex, and especially when the compass with low cost is used, the fault probability is greatly increased. Will theta k As an estimated variable, satisfy
Figure BDA0003079966230000064
In the formula
Figure BDA0003079966230000071
Is zero mean white noise.
The discrete time state space model is established as follows
Figure BDA0003079966230000072
Figure BDA0003079966230000073
In the formula
Figure BDA0003079966230000074
x k ＝[a k b k ] T Is the position of the satellite vehicle at time k, n a N +1, n is x k The dimension (c) of (a) is,
Figure BDA0003079966230000075
and
Figure BDA0003079966230000076
respectively nonlinear state function and measurement function, assuming
Figure BDA0003079966230000077
Is a white gaussian noise sequence and is characterized in that,
Figure BDA0003079966230000078
and is provided with
Figure BDA0003079966230000079
In the formula x k ＝[a k b k ] T Is the position of the slave vehicle at time k, u k ＝[v a,k v b,k ] T The forward and right speed measured for the DVL,
Figure BDA00030799662300000710
is an estimate of the heading angle at time k-1.
In the formula
Figure BDA00030799662300000711
Figure BDA00030799662300000712
In the formula (I), the compound is shown in the specification,
Figure BDA00030799662300000713
is a non-linear state function.
Step two: inputting unknown calculation of a state estimation value to be corrected by a course angle;
error covariance of state estimate
Figure BDA00030799662300000714
Can be decomposed into
Figure BDA00030799662300000715
In the formula S k-1/k-1 Is composed of
Figure BDA00030799662300000716
By triangulationAnd (5) solving the lower triangular matrix. Next, the volume point is calculated as follows
χ k-1,i ＝S k-1/k-1 ρ i +x k-1/k-1 (10)
In the formula
Figure BDA00030799662300000717
[I n ] i Representing an n-dimensional identity matrix I n Column i.
Volume point propagation is as follows
Figure BDA00030799662300000718
Figure BDA00030799662300000719
Figure BDA0003079966230000081
Figure BDA0003079966230000082
y k/k-1,i ＝h k (χ k/k-1,i ) (15)
Figure BDA0003079966230000083
In the formula
Figure BDA0003079966230000084
As a function of the nonlinear state f k-1 (ii) the spread out sample points,
Figure BDA0003079966230000085
in order to be the state prediction value,
Figure BDA0003079966230000086
is composed of
Figure BDA0003079966230000087
Of the error covariance matrix χ k/k-1,i Is a secondary sampling point, y k/k-1,i As a function of the non-linear measurement h k (ii) the spread out sample points,
Figure BDA0003079966230000088
for measuring the predicted value, Q k Is a process noise covariance matrix, ξ i And rho i The values are the same.
The measurement is updated as follows
Figure BDA0003079966230000089
Figure BDA00030799662300000810
Figure BDA00030799662300000811
Figure BDA00030799662300000812
Figure BDA00030799662300000813
In the formula
Figure BDA00030799662300000814
Is a cross-covariance matrix of the two-dimensional data,
Figure BDA00030799662300000815
to measure the auto-covariance matrix, R k To measure the noise covariance matrix, K k Is a gain matrix, y k In order to actually measure the information, the measurement device is provided with a sensor,
Figure BDA00030799662300000816
respectively, a state estimation value and an error covariance matrix when no course input exists.
Step three: the influence of outlier noise on state estimation is weakened by adaptively updating a measurement noise variance matrix;
adaptive updating of the metric noise variance matrix, i.e.
Figure BDA00030799662300000817
And is provided with
Figure BDA00030799662300000818
Wherein T is P,k|k-1 And T r,k Are respectively as
Figure BDA00030799662300000819
And the measured noise variance matrix R k And (5) performing triangle decomposition on the lower triangular matrix. In the co-location model, the system noise is assumed to be Gaussian noise, and thus there are
Figure BDA00030799662300000820
Where σ is the nuclear band width, the adaptive adjustment process will be described in step four, and
e k ＝ζ k -G k x k (24)
in the formula
Figure BDA0003079966230000091
Figure BDA0003079966230000092
m is the dimension of the measurement.
Step four: constructing a nuclear band width self-adaptive adjustment factor;
an innovation vector is defined as
Figure BDA0003079966230000093
By an information matrix
Figure BDA00030799662300000913
And a measurement error covariance matrix
Figure BDA0003079966230000094
Construction of adaptive adjustment factor as follows
Figure BDA0003079966230000095
In the formula
Figure BDA0003079966230000096
When the trace of the innovation matrix is less than or equal to the trace of the measurement error variance matrix, the value of the adaptive factor is 1, otherwise, the adaptive factor means that an abnormal value exists in the measurement information. In this case, the bandwidth σ is corrected in real time using the constructed adaptation factor, i.e., σ t ＝λ t σ t-1 And t is the number of iterations.
Step five: calculating a course estimation value and a corrected position vector estimation value;
the covariance matrix of the error of the course angle is
Figure BDA0003079966230000097
In the formula
Figure BDA0003079966230000098
The gain matrix of the position and heading estimation is respectively
Figure BDA0003079966230000099
Figure BDA00030799662300000910
The course estimation value is
Figure BDA00030799662300000911
The covariance matrices of the position estimation and the position estimation error after correction by the course angle estimation value are respectively
Figure BDA00030799662300000912
Figure BDA0003079966230000101
In the formula
Figure BDA0003079966230000102
Fig. 1 is a schematic diagram of acoustic communication of co-location relative distance information, and here, a classical scenario in which 2 pilot submergible vehicles provide measurement information for 1 satellite submergible vehicle in turn is considered. The navigator is provided with high-precision navigation equipment as communication and navigation auxiliary equipment, and the follower is provided with low-precision equipment such as compass and DVL (dynamic video language) and the like, so that speed and course information are acquired, and dead reckoning is carried out. In addition, both the navigator and the follower are equipped with a underwater modem. In the co-location process, the relative distance between the pilot and the follower is measured at intervals by the underwater acoustic device. Fig. 2 is an actual sailing track diagram of the multiple underwater vehicles of the co-location system, and it can be seen that the satellite underwater vehicle is located between the two pilot underwater vehicles, and the formation can improve observability of the system. Fig. 3 is a schematic diagram of the probability distribution of the actual measurement noise and noise in the co-location process, and it can be seen that the measurement noise no longer follows the gaussian distribution due to the existence of the measurement outlier. Our aim is to guarantee the positioning accuracy by estimating the heading angle. Fig. 4 is a comparison graph of the positioning errors of the RCKF, the MCRCKF under different values of the nuclear bandwidth σ, and the proposed AMCRCKF, and it can be seen that, under the non-gaussian measurement noise and compass ineffective conditions, the RCKF positioning error curve diverges violently, and the maximum positioning error reaches 380m, which indicates that the compass ineffective method RCKF cannot maintain good positioning accuracy under the non-gaussian measurement noise environment. Thus, MCC is introduced to handle non-gaussian noise, but as can be seen from fig. 4, improper bandwidth also leads to filter divergence. The AMCRCKF algorithm can adaptively adjust the width of a nuclear band to a reasonable value, the positioning error is within 20m, and the AMCRCKF algorithm still has better positioning accuracy under the conditions of compass failure and non-Gaussian distribution of measurement noise. Fig. 5 and fig. 6 are respectively a comparison graph of the heading angle estimated value and the heading angle error, and it can be seen that the heading angle error fluctuation of the RCKF and the MCRCKF is large, which has a great negative influence on the positioning accuracy, and the estimated heading angle of the AMCRCKF is closer to the actual heading angle.

Claims (3)

1. A multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure is characterized by comprising the following steps:
the method comprises the following steps: establishing a multi-underwater vehicle co-location model with unknown course angle;
step two: calculating a heading angle input unknown state estimation value to be corrected;
step three: the influence of outlier noise on state estimation is weakened by adaptively updating the measurement noise variance matrix, and the method specifically comprises the following steps:
adaptive updating of the metric noise variance matrix, namely:
Figure FDA0003820851330000011
and is provided with
Figure FDA0003820851330000012
Wherein T is P,k|k-1 And T r,k Are respectively as
Figure FDA0003820851330000013
And the measured noise variance matrix R k A lower triangular matrix after triangular decomposition; in the co-location model, the system noise is assumed to be gaussian noise, so there are:
Figure FDA0003820851330000014
where σ is the nuclear band width and has:
e k ＝ζ k -G k x k
in the formula
Figure 1
m is the dimension of the measured value, ε k To measure noise;
step four: constructing a nuclear band width self-adaptive adjustment factor, which specifically comprises the following steps:
an innovation vector is defined as
Figure FDA0003820851330000016
By an innovation matrix
Figure FDA0003820851330000017
And a measurement error covariance matrix
Figure FDA0003820851330000018
Constructing an adaptive adjustment factor:
Figure FDA0003820851330000019
Figure FDA00038208513300000110
in the formula
Figure FDA00038208513300000111
When the trace of the innovation matrix is less than or equal to the trace of the measurement error variance matrixWhen the bandwidth sigma is corrected, the adaptive factor value is 1, otherwise, the bandwidth sigma is corrected in real time by using the constructed adaptive factor, namely sigma t ＝λ t σ t-1 T is the number of iterations;
step five: calculating a course estimation value and a corrected position vector estimation value, specifically:
the error covariance matrix of the course angle is:
Figure FDA0003820851330000021
in the formula
Figure FDA0003820851330000022
The gain matrix of the position and course estimation is respectively:
Figure FDA0003820851330000023
Figure FDA0003820851330000024
the course estimation value is as follows:
Figure FDA0003820851330000025
the covariance matrices of the position estimation and position estimation errors after correction by the course angle estimation value are respectively:
Figure FDA0003820851330000026
Figure FDA0003820851330000027
in the formula
Figure FDA0003820851330000028
2. The method for multi-underwater-vehicle cooperative localization adaptive regulation robust filtering under compass failure according to claim 1, characterized in that: step one, establishing a multi-underwater vehicle co-location model with unknown course angle specifically comprises the following steps:
the equation of state and the measurement equation are as follows:
Figure FDA0003820851330000029
Figure FDA00038208513300000210
in the formula a k And b k East and north positions of the slave underwater vehicle at the moment k, delta t is a sampling period, v a,k ,v b,k Is the right and forward speed, theta, at time k, of the slave vehicle k Is the angle between the forward direction and the north direction at time k, w a,k ,w b,k Is zero mean white Gaussian noise, Z k The relative distance measurement information between the pilot underwater vehicle and the slave underwater vehicle,
Figure FDA00038208513300000211
for the position of the piloting vehicle at time k, epsilon k To measure noise;
will theta k As an estimated variable, satisfy
Figure FDA00038208513300000212
In the formula
Figure FDA0003820851330000031
Is zero mean white noise;
the discrete time state space model establishment specifically comprises the following steps:
Figure FDA0003820851330000032
Figure FDA0003820851330000033
in the formula
Figure FDA0003820851330000034
x k ＝[a k b k ] T Is the position of the slave vehicle at time k, n a N +1, n is x k The dimension(s) of (a) is,
Figure FDA0003820851330000035
and
Figure FDA0003820851330000036
respectively, the nonlinear state function and the measurement function at the time k after state expansion, assuming
Figure FDA0003820851330000037
Is a sequence of white gaussian noise, and is,
Figure FDA0003820851330000038
y k is the real measurement information, and comprises:
Figure FDA0003820851330000039
in the formula u k ＝[v a,k v b,k ] T The forward and right speed measured for the DVL,
Figure FDA00038208513300000310
the estimated value of the course angle at the k-1 moment is obtained;
in the formula
Figure FDA00038208513300000311
Figure FDA00038208513300000312
In the formula (I), the compound is shown in the specification,
Figure FDA00038208513300000313
as a non-linear state function at time k.
3. The method for multi-underwater-vehicle co-location adaptive adjustment robust filtering under compass failure according to claim 1 or 2, wherein: step two, the calculation of the unknown state estimation value to be corrected of the course angle input is specifically as follows:
error covariance of state estimates
Figure FDA00038208513300000314
Can be decomposed into:
Figure FDA00038208513300000315
in the formula S k-1/k-1 Is composed of
Figure FDA00038208513300000316
A lower triangular matrix after triangular decomposition;
the volume point calculation is specifically:
Figure FDA00038208513300000317
in the formula
Figure FDA00038208513300000318
[I n ] i Representing an n-dimensional identity matrix I n The ith column;
volume point propagation is specifically:
Figure FDA00038208513300000319
Figure FDA0003820851330000041
Figure FDA0003820851330000042
Figure FDA0003820851330000043
y k/k-1,i ＝h k (χ k/k-1,i )
Figure FDA0003820851330000044
in the formula
Figure FDA0003820851330000045
As a function of the nonlinear state at time k-1 k-1 (c) the spread out sample points,
Figure FDA0003820851330000046
in order to be a predicted value of the state,
Figure FDA0003820851330000047
is composed of
Figure FDA0003820851330000048
Of the error covariance matrix χ k/k-1,i Is a secondary sampling point, y k/k-1,i As a function of the non-linear measurement h k (ii) the spread out sample points,
Figure FDA0003820851330000049
for measuring the predicted value, Q k Is a process noise covariance matrix, ξ i And rho i The value taking modes are the same;
the measurement is updated as follows:
Figure FDA00038208513300000410
Figure FDA00038208513300000411
Figure FDA00038208513300000412
Figure FDA00038208513300000413
Figure FDA00038208513300000414
in the formula
Figure FDA00038208513300000415
Is a cross-covariance matrix of the two-dimensional data,
Figure FDA00038208513300000416
to measure the autocovariance matrix, R k Covariance for measuring noiseDifference matrix, K k Is a gain matrix, y k In order to obtain the real measurement information,
Figure FDA00038208513300000417
respectively a state estimation value and an error covariance matrix when no course input exists.
CN202110563429.1A 2021-05-24 2021-05-24 Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure Active CN113324546B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110563429.1A CN113324546B (en) 2021-05-24 2021-05-24 Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110563429.1A CN113324546B (en) 2021-05-24 2021-05-24 Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure

Publications (2)

Publication Number Publication Date
CN113324546A CN113324546A (en) 2021-08-31
CN113324546B true CN113324546B (en) 2022-12-13

Family

ID=77416362

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110563429.1A Active CN113324546B (en) 2021-05-24 2021-05-24 Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure

Country Status (1)

Country Link
CN (1) CN113324546B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114659526B (en) * 2022-02-11 2024-07-23 北京空间飞行器总体设计部 Spacecraft autonomous navigation robust filtering algorithm based on sequence image state expression
CN116702479B (en) * 2023-06-12 2024-02-06 哈尔滨工程大学 Unknown input and position estimation method and system for underwater vehicle
CN116680500B (en) * 2023-06-12 2024-03-22 哈尔滨工程大学 Position estimation method and system of underwater vehicle under non-Gaussian noise interference

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013130042A1 (en) * 2012-02-28 2013-09-06 Intel Corporation Dynamic optimization of carrier recovery performance for communications systems
CN104090271A (en) * 2014-07-30 2014-10-08 广西大学 Space time adaptive processing (STAP) method of high-speed radar at accelerated movement state
WO2018073626A1 (en) * 2016-10-20 2018-04-26 Harman Becker Automotive Systems Gmbh Noise control
CN109858454A (en) * 2019-02-15 2019-06-07 东北大学 One kind being based on dual model self-adaptive kernel correlation filtering method for tracing

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2252170A1 (en) * 1998-10-27 2000-04-27 Bruno Bessette A method and device for high quality coding of wideband speech and audio signals
US9020088B2 (en) * 2012-09-12 2015-04-28 Topcon Positioning Systems, Inc. Digital system and method of estimating quasi-harmonic signal non-energy parameters using a digital Phase Locked Loop
PT3011560T (en) * 2013-06-21 2018-11-09 Fraunhofer Ges Forschung Audio decoder having a bandwidth extension module with an energy adjusting module
CN104866715A (en) * 2015-05-14 2015-08-26 同济大学 Electrical power system robust state estimation method based on self-adaptive kernel density estimation
CN109459040B (en) * 2019-01-14 2021-06-18 哈尔滨工程大学 Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on RBF (radial basis function) neural network assisted volume Kalman filtering
EP3816672A1 (en) * 2019-10-28 2021-05-05 FRAUNHOFER-GESELLSCHAFT zur Förderung der angewandten Forschung e.V. Adaptive weighting matrix for adaptive tracking loops
CN110779519B (en) * 2019-11-18 2021-04-27 哈尔滨工程大学 Underwater vehicle single beacon positioning method with global convergence
CN110779518B (en) * 2019-11-18 2021-04-27 哈尔滨工程大学 Underwater vehicle single beacon positioning method with global convergence
CN112099060B (en) * 2020-08-25 2022-11-25 北京理工大学 Loop-based adaptive carrier frequency tracking method and device
CN112566060B (en) * 2020-12-18 2022-04-22 天津工业大学 Marine ranching underwater and overwater integrated communication system based on small-float anchor chain networking

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013130042A1 (en) * 2012-02-28 2013-09-06 Intel Corporation Dynamic optimization of carrier recovery performance for communications systems
CN104090271A (en) * 2014-07-30 2014-10-08 广西大学 Space time adaptive processing (STAP) method of high-speed radar at accelerated movement state
WO2018073626A1 (en) * 2016-10-20 2018-04-26 Harman Becker Automotive Systems Gmbh Noise control
CN109858454A (en) * 2019-02-15 2019-06-07 东北大学 One kind being based on dual model self-adaptive kernel correlation filtering method for tracing

Also Published As

Publication number Publication date
CN113324546A (en) 2021-08-31

Similar Documents

Publication Publication Date Title
CN113324546B (en) Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure
CN109459019B (en) Vehicle navigation calculation method based on cascade adaptive robust federal filtering
Xiong et al. Fault-tolerant GNSS/SINS/DVL/CNS integrated navigation and positioning mechanism based on adaptive information sharing factors
CN108134640B (en) Cooperative positioning system and method based on node motion state constraint
Gao et al. Adaptive Kalman filtering with recursive noise estimator for integrated SINS/DVL systems
CN113819906A (en) Combined navigation robust filtering method based on statistical similarity measurement
CN109974706B (en) Master-slave mode multi-AUV collaborative navigation method based on double motion model
CN110231636B (en) Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system
CN112525218B (en) Robust intelligent cooperative calibration method for INS/DVL (inertial navigation System/digital visual logging) integrated navigation system
CN109883426B (en) Dynamic distribution and correction multi-source information fusion method based on factor graph
CN102323602A (en) Carrier tracking loop based on self-adaptive second-order Kalman filter and filtering method of carrier tracking loop
CN106597507B (en) The Fast High-Precision Algorithm of GNSS/SINS tight integration filtering
CN110567455B (en) Tightly-combined navigation method for quadrature updating volume Kalman filtering
CN107607977B (en) Self-adaptive UKF (unscented Kalman Filter) integrated navigation method based on minimum skewness simplex sampling
CN112083457A (en) IMM satellite positioning and navigation method based on neural network optimization
CN116520380A (en) Dynamic target combined positioning method based on self-adaptive Kalman filtering
CN115113646A (en) Satellite formation flat root state continuous estimation method and system based on Kalman filtering
CN112444837B (en) RTK positioning method for estimating single-difference ambiguity
CN117419723A (en) Interactive model self-adaptive integrated navigation method based on factor graph
CN113434806A (en) Robust adaptive multi-model filtering method
CN114459489B (en) Multi-star formation distributed relative navigation method based on information fusion
CN115047505B (en) GNSS positioning method and navigation method based on carrier phase differential assistance
CN108226976B (en) Self-adaptive fading Kalman filtering algorithm for RTK
Liu et al. Effective Sage-Husa Kalman filter for SINS/Doppler/Platform compass integrated navigation system
CN110045408B (en) Satellite/inertia deep coupling method based on code phase approximation

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