CN113434806B - Robust adaptive multi-model filtering method - Google Patents
Robust adaptive multi-model filtering method Download PDFInfo
- Publication number
- CN113434806B CN113434806B CN202110173917.1A CN202110173917A CN113434806B CN 113434806 B CN113434806 B CN 113434806B CN 202110173917 A CN202110173917 A CN 202110173917A CN 113434806 B CN113434806 B CN 113434806B
- Authority
- CN
- China
- Prior art keywords
- filter
- model
- matrix
- moment
- probability
- 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
- 238000001914 filtration Methods 0.000 title claims abstract description 57
- 238000000034 method Methods 0.000 title claims abstract description 45
- 230000003044 adaptive effect Effects 0.000 title claims description 20
- 239000011159 matrix material Substances 0.000 claims abstract description 95
- 238000010998 test method Methods 0.000 claims abstract description 6
- 238000005259 measurement Methods 0.000 claims description 52
- 230000007704 transition Effects 0.000 claims description 16
- 230000004927 fusion Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 4
- 239000000203 mixture Substances 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 238000003491 array Methods 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 241000764238 Isis Species 0.000 description 1
- 235000006629 Prosopis spicigera Nutrition 0.000 description 1
- 240000000037 Prosopis spicigera Species 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000000827 velocimetry Methods 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
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Theoretical Computer Science (AREA)
- Pure & Applied Mathematics (AREA)
- Software Systems (AREA)
- Radar, Positioning & Navigation (AREA)
- Algebra (AREA)
- General Engineering & Computer Science (AREA)
- Databases & Information Systems (AREA)
- Remote Sensing (AREA)
- Operations Research (AREA)
- Computing Systems (AREA)
- Automation & Control Theory (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Probability & Statistics with Applications (AREA)
- Navigation (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses an robust self-adaptive multi-model filtering method, which specifically comprises the following steps: step 1: adopting a multi-model filtering structure of three filters, establishing a discrete state space model of an application object, and constructing a measuring noise variance array model set; step 2: calculating the model mixing probability among the filters, the mixing initial state of the filters and the mean square error matrix of the filters; step 3: the three filters respectively and simultaneously perform robust Kalman filtering; step 4: updating the probability of the filter model by adopting a Bayesian hypothesis test method; step 5: the estimated value of each filter is weighted and fused with the probability of the corresponding filter model, and the joint state estimation and the mean square error array are output; step 6: adaptively updating a measuring noise variance matrix model set; step 7: and repeating the steps 2 to 6 until the filtering is finished. The invention can effectively improve the filtering robust capability, adaptively and rapidly estimate the statistical characteristics of the measured noise, and improve the filtering precision.
Description
Technical Field
The invention relates to an robust self-adaptive multi-model filtering method, belongs to a multi-sensor information fusion technology, and is particularly suitable for the field of integrated navigation.
Background
The Kalman filtering is a linear, unbiased and optimal estimation algorithm with minimum error variance, is widely applied to multi-sensor information fusion, has the characteristics of simple algorithm and easy engineering realization, but the algorithm accuracy is highly dependent on the accuracy of a system model, and the statistical characteristics of system noise and measurement noise are required to be accurately known. In practical engineering applications, since the measuring environment of the sensor is generally complex and changeable, the statistical characteristics of the measuring noise are difficult to accurately obtain and have time variability. In addition, even very precise sensors have more or less coarse differences (outliers) in their measurement data. All of the above conditions result in significant degradation of Kalman filtering accuracy and even filter divergence.
Multimodal estimation has effects on the estimation problem in cases where the model of the processing system is uncertain, which other methods do not, but the estimation performance depends largely on the model set. For a fixed-structure multi-model algorithm, in order to cover all possible modes of the system, the model set is designed to be large enough, but not all system modes are completely known, and if the model set is increased, excessive model competition can be caused to affect estimation accuracy instead.
Take underwater inertial navigation/doppler integrated navigation as an example. Because the underwater environment is complex and changeable, the measurement data of the Doppler velocimeter inevitably has rough differences, meanwhile, the statistical characteristics of the measurement noise are often unknown and can change along with the change of the environment, and the filtering precision of a single fixed filter parameter can be reduced in the long-time running process of the system. In addition, the established system model is inevitably error-free. Thus, there is a need for a more efficient filtering method for the above situation.
Disclosure of Invention
In order to reduce the influence of measurement coarse difference and unknown variation of measurement noise statistical characteristics on filtering precision, the invention provides an anti-difference self-adaptive multi-model filtering method. The method can effectively improve the filtering robust capacity, adaptively and rapidly estimate the statistical characteristics of the measured noise, and improve the filtering precision.
The above object of the present invention is achieved by the following technical solutions:
the robust adaptive multi-model filtering method specifically comprises the following steps:
step S1: adopting a multi-model filtering structure of three filters, establishing a discrete state space model of an application object, and constructing a measuring noise variance array model set;
step S2: calculating the model mixing probability among the filters, the mixing initial state of the filters and the mean square error matrix of the filters;
step S3: the three filters respectively and simultaneously perform robust Kalman filtering;
step S4: updating the probability of the filter model by adopting a Bayesian hypothesis test method;
step S5: the estimated value of each filter is weighted and fused with the probability of the corresponding filter model, and the joint state estimation and the mean square error array are output;
step S6: adaptively updating a measuring noise variance matrix model set;
step S7: repeating the steps S2-S6 until the filtering is finished.
Further, the step S1 specifically includes the following steps:
s1.1, a multi-model filtering structure of three filters is adopted, and a discrete state space model of an application object is established, wherein the discrete state space model comprises a state equation and a measurement equation:
wherein,and->State vector and measurement vector of filter j at time k, respectively, < >>State vector for filter j at time k-1,/->For the state one-step transition matrix of the filter j from moment k-1 to moment k +.>System noise vector for time instant k-1 filter j +.>For the system noise input matrix of the k-1 time to k time filter j +.>For the measurement matrix of the k moment filter j, < >>For the k moment filter jJ=1, 2,3.
S1.2 constructing a measurement noise variance matrix model set
Wherein the model is concentratedMeasuring noise variance arrays corresponding to k time filters 1,2,3 respectively,/for each of the k time filters>For the estimated value of the noise variance matrix measured at the moment k, eta is a set positive constant, and for the initial noise variance matrix measured +.>May be given based on a priori knowledge.
Further, the step S2 specifically includes the following steps:
s2.1, assuming that transition among the filters follows a Markov process, and calculating the model mixing probability among the filters according to the model probability among the filters and the Markov transition probability among the filters at the last moment as follows:
wherein,for the model mixture probability, p, of the k-1 moment filter i to filter j i→j For the Markov transition probability of filter i to filter j, +.>For the model probability of the filter i at the time of k-1, the model probability of each filter at the initial time and the Markov transition probability among each filter need to be given according to priori knowledge.
S2.2, calculating the mixed initial state of each filter and a mean square error matrix of each filter:
wherein,and->The mixed initial state of the filter j at the time of k-1 and its corresponding mean square error matrix,and->State estimation and corresponding mean square error matrix of k-1 moment filter i are respectively marked by superscript T Representing the matrix transpose.
Further, the step S3 specifically includes the following steps:
s3.1, calculating a state one-step prediction and a mean square error matrix:
wherein,and->Respectively predicting the state of the filter j in one step and a corresponding mean square error matrix;
s3.2, calculating innovation:
wherein,an innovation vector for the filter j at time k, < >>Is the measurement vector of the filter j at time k.
S3.3 innovation standardization:
wherein,normalized innovation for the ith measurement component in time k filter j +.>For the mean square error estimation of the innovation of the ith measurement component in the k moment filter j,/>To cut off the innovation sequence of the ith measurement component in the current moment filter jThe number med is a median function.
S3.4, innovation M estimation robust processing:
wherein the function omega is a three-section IGGIII equivalent weight function, k 0 And k 1 As an empirical robust parameter, matrixInlet weighting matrix for filter j at time k, < ->And (3) a correction innovation sequence of the filter j at the moment k, wherein m is the dimension of the measurement vector.
S3.6, calculating a filter gain array and an innovation variance array:
wherein,a filter gain matrix for the filter j at time k, < >>Innovative variance matrix for k-moment filter j。
S3.7, calculating state estimation and a mean square error matrix:
wherein,and->The state estimation of the filter j at the k moment and the corresponding mean square error matrix are respectively adopted.
Further, the step S4 specifically includes the following steps:
s4.1, calculating likelihood function values of each filter model by the corrected innovation sequence and the innovation variance array of each filter:
wherein,the likelihood function value of the filter j at the moment k, and m is the dimension of the measurement vector.
S4.2, updating the probability of the filter model by adopting a Bayesian hypothesis test method, and calculating the probability value of each filter model:
wherein,filtering for time kModel probabilities for j.
Further, the step S5 specifically includes the following steps:
and (3) carrying out probability weighted fusion on each filter estimation value and the corresponding filter model, and outputting joint state estimation and a mean square error matrix:
wherein,and P k And respectively estimating the k moment joint state and the corresponding mean square error matrix.
Further, the step S6 specifically includes the following steps:
taking the measured noise variance matrix with the maximum probability of the corresponding filter model as the estimated value of the measured noise variance matrix at the next moment, and adaptively updating the measured noise variance matrix model set:
wherein,measuring noise variance matrix estimation value for k+1 time, ">For the measuring noise variance matrix corresponding to the k moment filter j,>j is equal to the model probability matrix mu k Position number of maximum value>And forming a model set of the k+1 moment measurement noise variance matrix.
Compared with the prior art, the method has the following advantages and beneficial effects:
(1) The invention adopts a variable structure multi-model filtering method, breaks through the limit of a fixed structure multi-model, and the model set can adaptively change towards the current real system model direction, thereby rapidly tracking and estimating the statistical characteristics of the measured noise, having good self-adaptability and solving the problem of filtering precision reduction when the statistical characteristics of the measured noise are unknown and changeable in a complex environment;
(2) According to the invention, an anti-difference filtering method based on M estimation is adopted, and the limitation correction is carried out on the innovation by introducing an equivalent weight matrix, so that the influence of the measurement coarse difference on the filtering precision is effectively reduced, and the filtering anti-difference capability is enhanced.
Drawings
FIG. 1 is a flow chart of a robust adaptive multi-model filtering method according to the present invention;
FIG. 2 is a schematic diagram of a Markov process followed by an inter-filter transition in the robust adaptive multi-model filtering method of the present invention;
FIG. 3 is a schematic diagram of adaptive adjustment of a model set in the robust adaptive multi-model filtering method according to the present invention;
FIG. 4 is a graph of Doppler velocimeter output measurements in an embodiment;
FIG. 5 is a graph of misalignment angle contrast for a robust adaptive multi-model filtering method and a Kalman filtering method according to the present invention in an embodiment;
FIG. 6 is a graph of velocity error contrast in a specific embodiment of the robust adaptive multi-model filtering method and Kalman filtering method according to the present invention;
FIG. 7 is a graph showing the comparison of the position errors of the robust adaptive multi-model filtering method and the Kalman filtering method according to the present invention in a specific embodiment;
fig. 8 is a graph of adaptive estimation of mean square error of measured noise in an embodiment of the robust adaptive multi-model filtering method according to the present invention.
Detailed Description
The technical scheme provided by the present invention will be described in detail below with reference to the accompanying drawings and specific examples, and it should be understood that the following specific embodiments are only for illustrating the present invention and are not intended to limit the scope of the present invention.
Referring to fig. 1, taking underwater inertial navigation/doppler integrated navigation as an example, the robust adaptive multi-model filtering method of the present invention mainly includes the following steps:
step S1: adopting a multi-model filtering structure of three filters, establishing a discrete state space model of the underwater inertial navigation/Doppler integrated navigation system, and constructing a measuring noise variance array model set:
s1.1, establishing a state equation of an underwater inertial navigation (SINS)/Doppler (DVL) integrated navigation system:
wherein X is a state vector, phi x ，φ y ，φ z Respectively represent each axial misalignment angle of the SINS,respectively representing the speed errors in the east, north and sky directions, and δL, δlambda and δh respectively representing the latitude error, longitude error and altitude error, epsilon x ，ε y ，ε z Respectively representing the constant drift of each axial gyro +.>Each axial accelerometer zero offset is represented respectively, δK represents DVL scale factor error, W is system noise vector, F is system matrix, and each symbol in F matrix is represented respectively as
In the method, in the process of the invention,coordinate transformation matrix, ω, representing navigation coordinate system n to carrier coordinate system b ie Indicating the rotational angular velocity of the earth> And->Respectively representing the velocity of the carrier in the three directions of east, north and sky under the navigation coordinate system n, L, lambda and h respectively representing the latitude, longitude and altitude in the traditional earth ellipsoid coordinate system, R M And R is N The curvature radiuses of the earth meridian circle and the mortise unitary circle are respectively, and->And respectively represent the specific force output measured by the accelerometers in the east, north and sky directions.
S1.2, establishing a SINS/DVL integrated navigation system measurement equation:
the difference between the SINS carrier line velocity and the DVL measured external velocity information is used as a quantity measure Z:
wherein,projection of the vector velocity calculated for SINS in the vector coordinate system b, +.>Projection of the actually measured ground speed for DVL in the carrier coordinate system b, +.>Projection of true value of DVL to ground speed measurement in carrier coordinate system b, V n For projection of the carrier speed in the navigation coordinate system n, < >>δV n Projection of carrier velocity error in navigational coordinate system n, V b For the projection of the carrier speed in the carrier coordinate system b, < >> In the calculation, the second order small quantity is omitted and upsilon is calculated as the direction cosine matrix corresponding to the misalignment angle d To measure noise.
According to the calculation, the SINS/DVL integrated navigation system measurement equation can be obtained:
Z＝HX+V
wherein H is a measurement matrix, V is a measurement noise vector, C ij (i, j=1, 2, 3) isIs an element of (a).
S1.3, establishing a filter discrete state space model according to a system state equation and a measurement equation.
Wherein,and->Respectively k time filters jStatus vector and measurement vector, ">State vector for filter j at time k-1,/->For the state one-step transition matrix of the filter j from moment k-1 to moment k +.>System noise vector for time instant k-1 filter j +.>For the system noise input matrix of the k-1 time to k time filter j +.>For the measurement matrix of the k moment filter j, < >>The measured noise vector for the k-moment filter j, j=1, 2,3.
S1.4 constructing a measuring noise variance matrix model set
Wherein the model is concentratedMeasuring noise variance arrays corresponding to k moment models 1,2 and 3 respectively,/>For the estimated value of the noise variance matrix measured at the k moment, eta is a set positive constant. Noise variance matrix estimation for initial measurements/>May be given based on a priori knowledge.
Step S2: calculating the model mixing probability among the filters, the mixing initial state of the filters and the mean square error matrix of the filters:
s2.1 assuming that the inter-filter transition follows a markov process, as shown in fig. 2, the inter-filter model mixture probability is calculated from the inter-filter model probability and the inter-filter markov transition probability at the previous time as:
wherein,for the model mixture probability, p, of the k-1 moment filter i to filter j i→j For the Markov transition probability of filter i to filter j, +.>The model probability for the filter i at time k-1. The probability of each filter model and the probability of the Markov transition between each filter at the initial moment are given according to prior knowledge.
S2.2, calculating the mixed initial state of each filter and a mean square error matrix of each filter:
wherein,and->The mixed initial state of the filter j at the time of k-1 and its corresponding mean square error matrix,and->State estimation and corresponding mean square error matrix of k-1 moment filter i are respectively marked by superscript T Representing the matrix transpose.
Step S3: the three filters respectively and simultaneously perform robust Kalman filtering:
s3.1, calculating a state one-step prediction and a mean square error matrix:
wherein,and->The state one-step prediction of the filter j and its corresponding mean square error matrix, respectively.
S3.2, calculating innovation:
wherein,an innovation vector for the filter j at time k, < >>Is the measurement vector of the filter j at time k.
S3.3 innovation standardization:
wherein,normalized innovation for the ith measurement component in time k filter j +.>For the mean square error estimation of the innovation of the ith measurement component in the k moment filter j,/>To cut off the innovation sequence of the ith measured component in the current moment filter j, the function med is a median function.
S3.4, innovation M estimation robust processing:
wherein the function omega is a three-section IGGIII equivalent weight function, k 0 And k 1 As an empirical robust parameter, matrixInlet weighting matrix for filter j at time k, < ->And (3) a correction innovation sequence of the filter j at the moment k, wherein m is the dimension of the measurement vector.
S3.6, calculating a filter gain array and an innovation variance array:
wherein,a filter gain matrix for the filter j at time k, < >>And the new variance matrix of the filter j at the moment k.
S3.7, calculating state estimation and a mean square error matrix:
wherein,and->State estimation and corresponding mean square error for k time instant filter jAnd (5) a differential array.
Step S4: model probability updating is carried out by adopting a Bayesian hypothesis testing method:
s4.1, calculating likelihood function values of each filter model by the corrected innovation sequence and the innovation variance array of each filter:
wherein,the likelihood function value of the filter j at the moment k, and m is the dimension of the measurement vector.
S4.2, updating the probability of the filter model by adopting a Bayesian hypothesis test method, and calculating the probability value of each filter model:
wherein,the model probability of filter j at time k.
Step S5: and (3) carrying out probability weighted fusion on each filter estimation value and the corresponding filter model, and outputting joint state estimation and a mean square error matrix:
wherein,and P k And respectively estimating the k moment joint state and the corresponding mean square error matrix.
Step S6: adaptively updating a measuring noise variance matrix model set:
taking the measured noise variance matrix with the maximum probability of the corresponding filter model as the estimated value of the measured noise variance matrix at the next moment, and adaptively updating the measured noise variance matrix model set:
wherein,measuring noise variance matrix estimation value for k+1 time, ">For the measuring noise variance matrix corresponding to the k moment filter j,>j is equal to the filter model probability matrix mu k The position number of the maximum value in the (c),and forming a model set of the k+1 moment measurement noise variance matrix. FIG. 3 shows a schematic view of adaptation of the model set, wherein FIG. 3 (a) is +.>Corresponding filter model probability->In the case of maximum value, FIG. 3 (b) is +.>Corresponding filter model probability->In the case of maximum value, FIG. 3 (c) is +.>Corresponding filter model probability->Maximum value.
Step S7: and repeating the steps S2 to S6 until the integrated navigation filtering is finished.
FIG. 4 is a graph of Doppler Velocimetry (DVL) output measurements in an exemplary embodiment, wherein FIG. 4 (a) is an x-axis velocity measurement of the DVL output, FIG. 4 (b) is a y-axis velocity measurement of the DVL output, and FIG. 4 (c) is a z-axis velocity measurement of the DVL output, the DVL measurement includes measurement coarse differences, and the measurement noise variance matrix varies. Fig. 5 is a comparison graph of misalignment angle simulation results of the method of the present invention and the Kalman filtering method in an embodiment, wherein fig. 5 (a) is an east misalignment angle comparison graph, fig. 5 (b) is a north misalignment angle comparison graph, and fig. 5 (c) is a sky misalignment angle comparison graph. Fig. 6 is a comparison graph of velocity error simulation results in a specific embodiment of the method of the present invention and the Kalman filtering method, wherein fig. 6 (a) is an east velocity error comparison graph, fig. 6 (b) is a north velocity error comparison graph, and fig. 6 (c) is an sky velocity error comparison graph. Fig. 7 is a comparison chart of simulation results of position errors in the method and the Kalman filtering method according to the present invention, wherein fig. 7 (a) is an eastern position error comparison chart, fig. 7 (b) is a north position error comparison chart, and fig. 7 (c) is an sky position error comparison chart. Fig. 8 is a graph of adaptive estimation of mean square error of measured noise in an embodiment of the robust adaptive multi-model filtering method according to the present invention. From the above figures, it can be seen that the robust adaptive multi-model filtering method of the present invention can effectively improve the robust filtering capability, adaptively and rapidly estimate the statistical characteristics of the measured noise, and improve the filtering precision.
The above is merely a preferred embodiment of the present invention, and the present invention is applicable to all multi-sensor information fusion systems, including but not limited to inertial/doppler integrated navigation systems.
Claims (4)
1. The robust adaptive multi-model filtering method is characterized by comprising the following steps of:
step S1: a multi-model filtering structure of three filters is adopted, a discrete state space model of an application object is established, and a measuring noise variance array model set is established, wherein the application object is an underwater inertial navigation/Doppler integrated navigation system;
step S2: calculating the model mixing probability among the filters, the mixing initial state of the filters and the mean square error matrix of the filters;
step S3: the three filters respectively and simultaneously perform robust Kalman filtering;
step S4: updating the probability of the filter model by adopting a Bayesian hypothesis test method;
step S5: the estimated value of each filter is weighted and fused with the probability of the corresponding filter model, and the joint state estimation and the mean square error array are output;
step S6: adaptively updating a measuring noise variance matrix model set;
step S7: repeating the steps S2 to S6 until the filtering is finished;
the step S1 specifically comprises the following steps:
s1.1, a multi-model filtering structure of three filters is adopted, and a discrete state space model of an application object is established, wherein the discrete state space model comprises a state equation and a measurement equation:
wherein,and->State vector and measurement vector of filter j at time k, respectively, < >>State vector for filter j at time k-1,/->For the state one-step transition matrix of the filter j from moment k-1 to moment k +.>System noise vector for time instant k-1 filter j +.>For the system noise input matrix of the k-1 time to k time filter j +.>For the measurement matrix of the k moment filter j, < >>For the measured noise vector of the k moment filter j, j=1, 2,3;
s1.2 constructing a measurement noise variance matrix model set
Wherein the model is concentratedMeasuring noise variance arrays corresponding to k time filters 1,2,3 respectively,/for each of the k time filters>For the estimated value of the noise variance matrix measured at the moment k, eta is a set positive constant, and for the initial noise variance matrix measured +.>Can be given according to a priori knowledge;
the step S2 specifically includes the following steps:
s2.1, assuming that transition among the filters follows a Markov process, and calculating the model mixing probability among the filters according to the model probability among the filters and the Markov transition probability among the filters at the last moment as follows:
wherein,for the model mixture probability, p, of the k-1 moment filter i to filter j i→j For the Markov transition probability of filter i to filter j, +.>The model probability of the filter i at the moment k-1 is given according to priori knowledge for the model probability of each filter at the initial moment and the Markov transition probability among the filters;
s2.2, calculating the mixed initial state of each filter and a mean square error matrix of each filter:
wherein,and->Mixed initial state of the filter j at time k-1 and its corresponding mean square error matrix, respectively,/>Andthe state estimation of the filter i at the moment of k-1 and the corresponding mean square error matrix are respectively adopted, and the superscript T represents matrix transposition;
the step S3 specifically comprises the following steps:
s3.1, calculating a state one-step prediction and a mean square error matrix:
wherein,and->Respectively predicting the state of the filter j in one step and a corresponding mean square error matrix;
s3.2, calculating innovation:
wherein,an innovation vector for the filter j at time k, < >>A measurement vector of the filter j at the moment k;
s3.3 innovation standardization:
wherein,normalized innovation for the ith measurement component in time k filter j +.>For the mean square error estimation of the innovation of the ith measurement component in the k moment filter j,/>For stopping the innovation sequence of the ith measurement component in the filter j at the current moment, the function med is a median function;
s3.4, innovation M estimation robust processing:
wherein the function omega is a three-section IGGIII equivalent weight function, k 0 And k 1 As an empirical robust parameter, matrixInlet weighting matrix for filter j at time k, < ->A correction innovation sequence of a filter j at the moment k, wherein m is the dimension of a measurement vector;
s3.6, calculating a filter gain array and an innovation variance array:
wherein,a filter gain matrix for the filter j at time k, < >>An innovation variance matrix of the filter j at the moment k;
s3.7, calculating state estimation and a mean square error matrix:
wherein,and->The state estimation of the filter j at the k moment and the corresponding mean square error matrix are respectively adopted.
2. The robust adaptive multi-model filtering method according to claim 1, wherein the step S4 specifically comprises the following procedures:
s4.1, calculating likelihood function values of each filter model by the corrected innovation sequence and the innovation variance array of each filter:
wherein,the likelihood function value of the filter j at the moment k, and m is the dimension of the measurement vector;
s4.2, updating the probability of the filter model by adopting a Bayesian hypothesis test method, and calculating the probability value of each filter model:
wherein,the model probability of filter j at time k.
3. The robust adaptive multi-model filtering method according to claim 2, wherein the step S5 specifically comprises the following procedures:
and (3) carrying out probability weighted fusion on each filter estimation value and the corresponding filter model, and outputting joint state estimation and a mean square error matrix:
wherein,and P k Joint state estimation for k time instants respectivelyAnd its corresponding mean square error matrix.
4. The robust adaptive multi-model filtering method according to claim 3, wherein the step S6 specifically includes the following procedures:
taking the measured noise variance matrix with the maximum probability of the corresponding filter model as the estimated value of the measured noise variance matrix at the next moment, and adaptively updating the measured noise variance matrix model set:
wherein,measuring noise variance matrix estimation value for k+1 time, ">For the measuring noise variance matrix corresponding to the k moment filter j,>j is equal to the filter model probability matrix mu k Position number of maximum value>And forming a model set of the k+1 moment measurement noise variance matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110173917.1A CN113434806B (en) | 2021-02-09 | 2021-02-09 | Robust adaptive multi-model filtering method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110173917.1A CN113434806B (en) | 2021-02-09 | 2021-02-09 | Robust adaptive multi-model filtering method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113434806A CN113434806A (en) | 2021-09-24 |
| CN113434806B true CN113434806B (en) | 2024-01-09 |
Family
ID=77752862
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110173917.1A Active CN113434806B (en) | 2021-02-09 | 2021-02-09 | Robust adaptive multi-model filtering method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113434806B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115560763A (en) * | 2022-09-24 | 2023-01-03 | 东南大学 | Elastic navigation interactive information fusion method based on robust estimation |
| CN115618299A (en) * | 2022-10-08 | 2023-01-17 | 东南大学 | Multi-source information fusion method based on projection statistic detector |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104102836A (en) * | 2014-07-14 | 2014-10-15 | 国家电网公司 | Method for quickly estimating robust state of power system |
-
2021
- 2021-02-09 CN CN202110173917.1A patent/CN113434806B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104102836A (en) * | 2014-07-14 | 2014-10-15 | 国家电网公司 | Method for quickly estimating robust state of power system |
Non-Patent Citations (2)
| Title |
|---|
| Robust MT Tracking Based on M-Estimation and Interacting Multiple Model Algorithm.;Hammes, Ulrich;Zoubir, Abdelhak M.;IEEE TRANSACTIONS ON SIGNAL PROCESSING;第59卷(第7期);第3398-3409页 * |
| 多AUV协同导航鲁棒自适应性滤波算法研究;卢少然;中国优秀硕士学位论文全文数据库工程科技Ⅱ辑(第3期);第C036-232页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113434806A (en) | 2021-09-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109211276B (en) | SINS initial alignment method based on GPR and improved SRCKF | |
| CN108827310B (en) | Marine star sensor auxiliary gyroscope online calibration method | |
| CN109724599B (en) | Wild value resistant robust Kalman filtering SINS/DVL integrated navigation method | |
| JP5237723B2 (en) | System and method for gyrocompass alignment using dynamically calibrated sensor data and iterative extended Kalman filter in a navigation system | |
| CN111156987B (en) | Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF | |
| CN111024064B (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
| CN113434806B (en) | Robust adaptive multi-model filtering method | |
| CN109945859B (en) | Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering | |
| CN110567455B (en) | Tightly-combined navigation method for quadrature updating volume Kalman filtering | |
| CN111750865B (en) | Self-adaptive filtering navigation method for difunctional deep sea unmanned submersible vehicle navigation system | |
| CN113175931B (en) | Cluster networking collaborative navigation method and system based on constraint Kalman filtering | |
| CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
| CN116358566B (en) | Coarse detection combined navigation method based on robust adaptive factor | |
| CN113984054A (en) | Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment | |
| CN114777812B (en) | Inter-advancing alignment and attitude estimation method for underwater integrated navigation system | |
| CN113175926B (en) | Self-adaptive horizontal attitude measurement method based on motion state monitoring | |
| CN112284388B (en) | Unmanned aerial vehicle multisource information fusion navigation method | |
| CN110736459B (en) | Angular deformation measurement error evaluation method for inertial quantity matching alignment | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| CN116952224A (en) | Adaptive inertia/geomagnetic integrated navigation method based on geomagnetic chart suitability evaluation | |
| CN116380067A (en) | Unmanned ship rotation modulation inertial navigation system and method suitable for challenging environment | |
| CN114018262B (en) | Improved derivative volume Kalman filtering integrated navigation method | |
| CN114216480B (en) | Precise alignment method of inertial navigation system | |
| CN111123323A (en) | Method for improving positioning precision of portable equipment | |
| CN114543799B (en) | Robust federal Kalman filtering method, device and system |
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 |