CN113324546A - 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
CN113324546A
CN113324546A CN202110563429.1A CN202110563429A CN113324546A CN 113324546 A CN113324546 A CN 113324546A CN 202110563429 A CN202110563429 A CN 202110563429A CN 113324546 A CN113324546 A CN 113324546A
Authority
CN
China
Prior art keywords
formula
matrix
noise
adaptive
measurement
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
CN202110563429.1A
Other languages
Chinese (zh)
Other versions
CN113324546B (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 invention realizes accurate positioning under the condition that the course angle is unknown and accompanied by 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 akAnd bkEast and north positions of the slave underwater vehicle at the moment k, delta t is a sampling period, va,k,vb,kIs the right and forward speed, theta, at time k, of the slave vehiclekIs the angle between the forward direction and the north direction at time k, wa,k,wb,kIs Gaussian of zero mean valueWhite noise, ZkThe relative distance measurement information between the pilot underwater vehicle and the slave underwater vehicle,
Figure BDA0003079966230000023
for the position of the piloting vehicle at time k, epsilonkTo measure noise;
will thetakAs 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
xk＝[ak bk]TIs the position of the slave vehicle at time k, naN is x +1kThe dimension(s) 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 uk＝[va,k vb,k]TThe 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 non-linear 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 estimates
Figure BDA0003079966230000031
Can be decomposed into:
Figure BDA0003079966230000032
in the formula Sk-1/k-1Is composed of
Figure BDA0003079966230000033
A lower triangular matrix after triangular decomposition;
the volume point calculation is specifically:
χk-1,i＝Sk-1/k-1ρi+xk-1/k-1
in the formula
Figure BDA0003079966230000034
[In]iRepresenting an n-dimensional identity matrix InThe ith column;
volume point propagation is specifically:
Figure BDA0003079966230000035
Figure BDA0003079966230000036
Figure BDA0003079966230000037
Figure BDA0003079966230000038
yk/k-1,i＝hk(χk/k-1,i)
Figure BDA0003079966230000039
in the formula
Figure BDA00030799662300000310
As a function of the nonlinear state fk-1(ii) 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,iIs a secondary sampling point, yk/k-1,iAs a function of the non-linear measurement hk(ii) the spread out sample points,
Figure BDA00030799662300000314
for measuring the predicted value, QkIs a process noise covariance matrix, ξiAnd rhoiThe 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, RkTo measure the noise covariance matrix, KkIs a gain matrix, ykIn order to obtain the real measurement information,
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 isP,k|k-1And Tr,kAre respectively as
Figure BDA0003079966230000044
And the measured noise variance matrix RkA 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:
ek＝ζk-Gkxk
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 used for correcting the bandwidth sigma in real time, namely sigmat＝λtσt-1And 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 akAnd bkEast and north positions of the slave underwater vehicle at the moment k, delta t is a sampling period, va,k,vb,kIs the right and forward speed, theta, at time k, of the slave vehiclekIs the angle between the forward direction and the north direction at time k, wa,k,wb,kIs zero mean white gaussian noise. ZkThe relative distance measurement information between the pilot underwater vehicle and the slave underwater vehicle,
Figure BDA0003079966230000063
for the position of the piloting vehicle at time k, epsilonkTo 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 thetakAs 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
xk＝[ak bk]TIs the position of the slave vehicle at time k, naN is x +1kThe dimension(s) of (a) is,
Figure BDA0003079966230000075
and
Figure BDA0003079966230000076
respectively nonlinear state function and measurement function, assuming
Figure BDA0003079966230000077
Is a sequence of white gaussian noise, and is,
Figure BDA0003079966230000078
and is provided with
Figure BDA0003079966230000079
In the formula xk＝[ak bk]TIs the position of the slave vehicle at time k, uk＝[va,k vb,k]TThe 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 estimates
Figure BDA00030799662300000714
Can be decomposed into
Figure BDA00030799662300000715
In the formula Sk-1/k-1Is composed of
Figure BDA00030799662300000716
And (5) performing triangle decomposition on the lower triangular matrix. Next, the volume point is calculated as follows
χk-1,i＝Sk-1/k-1ρi+xk-1/k-1 (10)
In the formula
Figure BDA00030799662300000717
[In]iRepresenting an n-dimensional identity matrix InColumn i.
Volume point propagation is as follows
Figure BDA00030799662300000718
Figure BDA00030799662300000719
Figure BDA0003079966230000081
Figure BDA0003079966230000082
yk/k-1,i＝hk(χk/k-1,i) (15)
Figure BDA0003079966230000083
In the formula
Figure BDA0003079966230000084
As a function of the nonlinear state fk-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,iIs a secondary sampling point, yk/k-1,iAs a function of the non-linear measurement hk(ii) the spread out sample points,
Figure BDA0003079966230000088
for measuring the predicted value, QkIs a process noise covariance matrix, ξiAnd rhoiThe 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
In the form of a cross-covariance matrix,
Figure BDA00030799662300000815
to measure the auto-covariance matrix, RkTo measure the noise covariance matrix, KkIs a gain matrix, ykIn order to obtain the real measurement information,
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 isP,k|k-1And Tr,kAre respectively as
Figure BDA00030799662300000819
And the measured noise variance matrix RkAnd (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
ek＝ζk-Gkxk (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 innovation 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 measured information. In this case, the bandwidth σ is corrected in real time using the constructed adaptation factor, i.e., σt＝λtσt-1And t is the number of iterations.
Step five: calculating a course estimation value and a corrected position vector estimation value;
the error covariance matrix of 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. During co-location, the relative distance between the pilot and the follower is measured at intervals by the underwater acoustic device. Fig. 2 is an actual navigation track diagram of multiple underwater vehicles of the co-location system, and it can be seen that the slave underwater vehicle is located between 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. The purpose of the method is to ensure the positioning accuracy by estimating the course 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 (6)

1. A multi-underwater vehicle collaborative positioning self-adaptive regulation 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 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.
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 FDA0003079966220000011
Figure FDA0003079966220000012
in the formula akAnd bkEast and north positions of the slave underwater vehicle at the moment k, delta t is a sampling period, va,k,vb,kIs the right and forward speed, theta, at time k, of the slave vehiclekIs the angle between the forward direction and the north direction at time k, wa,k,wb,kIs zero mean white Gaussian noise, ZkThe relative distance measurement information between the pilot underwater vehicle and the slave underwater vehicle,
Figure FDA0003079966220000013
for the position of the piloting vehicle at time k, epsilonkTo measure noise;
will thetakAs an estimated variable, satisfy
Figure FDA0003079966220000014
In the formula
Figure FDA0003079966220000015
Is zero mean white noise;
the discrete time state space model establishment specifically comprises the following steps:
Figure FDA0003079966220000016
Figure FDA0003079966220000017
in the formula
Figure FDA0003079966220000018
xk＝[ak bk]TIs the position of the slave vehicle at time k, naN is x +1kThe dimension(s) of (a) is,
Figure FDA0003079966220000019
and
Figure FDA00030799662200000110
respectively nonlinear state function and measurement function, assuming
Figure FDA00030799662200000111
Is a sequence of white gaussian noise, and is,
Figure FDA00030799662200000112
and has the following components:
Figure FDA00030799662200000113
in the formula uk＝[va,k vb,k]TThe forward and right speed measured for the DVL,
Figure FDA00030799662200000114
the estimated value of the course angle at the moment of k-1;
in the formula
Figure FDA0003079966220000021
Figure FDA0003079966220000022
In the formula (I), the compound is shown in the specification,
Figure FDA0003079966220000023
is a non-linear state function.
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 FDA0003079966220000024
Can be decomposed into:
Figure FDA0003079966220000025
in the formula Sk-1/k-1Is composed of
Figure FDA0003079966220000026
A lower triangular matrix after triangular decomposition;
the volume point calculation is specifically:
χk-1,i＝Sk-1/k-1ρi+xk-1/k-1
in the formula
Figure FDA0003079966220000027
[In]iRepresenting an n-dimensional identity matrix InThe ith column;
volume point propagation is specifically:
Figure FDA0003079966220000028
Figure FDA0003079966220000029
Figure FDA00030799662200000210
Figure FDA00030799662200000211
yk/k-1,i＝hk(χk/k-1,i)
Figure FDA00030799662200000212
in the formula
Figure FDA00030799662200000213
As a function of the nonlinear state fk-1(ii) the spread out sample points,
Figure FDA00030799662200000214
in order to be the state prediction value,
Figure FDA00030799662200000215
is composed of
Figure FDA00030799662200000216
Of the error covariance matrix χk/k-1,iIs a secondary sampling point, yk/k-1,iAs a function of the non-linear measurement hk(ii) the spread out sample points,
Figure FDA00030799662200000217
for measuring the predicted value, QkIs a process noise covariance matrix, ξiAnd rhoiThe values are the same.
The measurement is updated as follows:
Figure FDA0003079966220000031
Figure FDA0003079966220000032
Figure FDA0003079966220000033
Figure FDA0003079966220000034
Figure FDA0003079966220000035
in the formula
Figure FDA0003079966220000036
In the form of a cross-covariance matrix,
Figure FDA0003079966220000037
to measure the auto-covariance matrix, RkFor measuringNoise covariance matrix, KkIs a gain matrix, ykIn order to obtain the real measurement information,
Figure FDA0003079966220000038
respectively, a state estimation value and an error covariance matrix when no course input exists.
4. The method according to claim 3, wherein the adaptive robust filtering method for collaborative positioning of multiple underwater vehicles under compass failure comprises: the third step of weakening 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 FDA0003079966220000039
and is provided with
Figure FDA00030799662200000310
Wherein T isP,k|k-1And Tr,kAre respectively as
Figure FDA00030799662200000311
And the measured noise variance matrix RkA lower triangular matrix after triangular decomposition; in the co-location model, the system noise is assumed to be gaussian noise, so there are:
Figure FDA00030799662200000312
where σ is the nuclear band width and has:
ek＝ζk-Gkxk
in the formula
Figure FDA00030799662200000313
Figure FDA00030799662200000314
m is the dimension of the measurement.
5. The method according to claim 4, wherein the adaptive robust filtering method for collaborative positioning of multiple underwater vehicles under compass failure comprises: the step four of constructing the nuclear band width adaptive adjustment factor specifically comprises the following steps:
an innovation vector is defined as
Figure FDA00030799662200000315
By an innovation matrix
Figure FDA00030799662200000316
And a measurement error covariance matrix
Figure FDA00030799662200000317
Constructing an adaptive adjustment factor:
Figure FDA0003079966220000041
in the formula
Figure FDA0003079966220000042
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 used for correcting the bandwidth sigma in real time, namely sigmat＝λtσt-1And t is the number of iterations.
6. The method according to claim 5, wherein the adaptive robust filtering method for collaborative positioning of multiple underwater vehicles under compass failure comprises: step five, the calculation of the course estimation value and the corrected position vector estimation value specifically comprise:
the error covariance matrix of the course angle is:
Figure FDA0003079966220000043
in the formula
Figure FDA0003079966220000044
The gain matrix of the position and course estimation is respectively:
Figure FDA0003079966220000045
Figure FDA0003079966220000046
the course estimation value is as follows:
Figure FDA0003079966220000047
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 FDA0003079966220000048
Figure FDA0003079966220000049
in the formula
Figure FDA00030799662200000410
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 true CN113324546A (en) 2021-08-31
CN113324546B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116680500A (en) * 2023-06-12 2023-09-01 哈尔滨工程大学 Position estimation method and system of underwater vehicle under non-Gaussian noise interference
CN116702479A (en) * 2023-06-12 2023-09-05 哈尔滨工程大学 Unknown input and position estimation method and system for underwater vehicle

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2347743A1 (en) * 1998-10-27 2000-05-04 Voiceage Corporation A method and device for adaptive bandwidth pitch search in coding wideband signals
WO2013130042A1 (en) * 2012-02-28 2013-09-06 Intel Corporation Dynamic optimization of carrier recovery performance for communications systems
US20140072084A1 (en) * 2012-09-12 2014-03-13 Topcon Positioning Systems, Inc. Digital system and method of estimating quasi-harmonic signal non-energy parameters using a digital Phase Locked Loop
CN104090271A (en) * 2014-07-30 2014-10-08 广西大学 Space time adaptive processing (STAP) method of high-speed radar at accelerated movement state
WO2014202701A1 (en) * 2013-06-21 2014-12-24 Fraunhofer-Gesellschaft zur Förderung der angewandten Forschung e.V. 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
WO2018073626A1 (en) * 2016-10-20 2018-04-26 Harman Becker Automotive Systems Gmbh Noise control
CN109459040A (en) * 2019-01-14 2019-03-12 哈尔滨工程大学 More AUV co-located methods based on RBF neural auxiliary volume Kalman filtering
CN109858454A (en) * 2019-02-15 2019-06-07 东北大学 One kind being based on dual model self-adaptive kernel correlation filtering method for tracing
CN110779518A (en) * 2019-11-18 2020-02-11 哈尔滨工程大学 Underwater vehicle single beacon positioning method with global convergence
CN110779519A (en) * 2019-11-18 2020-02-11 哈尔滨工程大学 Underwater vehicle single beacon positioning method with global convergence
CN112099060A (en) * 2020-08-25 2020-12-18 北京理工大学 Self-adaptive carrier frequency tracking method and device based on loop
CN112566060A (en) * 2020-12-18 2021-03-26 天津工业大学 Marine ranching underwater and overwater integrated communication system based on small-float anchor chain networking
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

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2347743A1 (en) * 1998-10-27 2000-05-04 Voiceage Corporation A method and device for adaptive bandwidth pitch search in coding wideband signals
WO2013130042A1 (en) * 2012-02-28 2013-09-06 Intel Corporation Dynamic optimization of carrier recovery performance for communications systems
US20140072084A1 (en) * 2012-09-12 2014-03-13 Topcon Positioning Systems, Inc. Digital system and method of estimating quasi-harmonic signal non-energy parameters using a digital Phase Locked Loop
WO2014202701A1 (en) * 2013-06-21 2014-12-24 Fraunhofer-Gesellschaft zur Förderung der angewandten Forschung e.V. Audio decoder having a bandwidth extension module with an energy adjusting module
CN104090271A (en) * 2014-07-30 2014-10-08 广西大学 Space time adaptive processing (STAP) method of high-speed radar at accelerated movement state
CN104866715A (en) * 2015-05-14 2015-08-26 同济大学 Electrical power system robust state estimation method based on self-adaptive kernel density estimation
WO2018073626A1 (en) * 2016-10-20 2018-04-26 Harman Becker Automotive Systems Gmbh Noise control
CN109459040A (en) * 2019-01-14 2019-03-12 哈尔滨工程大学 More AUV co-located methods based on RBF neural auxiliary volume Kalman filtering
CN109858454A (en) * 2019-02-15 2019-06-07 东北大学 One kind being based on dual model self-adaptive kernel correlation filtering method for tracing
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
CN110779518A (en) * 2019-11-18 2020-02-11 哈尔滨工程大学 Underwater vehicle single beacon positioning method with global convergence
CN110779519A (en) * 2019-11-18 2020-02-11 哈尔滨工程大学 Underwater vehicle single beacon positioning method with global convergence
CN112099060A (en) * 2020-08-25 2020-12-18 北京理工大学 Self-adaptive carrier frequency tracking method and device based on loop
CN112566060A (en) * 2020-12-18 2021-03-26 天津工业大学 Marine ranching underwater and overwater integrated communication system based on small-float anchor chain networking

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
BO XU等: ""Cooperative Localization in Harsh Underwater Environment Based on the MC-ANFIS"", 《IEEE ACCESS》 *
BO XU等: ""Maximum Correntropy Extend Kalman Filter for Multiple AUVs Cooperative Localization"", 《2019 CHINESE CONTROL AND DECISION CONFERENCE (CCDC)》 *
DANIELA OSTERRIEDER等: ""THE VOLATILITY OF LONG-TERM BOND RETURNS: PERSISTENT INTEREST SHOCKS AND TIME-VARYING RISK PREMIUMS"", 《REVIEW OF ECONOMICS & STATISTICS》 *
徐博，等: ""一种基于自适应神经模糊推理***的多 AUV 协同定位方法"", 《中国惯性技术学报》 *
马煦，等: ""基于带宽比例因子的导航信号带宽优化"", 《数字通信世界》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116680500A (en) * 2023-06-12 2023-09-01 哈尔滨工程大学 Position estimation method and system of underwater vehicle under non-Gaussian noise interference
CN116702479A (en) * 2023-06-12 2023-09-05 哈尔滨工程大学 Unknown input and position estimation method and system for underwater vehicle
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

Also Published As

Publication number Publication date
CN113324546B (en) 2022-12-13

Similar Documents

Publication Publication Date Title
CN113324546B (en) Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure
Xiong et al. Fault-tolerant GNSS/SINS/DVL/CNS integrated navigation and positioning mechanism based on adaptive information sharing factors
CN110231636B (en) Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system
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
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
CN102323602A (en) Carrier tracking loop based on self-adaptive second-order Kalman filter and filtering method of carrier tracking loop
US8229661B2 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN110031798A (en) A kind of indoor objects tracking based on simplified Sage-Husa adaptive-filtering
CN106646543A (en) High-dynamic satellite navigation signal carrier tracking method based on master-slave AUKF algorithm
CN107515414A (en) Method for adaptive kalman filtering based on vector tracking under a kind of ionospheric scintillation
CN112083457A (en) IMM satellite positioning and navigation method based on neural network optimization
CN116680500B (en) Position estimation method and system of underwater vehicle under non-Gaussian noise interference
CN115047505B (en) GNSS positioning method and navigation method based on carrier phase differential assistance
CN110912535A (en) Novel pilot-free Kalman filtering method
CN116520380A (en) Dynamic target combined positioning method based on self-adaptive Kalman filtering
CN107421543B (en) Implicit function measurement model filtering method based on state dimension expansion
CN112444837B (en) RTK positioning method for estimating single-difference ambiguity
Liu et al. Effective Sage-Husa Kalman filter for SINS/Doppler/Platform compass integrated navigation system
CN103281054A (en) Self adaption filtering method adopting noise statistic estimator
CN113376672A (en) Beidou pseudo-range positioning algorithm based on fuzzy Kalman filtering
Li et al. An Improved Multiple-Outlier Robust Filter Based on Maximum Correntropy Criterion for Integrated Navigation
CN114593724B (en) Cluster fusion positioning method and device

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