CN116680500B - Position estimation method and system of underwater vehicle under non-Gaussian noise interference - Google Patents
Position estimation method and system of underwater vehicle under non-Gaussian noise interference Download PDFInfo
- Publication number
- CN116680500B CN116680500B CN202310690292.5A CN202310690292A CN116680500B CN 116680500 B CN116680500 B CN 116680500B CN 202310690292 A CN202310690292 A CN 202310690292A CN 116680500 B CN116680500 B CN 116680500B
- Authority
- CN
- China
- Prior art keywords
- state
- error
- estimation
- underwater vehicle
- expression
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 70
- 238000005259 measurement Methods 0.000 claims abstract description 50
- 239000011159 matrix material Substances 0.000 claims abstract description 46
- 230000003190 augmentative effect Effects 0.000 claims abstract description 19
- 230000003416 augmentation Effects 0.000 claims abstract description 8
- 238000010276 construction Methods 0.000 claims description 25
- 230000008569 process Effects 0.000 claims description 24
- 238000005457 optimization Methods 0.000 claims description 15
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 6
- 230000007423 decrease Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000008045 co-localization Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012938 design process Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000012417 linear regression Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 230000003313 weakening effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Computing Systems (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses a method and a system for estimating the position of an underwater vehicle under non-Gaussian noise interference, wherein the method comprises the following steps: constructing a co-location system state space model according to the acquired position data, speed data and heading data of the underwater vehicle; on the basis of obtaining an optimized measurement equation based on linearization error compensation, an augmented state model is constructed; constructing a cost function based on a student' st kernel function and a minimum error entropy criterion by using an augmentation state model; based on the cost function, obtaining a weighted least square expression form of posterior state estimation; based on the weighted least square expression form of the posterior state estimation, the final expression form of the posterior state estimation and the corresponding estimation error covariance matrix is obtained by using matrix inversion primer, and the position estimation of the underwater vehicle is completed. The invention can effectively weaken the influence of non-Gaussian noise on the position estimation and improve the positioning precision of the underwater vehicle in a non-ideal working environment.
Description
Technical Field
The invention belongs to the technical field of underwater vehicle co-location, and particularly relates to a method and a system for estimating the position of an underwater vehicle under non-Gaussian noise interference.
Background
The complex underwater environment exacerbates the uncertainty in measuring noise. In actual co-localization, multipath propagation between the sound source and the receiver can lead to outliers in the range measurement. In addition, the speed of sound varies with depth and salinity, and the DVL water lock effect also causes the measured speed to be the speed of water rather than the speed to ground, and the resulting speed uncertainty may also lead to the occurrence of outliers. The occurrence of outliers is often intermittent in time and measuring outliers may cause non-gaussian noise in thick tail distributions.
Since the conventional filtering algorithm is derived under gaussian noise assumption, it is not robust to non-gaussian noise. At present, a series of improved methods based on an optimization criterion, such as a robust optimization algorithm based on a Huber criterion, a robust optimization algorithm based on a maximum cross-correlation entropy criterion and the like, are proposed, and all the improved algorithms weaken the influence of non-Gaussian noise to a certain extent. However, the Huber kernel function used in the design process of the optimization algorithm is slow to decline, so that the response to a large outlier is not fast enough; the gaussian kernel function may cause loss of effective measurement data due to too fast a falling speed, and may easily cause a decrease in numerical stability. The existence of these problems makes the existing method unable to better cope with interference of non-gaussian noise, resulting in a decrease in positioning accuracy under non-gaussian noise.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a method and a system for estimating the position of an underwater vehicle under the interference of non-Gaussian noise, which can effectively weaken the influence of the non-Gaussian noise on the position estimation and improve the positioning precision of the underwater vehicle in a non-ideal working environment.
In order to achieve the above object, the present invention provides the following solutions:
a method of estimating a position of an underwater vehicle under non-gaussian noise interference, comprising the steps of:
s1: constructing a co-location system state space model according to the acquired position data, speed data and heading data;
s2: based on the state space model of the co-location system, obtaining an optimized measurement equation for linearization error compensation, and constructing an augmented state model based on the optimized measurement equation for linearization error compensation;
s3: constructing a cost function based on a student's t kernel function and a minimum error entropy criterion by using the augmentation state model;
s4: based on the cost function, obtaining a weighted least square expression form of posterior state estimation;
s5: and based on the weighted least square expression form of the posterior state estimation, obtaining a final expression form of the posterior state estimation and a corresponding estimation error covariance matrix by using matrix inversion primer, and completing the position estimation of the underwater vehicle.
Preferably, in the step S1, the expression of the co-location system state space model is:
wherein x is k ∈R n×1 ，z k ∈R m×1 F (·) is the state transfer function, h (·) is the measurement function, and E (ω) k δ l T )＝0；
The discrete time model expression is:
wherein the position coordinates of the navigator and the follower at the time k are respectively as followsx k ＝[a k ,b k ] T ，z k For the relative distance between the pilot and the follower, (-) T For transpose operation, Δt is the sampling period, ω k-1 ＝[q x,k-1 ,q y,k-1 ] T And delta k Process noise and measurement noise, respectively, +.>Is a velocity vector, wherein>And->Forward and right speeds, respectively +.>Is heading.
Preferably, in the step S2, the method for constructing the augmented state model based on the optimized measurement equation for linearization error compensation includes:
wherein,is a one-step predicted value of state, H k For observing matrix +.>For measuring one-step predictive value,/->To amplify the noise vector, a state error vector and a measurement error vector are included, i.e.)>Wherein,vk is the error brought by the linearization process;
the covariance matrix of the augmented noise vector is expressed as:
wherein Σ is p/k,k-1 ，Σ r/k Sum sigma k Respectively by pairs ofΥ k +R k And->After Cholesky decomposition, statistical linearization error v k Considered as compensation factors in the recursive model, for compensating errors introduced by the linearization process,
the method further comprises the following steps:
three amounts of which satisfyWherein τ k ＝[τ 1,k ,τ 2,k ,...,τ n+m,k ] T ，ζ k ＝[ζ 1,k ,ζ 2,k ,...,ζ n+m,k ] T 。
Preferably, in the step S3, the method for constructing the cost function based on the student' S t kernel function and the minimum error entropy criterion includes:
the expression of the student's t kernel function is specifically:
wherein v and σ are two important parameters of the kernel function, σ determines the kernel band width, v is used to flexibly adjust the kernel function shape, ζ i And zeta j Respectively representing the i-th and j-th error amounts;
based on the minimum error entropy theory and the student' st kernel function, the obtained optimization cost function is as follows:
wherein n=n+m is the sum of the state quantity and the dimension of the observed quantity, N is the state dimension, and m is the measurement dimension;
the expression for optimizing the estimate is:
preferably, in the step S4, the method for obtaining the weighted least squares expression form of the posterior state estimation based on the cost function includes:
the cost function is biased and the bias of the cost function is set to be 0, and the bias of the cost function is expressed as a matrix form:
the state estimation result is obtained through fixed point iteration, and the specific expression is:
wherein,
preferably, in the step S5, the method for obtaining the final expression form of the posterior state estimation and the corresponding estimation error covariance matrix by using matrix inversion primer includes:
wherein the gain matrixPrediction error covariance matrix->And measurement error variance matrix->The expression form of (a) is as follows:
the corresponding posterior state estimation error covariance matrix is:
wherein,
the invention also provides a position estimation system of an underwater vehicle under non-Gaussian noise interference, comprising: the device comprises a first construction module, a second construction module, a third construction module, a weighting module and a position estimation module;
the first construction module is used for constructing a co-location system state space model according to the acquired position data, speed data and heading data;
the second construction module is used for obtaining an optimized measurement equation for linearization error compensation based on the state space model of the co-location system, and constructing an augmented state model based on the optimized measurement equation for linearization error compensation;
the third construction module is used for constructing a cost function based on a student's t kernel function and a minimum error entropy criterion by using the augmentation state model;
the weighting module is used for obtaining a weighted least square expression form of posterior state estimation based on the cost function;
the position estimation module is used for obtaining a final expression form of the posterior state estimation and the corresponding estimation error covariance matrix by using matrix inversion theorem based on the weighted least square expression form of the posterior state estimation, and completing position estimation of the underwater vehicle.
Preferably, in the first construction module, the expression of the co-location system state space model is:
wherein x is k ∈R n×1 ，z k ∈R m×1 F (·) is the state transfer function, h (·) is the measurement function, and E (ω) k δ l T )＝0；
The discrete time model expression is:
wherein the position coordinates of the navigator and the follower at the time k are respectively as followsx k ＝[a k ,b k ] T ，z k For the relative distance between the pilot and the follower, (-) T For transpose operation, Δt is the sampling period, ω k-1 ＝[q x,k-1 ,q y,k-1 ] T And delta k Process noise and measurement noise, respectively, +.>Is a velocity vector, wherein>And->Forward and right respectivelyDirection speed, or->Is heading.
Preferably, in the second building module, the process of building the augmented state model based on the optimized measurement equation for linearization error compensation includes:
wherein,is a one-step predicted value of state, H k For observing matrix +.>For measuring one-step predictive value,/->To amplify the noise vector, a state error vector and a measurement error vector are included, i.e.)>Wherein (1)>v k Errors brought about by the linearization process;
the covariance matrix of the augmented noise vector is expressed as:
wherein Σ is p/k,k-1 ，Σ r/k Sum sigma k Respectively by pairs ofΥ k +R k And->After Cholesky decomposition, statistical linearization error v k Considered as compensation factors in the recursive model, for compensating errors introduced by the linearization process,
the method further comprises the following steps:
three amounts of which satisfyWherein τ k ＝[τ 1,k ,τ 2,k ,...,τ n+m,k ] T ，ζ k ＝[ζ 1,k ,ζ 2,k ,...,ζ n+m,k ] T 。
Preferably, in the third construction module, the method for constructing the cost function based on the student's t kernel function and the minimum error entropy criterion includes:
the expression of the student's t kernel function is specifically:
wherein v and σ are two important parameters of the kernel function, σ determines the kernel band width, v is used to flexibly adjust the kernel function shape, ζ i And zeta j Respectively representing the i-th and j-th error amounts;
based on the minimum error entropy theory and the student' st kernel function, the obtained optimization cost function is as follows:
wherein n=n+m is the sum of the state quantity and the dimension of the observed quantity, N is the state dimension, and m is the measurement dimension;
the expression for optimizing the estimate is:
compared with the prior art, the invention has the beneficial effects that:
the invention considers the co-location problem under the interference of non-Gaussian noise in a complex underwater environment, and researches a robust volume Kalman filtering method based on a student's t kernel function under the non-Gaussian noise. For the problem of weakening non-Gaussian noise interference, the existing robust optimization algorithm based on the Huber criterion, the robust optimization algorithm based on the maximum cross-correlation entropy criterion and the like all achieve certain effects, but are influenced by the fact that a kernel function drops too slowly or drops to 0 too quickly, so that the improved method based on the optimization criterion cannot deal with the non-Gaussian noise interference more flexibly, and the positioning accuracy is reduced under the non-Gaussian noise.
Aiming at the problems that the existing robust optimization algorithm is slowly influenced by the descent of a used kernel function and the response to a large abnormal value is not fast enough, or the descent speed of the kernel function is too fast, so that the loss of effective measurement data is likely to be caused, and the numerical stability is easy to be reduced, the invention introduces a student's t kernel function in the construction process of the cost function, uses a more sufficient minimum error entropy criterion to construct the cost function in combination with error data, effectively improves the sensitivity of the kernel function to non-Gaussian noise, and improves the position estimation precision of the system under the non-Gaussian noise. The invention can be used in the field of co-positioning of multiple submarines under non-ideal conditions.
Drawings
In order to more clearly illustrate the technical solutions of the present invention, the drawings that are needed in the embodiments are briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic flow chart of a method for estimating a position of an underwater vehicle under non-Gaussian noise interference in an embodiment of the invention;
FIG. 2 is a schematic illustration of actual navigation of a co-location system in accordance with an embodiment of the present invention;
FIG. 3 is a diagram showing a comparison of positioning errors under non-Gaussian noise for different methods according to an embodiment of the invention;
FIG. 4 is a graph showing the comparison of average positioning errors under different methods according to an embodiment of the present invention;
FIG. 5 is a graph showing the comparison of kernel functions under different parameters according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
Example 1
As shown in fig. 1, the invention discloses a method for estimating the position of an underwater vehicle under non-gaussian noise interference, which comprises the following steps:
s1: constructing a co-location system state space model according to the acquired position data, speed data and heading data;
s2: based on the state space model of the co-location system, obtaining an optimized measurement equation for linearization error compensation, and constructing an augmented state model based on the optimized measurement equation for linearization error compensation;
s3: constructing a cost function based on a student's t kernel function and a minimum error entropy criterion by using an augmentation state model;
s4: based on the cost function, obtaining a weighted least square expression form of posterior state estimation;
s5: based on the weighted least square expression form of the posterior state estimation, the final expression form of the posterior state estimation and the corresponding estimation error covariance matrix is obtained by using matrix inversion primer, and the position estimation of the underwater vehicle is completed.
In this embodiment, in S1, the method for constructing the co-location system state space model according to the acquired position data, speed data and heading data includes:
the discrete time model expression is as follows:
wherein the position coordinates of the navigator and the follower at the moment k are respectively as followsx k ＝[a k ,b k ] T 。z k For the relative distance between the pilot and the follower, (-) T For transpose operation, Δt is the sampling period, ω k-1 ＝[q x,k-1 ,q y,k-1 ] T And delta k Process noise and measurement noise, respectively. />Is a velocity vector, wherein>And->Forward and right speeds, respectively +.>Is heading.
The expression form of the system state space model is as follows:
wherein x is k ∈R n×1 ,z k ∈R m×1 F (·) is the state transfer function, h (·) is the measurement function, and E (ω) k δ l T )＝0。
In this embodiment, in the step S2, an augmented state model is constructed on the basis of obtaining an optimized measurement equation based on linearization error compensation;
wherein the method comprises the steps ofIs a one-step predicted value of state, H k For observing matrix +.>For measuring one-step predictive value,/->To amplify the noise vector, it contains a state error vector and a measurement error vector, i.e. +.>Wherein->v k Errors introduced into the linearization process.
The covariance matrix of the augmented noise vector is expressed as
Wherein Σ is p/k,k-1 ，Σ r/k Sum sigma k Can be respectively through pairs ofΥ k +R k And->Is obtained after Cholesky decomposition. Statistical linearization error v k Can be regarded as a compensation factor in the recursive model for compensating errors caused by the linearization process.
The method further comprises the following steps:
three amounts of which satisfyWherein τ k ＝[τ 1,k ,τ 2,k ,...,τ n+m,k ] T ，ζ k ＝[ζ 1,k ,ζ 2,k ,...,ζ n+m,k ] T 。
In this embodiment, in the step S3, the cost function construction is completed based on the student' S t kernel function and the minimum error entropy criterion;
the expression form of the student's t kernel function is specifically as follows:
where v and σ are two important parameters of the kernel function, σ determines the kernel band width, v is used to flexibly adjust the kernel function shape, ζ i And zeta j The i-th and j-th error amounts are indicated, respectively.
Based on the minimum error entropy theory and the student' st kernel function, the obtained optimization cost function is as follows:
where n=n+m is the sum of the state quantity and the dimension of the observed quantity, N is the state dimension, and m is the measurement dimension.
The expression form of the optimization estimate is as follows:
in this embodiment, in S4, a weighted least squares expression form of the posterior state estimation is solved;
to obtain an optimal solution, the cost function is biased to 0 and expressed in matrix form to simplify the expression form of the double summation:
wherein the method comprises the steps of
The state estimation result can be obtained through fixed-point iteration, and the concrete expression form is as follows:
wherein the method comprises the steps of
In this embodiment, in S5, a final expression form of the posterior state estimation and the corresponding estimation error covariance matrix is obtained by using matrix inversion theory.
Wherein the gain matrixPrediction error covariance matrix->And measurement error variance matrix->The expression form of (a) is as follows:
the corresponding posterior state estimation error covariance matrix is:
wherein the method comprises the steps of
Fig. 2 is a diagram of the actual sailing trajectory of a co-location system with a plurality of navigators, here we consider a classical scenario of 1 follower for 2 navigators, during which the relative distance between the navigators and the followers is measured at intervals by means of a hydroacoustic device. Two pilots are located on each side of the follower, and this formation can improve the observability of the system. Fig. 3 is a comparison diagram of the positioning error of the StKCKF and the existing algorithm, which is presented in the present invention, it can be seen that, when the initial kernel bandwidth is not properly selected, the existing algorithm has a larger error when an abnormal value is measured, and the StKCKF method can more flexibly cope with non-gaussian noise interference due to the use of the auxiliary parameter v, so as to ensure the positioning accuracy. Fig. 4 is a comparison of the average positioning errors under multiple monte carlo experiments, and it can be seen that the gaussian kernel function converges to 0 too fast, which may cause a numerical stability problem, resulting in a large deviation, while the StKCKF method is stable, and the errors remain within a reasonable range all the time. Fig. 5 is a graph comparing kernel functions under different parameters and positioning errors under different parameters, and it can be seen that the student's t kernel function is flexible to change, so that a rapid and reasonable reaction can be made under a non-gaussian noise environment, and the position estimation accuracy is effectively ensured.
Example two
The invention also provides a position estimation system of an underwater vehicle under non-Gaussian noise interference, comprising: the device comprises a first construction module, a second construction module, a third construction module, a weighting module and a position estimation module;
the first construction module is used for constructing a co-location system state space model according to the acquired position data, speed data and heading data;
the second construction module is used for obtaining an optimized measurement equation of linearization error compensation based on the state space model of the co-location system and constructing an augmented state model based on the optimized measurement equation of linearization error compensation;
the third construction module is used for constructing a cost function based on a student's t kernel function and a minimum error entropy criterion by using the augmentation state model;
the weighting module is used for obtaining a weighted least square expression form of posterior state estimation based on the cost function;
the position estimation module is used for obtaining the final expression form of the posterior state estimation and the corresponding estimation error covariance matrix by using matrix inversion quotients based on the weighted least square expression form of the posterior state estimation, and completing the position estimation of the underwater vehicle.
In this embodiment, in the first building module, the expression of the co-location system state space model is:
wherein x is k ∈R n×1 ，z k ∈R m×1 F (·) is the state transfer function, h (·) is the measurement function, and E (ω) k δ l T )＝0；
The discrete time model expression is:
wherein the position coordinates of the navigator and the follower at the time k are respectively as followsx k ＝[a k ,b k ] T ，z k For the relative distance between the pilot and the follower, (-) T For transpose operation, Δt is the sampling period, ω k-1 ＝[q x,k-1 ,q y,k-1 ] T And delta k Process noise and measurement noise, respectively, +.>Is a velocity vector, wherein>And->Forward and right speeds, respectively +.>Is heading.
In this embodiment, in the second building module, the process of building the augmented state model based on the optimized measurement equation for linearization error compensation includes:
wherein,is a one-step predicted value of state, H k For observing matrix +.>For measuring one-step predictive value,/->To amplify the noise vector, a state error vector and a measurement error vector are included, i.e.)>Wherein,v k errors brought about by the linearization process;
the covariance matrix of the augmented noise vector is expressed as:
wherein Σ is p/k,k-1 ，Σ r/k Sum sigma k Respectively by pairs ofΥ k +R k And->After Cholesky decomposition, statistical linearization error v k Considered as compensation factors in the recursive model, for compensating errors introduced by the linearization process,
the method further comprises the following steps:
three amounts of which satisfyWherein τ k ＝[τ 1,k ,τ 2,k ,...,τ n+m,k ] T ，ζ k ＝[ζ 1,k ,ζ 2,k ,...,ζ n+m,k ] T 。
In this embodiment, in the third building module, the method for building the cost function based on the student's t kernel function and the minimum error entropy criterion includes:
the expression of the student's t kernel function is specifically:
wherein v and σ are two important parameters of the kernel function, σ determines the kernel band width, v is used to flexibly adjust the kernel function shape, ζ i And zeta j Respectively representing the i-th and j-th error amounts;
based on the minimum error entropy theory and the student' st kernel function, the obtained optimization cost function is as follows:
wherein n=n+m is the sum of the state quantity and the dimension of the observed quantity, N is the state dimension, and m is the measurement dimension;
the expression for optimizing the estimate is:
in this embodiment, the process of obtaining the weighted least squares expression form of the posterior state estimation in the weighting module based on the cost function includes:
the cost function is biased and the bias of the cost function is set to be 0, and the bias of the cost function is expressed as a matrix form:
the state estimation result is obtained through fixed point iteration, and the specific expression is:
wherein,/>
in this embodiment, in the location estimation module, the method for obtaining the final expression form of the posterior state estimation and the corresponding estimation error covariance matrix by using matrix inversion primer includes:
wherein the gain matrixPrediction error covariance matrix->And measurement error variance matrix->The expression form of (a) is as follows:
the corresponding posterior state estimation error covariance matrix is:
wherein,
the main advantages of the invention are as follows:
according to the invention, a statistical linear regression method is adopted to linearize a nonlinear measurement equation of a co-location system, a statistical linearization error covariance is deduced, and an augmentation model is constructed on the basis, so that the influence of linearization error is weakened;
according to the invention, the student's t kernel function is introduced, so that the objective function can reasonably process more complex non-Gaussian noise, thereby better capturing thick tail and multimodal characteristics of noise, and the method is more adaptive in complex actual application scenes;
the invention uses the fixed point iteration method to update the posterior state estimation, the convergence of which is proved, and the validity of the fixed point iteration output result is effectively ensured, thereby ensuring the precision of the posterior state estimation result.
The above embodiments are merely illustrative of the preferred embodiments of the present invention, and the scope of the present invention is not limited thereto, but various modifications and improvements made by those skilled in the art to which the present invention pertains are made without departing from the spirit of the present invention, and all modifications and improvements fall within the scope of the present invention as defined in the appended claims.
Claims (6)
1. The method for estimating the position of the underwater vehicle under the interference of non-Gaussian noise is characterized by comprising the following steps:
s1: constructing a co-location system state space model according to the acquired position data, speed data and heading data of the underwater vehicle;
s2: based on the state space model of the co-location system, obtaining an optimized measurement equation for linearization error compensation, and constructing an augmented state model based on the optimized measurement equation for linearization error compensation;
s3: constructing a cost function based on a student's t kernel function and a minimum error entropy criterion by using the augmentation state model;
s4: based on the cost function, obtaining a weighted least square expression form of posterior state estimation;
s5: based on the weighted least square expression form of the posterior state estimation, obtaining a final expression form of the posterior state estimation and a corresponding estimation error covariance matrix by using matrix inversion primer, and completing the position estimation of the underwater vehicle;
in the step S1, the expression of the co-location system state space model is:
wherein x is k ∈R n×1 ，z k ∈R m×1 F (·) is the state transfer function, h (·) is the measurement function, and E (ω) k δ l T )＝0；
The discrete time model expression is:
wherein the position coordinates of the navigator and the follower at the time k are respectively as followsx k ＝[a k ,b k ] T ，z k For the relative distance between the pilot and the follower, (-) T For transpose operation, Δt is the sampling period, ω k-1 ＝[q x,k-1 ,q y,k-1 ] T And delta k Process noise and measurement noise, respectively, +.>Is a velocity vector, wherein>And->Forward and right speeds, respectively +.>Is a heading;
in the step S3, the method for constructing the cost function based on the student' S t kernel function and the minimum error entropy criterion comprises the following steps:
the expression of the student's t kernel function is specifically:
wherein v and σ are two important parameters of the kernel function, σ determines the kernel band width, v is used to flexibly adjust the kernel function shape, ζ i And zeta j Respectively representing the i-th and j-th error amounts;
based on the minimum error entropy theory and the student's t kernel function, the obtained optimization cost function is as follows:
wherein n=n+m is the sum of the state quantity and the dimension of the observed quantity, N is the state dimension, and m is the measurement dimension;
the expression for optimizing the estimate is:
2. the method for estimating a position of an underwater vehicle under non-gaussian noise interference according to claim 1, wherein in S2, the method for constructing an augmented state model based on the optimized measurement equation for linearization error compensation comprises:
wherein,is a one-step predicted value of state, H k For observing matrix +.>For measuring one-step predictive value,/->To amplify the noise vector, a state error vector and a measurement error vector are included, i.e. +.>Wherein,v k errors brought about by the linearization process;
the covariance matrix of the augmented noise vector is expressed as:
wherein Σ is p/k,k-1 ，Σ r/k Sum sigma k Respectively by pairs ofΥ k +R k And->After Cholesky decomposition, statistical linearization error v k Considered as compensation factors in the recursive model, for compensating errors introduced by the linearization process,
the method further comprises the following steps:
three amounts of which satisfyWherein-> ζ k ＝[ζ 1,k ,ζ 2,k ,...,ζ n+m,k ] T 。
3. The method for estimating a position of an underwater vehicle under non-gaussian noise interference according to claim 1, wherein in S4, the method for obtaining a weighted least squares expression of a posterior state estimate based on the cost function comprises:
the cost function is biased and the bias of the cost function is set to be 0, and the bias of the cost function is expressed as a matrix form:
the state estimation result is obtained through fixed point iteration, and the specific expression is:
wherein,
4. the method for estimating a position of an underwater vehicle under non-gaussian noise interference according to claim 1, wherein in S5, the method for obtaining a final expression form of a posterior state estimate and a corresponding estimated error covariance matrix using matrix inversion theorem comprises:
wherein the gain matrixPrediction error covariance matrix->And a measurement error variance matrix->The expression form of (a) is as follows:
the corresponding posterior state estimation error covariance matrix is:
wherein,
5. a system for estimating a position of an underwater vehicle under non-gaussian noise interference, comprising: the device comprises a first construction module, a second construction module, a third construction module, a weighting module and a position estimation module;
the first construction module is used for constructing a co-location system state space model according to the acquired position data, speed data and heading data;
the second construction module is used for obtaining an optimized measurement equation for linearization error compensation based on the state space model of the co-location system, and constructing an augmented state model based on the optimized measurement equation for linearization error compensation;
the third construction module is used for constructing a cost function based on a student's t kernel function and a minimum error entropy criterion by using the augmentation state model;
the weighting module is used for obtaining a weighted least square expression form of posterior state estimation based on the cost function;
the position estimation module is used for obtaining a final expression form of the posterior state estimation and a corresponding estimation error covariance matrix by using matrix inversion theorem based on the weighted least square expression form of the posterior state estimation, and completing position estimation of the underwater vehicle;
in the first construction module, the expression of the co-location system state space model is:
wherein x is k ∈R n×1 ，z k ∈R m×1 F (·) is the state transfer function, h (·) is the measurement function, and E (ω) k δ l T )＝0；
The discrete time model expression is:
wherein the position coordinates of the navigator and the follower at the time k are respectively as followsx k ＝[a k ,b k ] T ，z k For the relative distance between the pilot and the follower, (-) T For transpose operation, Δt is the sampling period, ω k-1 ＝[q x,k-1 ,q y,k-1 ] T And delta k Process noise and measurement noise, respectively, +.>Is a velocity vector, wherein>And->Forward and right speeds, respectively +.>Is a heading;
in the third construction module, the method for constructing the cost function based on the student's t kernel function and the minimum error entropy criterion comprises the following steps:
the expression of the student's t kernel function is specifically:
wherein v and σ are two important parameters of the kernel function, σ determines the kernel band width, v is used to flexibly adjust the kernel function shape, ζ i And zeta j Respectively representing the i-th and j-th error amounts;
based on the minimum error entropy theory and the student's t kernel function, the obtained optimization cost function is as follows:
wherein n=n+m is the sum of the state quantity and the dimension of the observed quantity, N is the state dimension, and m is the measurement dimension;
the expression for optimizing the estimate is:
6. the system for estimating a position of an underwater vehicle under non-gaussian noise interference according to claim 5, wherein the process of constructing an augmented state model based on the optimized metrology equation for linearization error compensation in the second construction module comprises:
wherein,is a one-step predicted value of state, H k For observing matrix +.>For measuring one-step predictive value,/->To amplify the noise vector, a state error vector and a measurement error vector are included, i.e. +.>Wherein (1)>v k Errors brought about by the linearization process;
the covariance matrix of the augmented noise vector is expressed as:
wherein Σ is p/k,k-1 ，Σ r/k Sum sigma k Respectively by pairs ofΥ k +R k And->After Cholesky decomposition, statistical linearization error v k Considered as compensation factors in the recursive model, for compensating errors introduced by the linearization process,
the method further comprises the following steps:
three amounts of which satisfyWherein-> ζ k ＝[ζ 1,k ,ζ 2,k ,...,ζ n+m,k ] T 。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310690292.5A CN116680500B (en) | 2023-06-12 | 2023-06-12 | Position estimation method and system of underwater vehicle under non-Gaussian noise interference |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310690292.5A CN116680500B (en) | 2023-06-12 | 2023-06-12 | Position estimation method and system of underwater vehicle under non-Gaussian noise interference |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116680500A CN116680500A (en) | 2023-09-01 |
| CN116680500B true CN116680500B (en) | 2024-03-22 |
Family
ID=87780652
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310690292.5A Active CN116680500B (en) | 2023-06-12 | 2023-06-12 | Position estimation method and system of underwater vehicle under non-Gaussian noise interference |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116680500B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117973429B (en) * | 2024-04-01 | 2024-06-07 | 南京信息工程大学 | Model parameter ratio estimation method applied to non-Gaussian noise filtering |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109459040A (en) * | 2019-01-14 | 2019-03-12 | 哈尔滨工程大学 | More AUV co-located methods based on RBF neural auxiliary volume Kalman filtering |
| 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 |
| CN110749891A (en) * | 2019-10-21 | 2020-02-04 | 哈尔滨工程大学 | Self-adaptive underwater single beacon positioning method capable of estimating unknown effective sound velocity |
| CN113324546A (en) * | 2021-05-24 | 2021-08-31 | 哈尔滨工程大学 | Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure |
| WO2022088797A1 (en) * | 2020-10-26 | 2022-05-05 | 东南大学 | Measurement abnormality-considered cooperative localization method for cluster type multi-deep-sea underwater vehicle |
| JP2022123401A (en) * | 2021-02-12 | 2022-08-24 | 三菱重工業株式会社 | Route planning device, moving object, route planning method and program |
| CN115711622A (en) * | 2022-11-21 | 2023-02-24 | 电子科技大学 | Underwater unmanned vehicle positioning method based on generalized minimum error entropy Kalman |
| WO2023045357A1 (en) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | Integrated navigation robust filtering method based on statistical similarity measurement |
-
2023
- 2023-06-12 CN CN202310690292.5A patent/CN116680500B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109459040A (en) * | 2019-01-14 | 2019-03-12 | 哈尔滨工程大学 | More AUV co-located methods based on RBF neural auxiliary volume Kalman filtering |
| 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 |
| CN110749891A (en) * | 2019-10-21 | 2020-02-04 | 哈尔滨工程大学 | Self-adaptive underwater single beacon positioning method capable of estimating unknown effective sound velocity |
| WO2022088797A1 (en) * | 2020-10-26 | 2022-05-05 | 东南大学 | Measurement abnormality-considered cooperative localization method for cluster type multi-deep-sea underwater vehicle |
| JP2022123401A (en) * | 2021-02-12 | 2022-08-24 | 三菱重工業株式会社 | Route planning device, moving object, route planning method and program |
| CN113324546A (en) * | 2021-05-24 | 2021-08-31 | 哈尔滨工程大学 | Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure |
| WO2023045357A1 (en) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | Integrated navigation robust filtering method based on statistical similarity measurement |
| CN115711622A (en) * | 2022-11-21 | 2023-02-24 | 电子科技大学 | Underwater unmanned vehicle positioning method based on generalized minimum error entropy Kalman |
Non-Patent Citations (3)
| Title |
|---|
| Maximum Correntropy Extend Kalman Filter for Multiple AUVs Cooperative Localization;Bo Xu 等;《2019 Chinese control and decision conference(CCDC)》;第4670-4675页 * |
| Student’s t-Kernel-Based Maximum Correntropy Kalman Filter;Hongliang Huang 等;《Sensors》;第1-18页 * |
| 基于四阵元基站的水下航行器自定位方法;翟京生 等;《天津大学学报(自然科学与工程技术版)》;第56卷(第3期);第252-259页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116680500A (en) | 2023-09-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109459040B (en) | Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on RBF (radial basis function) neural network assisted volume Kalman filtering | |
| CN107677272B (en) | AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering | |
| CN116680500B (en) | Position estimation method and system of underwater vehicle under non-Gaussian noise interference | |
| CN104112079A (en) | Fuzzy adaptive variational Bayesian unscented Kalman filter method | |
| Xu et al. | Cooperative localization in harsh underwater environment based on the MC-ANFIS | |
| CN103217175A (en) | Self-adaptive volume Kalman filtering method | |
| Jwo et al. | Adaptive Kalman filter for navigation sensor fusion | |
| Bai et al. | A novel mixture distributions-based robust Kalman filter for cooperative localization | |
| CN110794409A (en) | Underwater single beacon positioning method capable of estimating unknown effective sound velocity | |
| Xu et al. | A novel adaptive filter for cooperative localization under time-varying delay and non-Gaussian noise | |
| CN108226887B (en) | Water surface target rescue state estimation method under condition of transient observation loss | |
| CN115307643A (en) | Double-responder assisted SINS/USBL combined navigation method | |
| CN110555398A (en) | Fault diagnosis method for determining first arrival moment of fault based on optimal filtering smoothness | |
| Bai et al. | A Robust Generalized $ t $ Distribution-Based Kalman Filter | |
| JP2002181926A (en) | Method and apparatus for tracking of target | |
| Dektor et al. | Robust adaptive terrain-relative navigation | |
| CN109582915B (en) | Improved nonlinear observability self-adaptive filtering method applied to pure azimuth tracking | |
| CN116772867A (en) | Multi-AUV self-adaptive co-location method and system based on node optimization of factor graph | |
| CN109582914B (en) | Parallel fusion estimation method of noise-related deviation system | |
| CN116088303B (en) | Uncertain complex dynamic network state time-varying recursion estimation method | |
| CN115047505B (en) | GNSS positioning method and navigation method based on carrier phase differential assistance | |
| Hou et al. | Improved exponential weighted moving average based measurement noise estimation for strapdown inertial navigation system/Doppler velocity log integrated system | |
| Dubois et al. | Performance evaluation of a moving horizon estimator for multi-rate sensor fusion with time-delayed measurements | |
| CN114061592B (en) | Adaptive robust AUV navigation method based on multiple models | |
| CN112946697B (en) | Satellite signal cycle slip detection and repair method based on deep learning |
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 |