CN111290008A - Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm - Google Patents

Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm Download PDF

Info

Publication number
CN111290008A
CN111290008A CN202010207331.8A CN202010207331A CN111290008A CN 111290008 A CN111290008 A CN 111290008A CN 202010207331 A CN202010207331 A CN 202010207331A CN 111290008 A CN111290008 A CN 111290008A
Authority
CN
China
Prior art keywords
matrix
representing
error
filtering
kalman filter
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.)
Pending
Application number
CN202010207331.8A
Other languages
Chinese (zh)
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.)
Lanzhou Jiaotong University
Original Assignee
Lanzhou Jiaotong 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 Lanzhou Jiaotong University filed Critical Lanzhou Jiaotong University
Priority to CN202010207331.8A priority Critical patent/CN111290008A/en
Publication of CN111290008A publication Critical patent/CN111290008A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H21/00—Adaptive networks
    • H03H21/0012—Digital adaptive filters
    • H03H21/0025—Particular filtering methods
    • H03H21/0029—Particular filtering methods based on statistics
    • H03H21/003—KALMAN filters

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Probability & Statistics with Applications (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention discloses a dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm, which comprises the following steps: s1, estimating the system model error for correcting the error; s2, predicting the system state vector; s3, predicting the covariance matrix of the system state; s4, calculating a Kalman filtering gain matrix; s5, constructing a dynamic adjustment self-adaptive factor; s6, updating the system state; s7, the system state updates the covariance. The method estimates the model error of the uncertain system by using model prediction filtering, and corrects the model error to make up the influence of the model error on the state updating of the filter and improve the output precision of the system; and a filtering factor is introduced through model prediction filtering to realize dynamic adjustment of EKF filtering gain so as to improve the fault-tolerant capability of the EKF filtering gain.

Description

Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm
Technical Field
The invention relates to a filtering fault-tolerant algorithm, in particular to a dynamic adaptive Extended Kalman Filtering (EKF) fault-tolerant algorithm based on model prediction filtering.
Background
At present, with the rapid development of science and technology, the hidden trouble of faults and uncertainty factors of a complex system are continuously increased, and the application of Kalman filtering is seriously influenced. Kalman filtering is applied to various fields as an information fusion filtering algorithm. The filtering methods adopted by the combined pilot system in engineering are mainly Kalman Filtering (KF) and Extended Kalman Filtering (EKF). Kalman filtering requires that a system mathematical model must be linear, and extended Kalman filtering is provided on the basis of Kalman in order to solve the nonlinear problem. However, due to the instability of the components of the system and the influence of uncertain factors of the external application environment, the statistical characteristics of the system and observation noise are difficult to describe accurately, so that the convergence and stability of the Kalman filter cannot be guaranteed in practical application, and the fault tolerance performance is poor. In order to improve the fault tolerance of Kalman filtering, a large amount of research is carried out by scholars at home and abroad, such as a genetic fuzzy inference adaptive UKF filtering algorithm, a robust neural network fault-tolerant filtering algorithm, a fault-tolerant filtering algorithm adopting a two-state propagation chi-square test and a fuzzy adaptive fault-tolerant filtering algorithm, and the like, although the fault tolerance of filtering is improved to different degrees by the above method, some defects also exist, such as difficulty in accurately estimating a model and measuring noise at the same time by the robust neural network, and easiness in divergence when the state order is higher; the attenuation factor in the strong tracking filtering is estimated to be suboptimal, and the calculation is more complicated.
A Model Predictive Filter (MPF) is a real-time nonlinear filtering technique proposed by Crassidis et al based on MME criterion and Predictive control theory. The model prediction filter has no limit on the system model error, so the MPF method can be adopted to estimate the actual model error of the system in real time. Based on the method, the dynamic self-adaptive EKF fault-tolerant algorithm based on the model prediction filtering is provided by combining the prediction filtering and the extended Kalman filtering, on one hand, the model error of the uncertain system is estimated by utilizing the model prediction filtering, and is corrected, so that the influence of the model error on the updating of the state of the filter is compensated, and the output precision of the system is improved. On the other hand, a filtering factor is introduced through model prediction filtering to realize dynamic adjustment of EKF filtering gain so as to improve the fault-tolerant capability of the EKF filtering gain.
Disclosure of Invention
The invention aims at the problems and provides a dynamic adaptive Extended Kalman Filter (EKF) fault-tolerant algorithm based on model prediction filtering.
The technical scheme adopted by the invention is as follows: a dynamic adaptive extended Kalman filter fault-tolerant algorithm comprises the following steps:
s1, estimating the system model error for correcting the error;
s2, predicting the system state vector;
s3, predicting the covariance matrix of the system state;
s4, calculating a Kalman filtering gain matrix;
s5, constructing a dynamic adjustment self-adaptive factor;
s6, updating the system state;
s7, the system state updates the covariance.
Further, the step S1 is specifically:
estimating the system model error for correcting the error:
Figure RE-RE-DEST_PATH_IMAGE001
（1）
Figure 100002_RE-RE-DEST_PATH_IMAGE002
representing the current system model error estimate, k represents the number of system updates,
Figure RE-DEST_PATH_IMAGE003
is the current system
A formula simplification term of the model error estimation;
Figure RE-RE-DEST_PATH_IMAGE004
Figure RE-DEST_PATH_IMAGE005
to representThe estimation of the state vector of the system,
Figure RE-RE-DEST_PATH_IMAGE006
which represents an estimate of the vector of the system output,
Figure 100002_RE-RE-DEST_PATH_IMAGE007
system of representations
The output vector of the next step is unified,
Figure RE-RE-DEST_PATH_IMAGE008
the time increment is represented by the time increment,
Figure RE-DEST_PATH_IMAGE009
one term in a taylor expansion representing an observation estimate.
Further, the step S2 specifically includes:
predicting the system state vector:
Figure RE-RE-DEST_PATH_IMAGE010
（2）
wherein,
Figure RE-DEST_PATH_IMAGE011
representing a system model error one-step transfer matrix;
Figure RE-RE-DEST_PATH_IMAGE012
representing a one-step transition matrix of the system state;
Figure 100002_RE-RE-DEST_PATH_IMAGE013
representing a system state vector;
Figure RE-RE-DEST_PATH_IMAGE014
representing the last step of system state vector estimation;
Figure RE-DEST_PATH_IMAGE015
representing the last step of system model error estimation.
Further, the step S3 specifically includes:
predicting the covariance matrix of the system state:
Figure RE-RE-DEST_PATH_IMAGE016
（3）
wherein,
Figure 100002_RE-RE-DEST_PATH_IMAGE017
a representation of the system noise drive matrix is shown,
Figure RE-RE-DEST_PATH_IMAGE018
which represents the prediction of the system state vector,
Figure 100002_RE-RE-DEST_PATH_IMAGE019
a one-step transition matrix representing the state of the system,
Figure RE-RE-DEST_PATH_IMAGE020
representing the process error covariance matrix.
Further, the step S4 specifically includes:
calculating a Kalman filtering gain matrix:
Figure 100002_RE-RE-DEST_PATH_IMAGE021
（4）
wherein,
Figure RE-RE-DEST_PATH_IMAGE022
an equivalent covariance matrix representing the observation vector,
Figure RE-DEST_PATH_IMAGE023
a matrix of the gain of the kalman filter is represented,
Figure RE-609458DEST_PATH_IMAGE018
which represents the prediction of the system state vector,
Figure 100002_RE-RE-DEST_PATH_IMAGE024
representing an observation matrix.
Further, the step S5 specifically includes:
constructing dynamically adjusted adaptation factors
Figure RE-RE-DEST_PATH_IMAGE025
：
Figure RE-DEST_PATH_IMAGE026
（5）
Figure 100002_RE-RE-DEST_PATH_IMAGE027
Wherein,
Figure 100002_RE-DEST_PATH_IMAGE028
it is indicated that the adaptive factor is adjusted dynamically,
Figure 100002_RE-RE-DEST_PATH_IMAGE029
represents the robust solution of the current epoch state parameter,
Figure RE-RE-DEST_PATH_IMAGE030
representing custom parameters.
Further, the step S6 specifically includes:
and updating the system state:
Figure RE-DEST_PATH_IMAGE031
（6）
wherein,
Figure RE-RE-DEST_PATH_IMAGE032
which represents the estimate of the state vector of the system,
Figure RE-RE-DEST_PATH_IMAGE033
the unit matrix is represented by a matrix of units,
Figure 100002_RE-DEST_PATH_IMAGE034
a matrix of the gain of the kalman filter is represented,
Figure 100002_RE-RE-DEST_PATH_IMAGE035
representing the system output vector.
Further, the step S7 specifically includes:
system state update covariance:
Figure RE-DEST_PATH_IMAGE036
（7）
wherein,
Figure RE-664877DEST_PATH_IMAGE032
which represents the estimate of the state vector of the system,
Figure RE-400752DEST_PATH_IMAGE033
the unit matrix is represented by a matrix of units,
Figure RE-68493DEST_PATH_IMAGE034
a matrix of the gain of the kalman filter is represented,
Figure RE-206214DEST_PATH_IMAGE024
a representation of an observation matrix is shown,
Figure RE-RE-DEST_PATH_IMAGE037
representing the observed noise covariance matrix.
The invention has the advantages that:
the method estimates the model error of the uncertain system by using model prediction filtering, and corrects the model error to make up the influence of the model error on the state updating of the filter and improve the output precision of the system; and a filtering factor is introduced through model prediction filtering to realize dynamic adjustment of EKF filtering gain so as to improve the fault-tolerant capability of the EKF filtering gain.
In addition to the objects, features and advantages described above, other objects, features and advantages of the present invention are also provided. The present invention will be described in further detail below with reference to the drawings.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention.
FIG. 1 is a flow chart of a dynamic adaptive extended Kalman filter fault-tolerant algorithm of the present invention;
FIG. 2 is an east position error plot of the present invention;
FIG. 3 is a north position error graph of the present invention;
FIG. 4 is a velocity error simulation graph of the present invention;
FIG. 5 is a field diagram of a vehicle-mounted experiment of the present invention;
FIG. 6 is a diagram of the onboard test route for the combined GPS/INS positioning system of the present invention;
FIG. 7 is a comparison of onboard test longitude data processing of the present invention;
FIG. 8 is a graph of an EKF algorithm filter error profile of the present invention;
fig. 9 is a filtering error distribution diagram of the MPEKF algorithm of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Referring to fig. 1, as shown in fig. 1, a dynamic adaptive extended kalman filter fault-tolerant algorithm includes the following steps:
s1, estimating the system model error for correcting the error;
s2, predicting the system state vector;
s3, predicting the covariance matrix of the system state;
s4, calculating a Kalman filtering gain matrix;
s5, constructing a dynamic adjustment self-adaptive factor;
s6, updating the system state;
s7, the system state updates the covariance.
The invention estimates the model error of the uncertain system by using model prediction filtering and corrects the model error
The influence of model errors on the state updating of the filter is made up, and the output precision of the system is improved; and a filtering factor is introduced through model prediction filtering to realize dynamic adjustment of EKF filtering gain so as to improve the fault-tolerant capability of the EKF filtering gain.
The step S1 specifically includes:
estimating the system model error for correcting the error:
Figure RE-871681DEST_PATH_IMAGE001
（1）
Figure RE-109501DEST_PATH_IMAGE002
representing the current system model error estimate, k represents the number of system updates,
Figure RE-315354DEST_PATH_IMAGE003
is the current system
A formula simplification term of the model error estimation;
Figure RE-42002DEST_PATH_IMAGE004
Figure RE-940688DEST_PATH_IMAGE005
which represents the estimate of the state vector of the system,
Figure RE-651155DEST_PATH_IMAGE006
which represents an estimate of the vector of the system output,
Figure RE-129540DEST_PATH_IMAGE007
system of representations
The output vector of the next step is unified,
Figure RE-241853DEST_PATH_IMAGE008
the time increment is represented by the time increment,
Figure RE-45861DEST_PATH_IMAGE009
one term in a taylor expansion representing an observation estimate.
The step S2 specifically includes:
predicting the system state vector:
Figure RE-243624DEST_PATH_IMAGE010
（2）
wherein,
Figure RE-791280DEST_PATH_IMAGE011
representing a system model error one-step transfer matrix;
Figure 100002_RE-RE-DEST_PATH_IMAGE038
representing a one-step transition matrix of the system state;
Figure RE-961361DEST_PATH_IMAGE013
representing a system state vector;
Figure RE-201850DEST_PATH_IMAGE014
representing the last step of system state vector estimation;
Figure RE-355751DEST_PATH_IMAGE015
representing the last step of system model error estimation.
The step S3 specifically includes:
predicting the covariance matrix of the system state:
Figure RE-704168DEST_PATH_IMAGE016
（3）
wherein,
Figure RE-525493DEST_PATH_IMAGE017
a representation of the system noise drive matrix is shown,
Figure RE-671304DEST_PATH_IMAGE018
which represents the prediction of the system state vector,
Figure RE-578080DEST_PATH_IMAGE019
a one-step transition matrix representing the state of the system,
Figure RE-467539DEST_PATH_IMAGE020
representing the process error covariance matrix.
The step S4 specifically includes:
calculating a Kalman filtering gain matrix:
Figure RE-408950DEST_PATH_IMAGE021
（4）
wherein,
Figure RE-991241DEST_PATH_IMAGE022
an equivalent covariance matrix representing the observation vector,
Figure RE-385313DEST_PATH_IMAGE023
a matrix of the gain of the kalman filter is represented,
Figure RE-78463DEST_PATH_IMAGE018
which represents the prediction of the system state vector,
Figure RE-343222DEST_PATH_IMAGE024
representing an observation matrix.
The step S5 specifically includes:
constructing dynamically adjusted adaptation factors
Figure RE-96414DEST_PATH_IMAGE025
：
Figure RE-977783DEST_PATH_IMAGE026
（5）
Figure RE-209044DEST_PATH_IMAGE027
Wherein,
Figure RE-859468DEST_PATH_IMAGE028
it is indicated that the adaptive factor is adjusted dynamically,
Figure RE-517982DEST_PATH_IMAGE029
represents the robust solution of the current epoch state parameter,
Figure RE-889576DEST_PATH_IMAGE030
representing custom parameters.
Figure 100002_RE-DEST_PATH_IMAGE039
The value range is 1.0 to 1.5,
Figure RE-RE-DEST_PATH_IMAGE040
the value range is 3.0 to 8.0.
The step S6 specifically includes:
and updating the system state:
Figure RE-596632DEST_PATH_IMAGE031
（6）
wherein,
Figure RE-367142DEST_PATH_IMAGE032
which represents the estimate of the state vector of the system,
Figure RE-930979DEST_PATH_IMAGE033
the unit matrix is represented by a matrix of units,
Figure RE-786939DEST_PATH_IMAGE034
a matrix of the gain of the kalman filter is represented,
Figure RE-360003DEST_PATH_IMAGE035
representing the system output vector.
The step S7 specifically includes:
system state update covariance:
Figure RE-719440DEST_PATH_IMAGE036
（7）
wherein,
Figure RE-985337DEST_PATH_IMAGE032
which represents the estimate of the state vector of the system,
Figure RE-63014DEST_PATH_IMAGE033
the unit matrix is represented by a matrix of units,
Figure RE-705348DEST_PATH_IMAGE034
a matrix of the gain of the kalman filter is represented,
Figure RE-919292DEST_PATH_IMAGE024
a representation of an observation matrix is shown,
Figure RE-356089DEST_PATH_IMAGE037
representing the observed noise covariance matrix.
The method estimates the system model error in real time through model prediction filtering, corrects the system model error, and adjusts the Kalman filtering gain matrix by utilizing the model prediction filtering state so as to improve the fault-tolerant capability and the output precision.
Algorithm simulation and test:
carrying out algorithm simulation:
in order to verify the effectiveness of the algorithm provided by the invention, a GPS/INS integrated navigation system is used as a simulation platform through MATLAB, a pseudo range/pseudo range rate-based tight coupling mode is adopted, and the state quantity is measured:
Figure RE-DEST_PATH_IMAGE041
(8)
wherein,
Figure RE-RE-DEST_PATH_IMAGE042
representing position errors in the latitude, longitude and altitude directions;
Figure RE-DEST_PATH_IMAGE043
representing east, north and sky velocity errors;
Figure RE-RE-DEST_PATH_IMAGE044
is the attitude angle error;
Figure RE-DEST_PATH_IMAGE045
a random constant drift error for the gyroscope;
Figure RE-RE-DEST_PATH_IMAGE046
a first order Markov process for gyroscope drift error;
Figure RE-RE-DEST_PATH_IMAGE047
is the accelerometer error;
Figure RE-RE-DEST_PATH_IMAGE048
is the distance error equivalent to the clock frequency error;
Figure RE-DEST_PATH_IMAGE049
is the range error equivalent to the clock error.
And X is a state vector formed by the position errors in the corresponding longitude and latitude height directions, east, north and sky speed errors, attitude angle errors, gyroscope random constant drift errors, a Markov process of order, accelerometer errors, distance errors equivalent to clock frequency errors, distance errors equivalent to clock errors and other parameters.
Assuming that the number of GPS satellites participating in the combination is 4, the observation quantity is taken
Figure RE-RE-DEST_PATH_IMAGE050
（9）
Z represents the pseudoranges of the INS and the GPS.
Wherein,
Figure RE-RE-DEST_PATH_IMAGE051
the pseudoranges and pseudorange differences for the INS and GPS are indicated respectively,
Figure RE-DEST_PATH_IMAGE052
and
Figure RE-RE-DEST_PATH_IMAGE053
the pseudoranges and pseudorange rates for the INS and GPS are indicated, respectively.
The system simulation parameters are set as follows: the value ranges of the satellite pseudo range and the pseudo range rate residual are 0 to 10; and the adjustment range of the corresponding Kalman filtering gain factor is 1-0.04, and when the value of the pseudo-range and the pseudo-range rate measurement residual error is 10, the corresponding gain factor is 0.04 at the minimum value.
Carrying out 1000s simulation on a GPS/INS combined system simulation platform built by MATLAB, wherein part of initial conditions are as follows: initial position of moving carrier: latitude 36.03, longitude 103.40; the initial speed of movement in each direction is 1000 (m/s); the initial error angles of east and north are both 0.01; the initial position error of the longitude and the latitude is 0.2; the initial speed error is 0.2 m/s; gyro constant and random drift: 0.09/h and 0.04/h; the correlation time is 2 h; accelerometer mean error 110 ug; acceleration of the carrier:
Figure RE-RE-DEST_PATH_IMAGE054
(ii) a Pseudo range error: 10 m; pseudo range rate error: 0.1 m/s.
In order to check the fault-tolerant effect of the algorithm, fault noise is introduced from 501s, namely the measurement noise is enlarged to 10 times of the initial value, and in order to illustrate the superiority of the algorithm, the algorithm is compared with a standard EKF filtering algorithm, and the result is shown in the figure. Wherein FIG. 2 is an east position error curve; FIG. 3 is a north position error curve; fig. 4 is a velocity error curve. It can be seen from the figure that compared with the conventional EKF algorithm, the dynamic adaptive EKF fault-tolerant algorithm (MPEKF) based on the model prediction filtering has better fault-tolerant capability and filtering effect.
Vehicle-mounted test analysis:
in order to further verify the effectiveness and the usability of the algorithm provided by the invention, a GPS/INS combined positioning platform is built by adopting a GPS satellite receiving board card (K700) and an IMU inertial measurement unit (MPU 6050), as shown in FIG. 5, and a test route is shown in FIG. 6.
It can be seen from the figure that the positioning accuracy after the algorithm processing is improved, and in order to further explain the scientificity of the data, the measurement error is further processed, and the result is shown in fig. 8 and 9.
Wherein, fig. 8 is the processed error distribution of the conventional EKF algorithm, and the statistical variance is calculated according to the data of fig. 7 as follows:
Figure RE-DEST_PATH_IMAGE055
（10）
d denotes the sample variance and E denotes the sample expectation.
Wherein, fig. 9 is the processed error distribution of the MPEKF algorithm, and the statistical variance is calculated according to the data of fig. 9 as:
Figure RE-RE-DEST_PATH_IMAGE056
（11）
therefore, it can be seen that the filtering accuracy of the improved EKF is improved:
Figure RE-RE-DEST_PATH_IMAGE057
（12）
according to the invention, through simulation and physical test analysis, the fault-tolerant capability and the filtering precision of the EKF algorithm introduced with model prediction filtering can be greatly improved, and a certain reference value is provided for further application of the EKF algorithm.
The invention carries out test simulation through a GPS/INS integrated navigation platform. The result shows that the improved extended Kalman filtering has stronger fault-tolerant performance and overall precision, and the filtering performance is obviously superior to that of the extended Kalman filtering algorithm.
The invention provides a dynamic adaptive Extended Kalman Filter (EKF) fault-tolerant algorithm based on model prediction filtering, aiming at the problem of insufficient filtering fault-tolerant performance caused by uncertainty of system errors. On one hand, model errors of the uncertain system are estimated by utilizing model prediction filtering, and are corrected, so that the influence of the model errors on the state updating of the filter is made up, and the output precision of the system is improved. On the other hand, a filtering factor is introduced through model prediction filtering to realize dynamic adjustment of EKF filtering gain so as to improve the fault-tolerant capability of the EKF filtering gain. And carrying out test simulation through the GPS/INS integrated navigation platform.
The result shows that the improved extended Kalman filtering has stronger fault-tolerant performance and overall precision, and the filtering performance is obviously superior to that of the extended Kalman filtering algorithm.
The dynamic self-adaptive EKF fault-tolerant algorithm based on model prediction filtering solves the problems of insufficient fault-tolerant performance and poor precision of a filter by error model prediction and introduction of a Kalman gain factor. The MATLAB simulation and the physical test are used for verification, and the result shows that the model prediction filtering dynamic self-adaptive EKF algorithm has stronger fault-tolerant capability than the traditional extended Kalman filtering on the premise of improving the filtering precision.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.

Claims (8)

1. A dynamic adaptive extended Kalman filter fault-tolerant algorithm is characterized by comprising
The method comprises the following steps:
s1, estimating the system model error for correcting the error;
s2, predicting the system state vector;
s3, predicting the covariance matrix of the system state;
s4, calculating a Kalman filtering gain matrix;
s5, constructing a dynamic adjustment self-adaptive factor;
s6, updating the system state;
s7, the system state updates the covariance.
2. The dynamic adaptive extended Kalman filter fault tolerant algorithm of claim 1,
it is characterized in that the step S1 specifically includes:
estimating the system model error for correcting the error:
Figure RE-DEST_PATH_IMAGE001
（1）
Figure RE-RE-DEST_PATH_IMAGE002
representing the current system model error estimate, k represents the number of system updates,
Figure RE-RE-DEST_PATH_IMAGE003
is the current system
A formula simplification term of the model error estimation;
Figure RE-DEST_PATH_IMAGE004
Figure RE-RE-DEST_PATH_IMAGE005
which represents the estimate of the state vector of the system,
Figure RE-DEST_PATH_IMAGE006
which represents an estimate of the vector of the system output,
Figure RE-RE-DEST_PATH_IMAGE007
system of representations
The output vector of the next step is unified,
Figure RE-DEST_PATH_IMAGE008
the time increment is represented by the time increment,
Figure RE-RE-DEST_PATH_IMAGE009
one term in a taylor expansion representing an observation estimate.
3. The dynamic adaptive extended Kalman filter fault tolerant algorithm of claim 1,
it is characterized in that the step S2 specifically includes:
predicting the system state vector:
Figure RE-DEST_PATH_IMAGE010
（2）
wherein,
Figure RE-RE-DEST_PATH_IMAGE011
representing a system model error one-step transfer matrix;
Figure RE-DEST_PATH_IMAGE012
representing a one-step transition matrix of the system state;
Figure RE-RE-DEST_PATH_IMAGE013
representing a system state vector;
Figure RE-DEST_PATH_IMAGE014
representing the last step of system state vector estimation;
Figure RE-RE-DEST_PATH_IMAGE015
representing the last step of system model error estimation.
4. The dynamic adaptive extended Kalman filter fault tolerant algorithm of claim 1,
it is characterized in that the step S3 specifically includes:
predicting the covariance matrix of the system state:
Figure RE-DEST_PATH_IMAGE016
（3）
wherein,
Figure RE-RE-DEST_PATH_IMAGE017
a representation of the system noise drive matrix is shown,
Figure RE-DEST_PATH_IMAGE018
which represents the prediction of the system state vector,
Figure RE-RE-DEST_PATH_IMAGE019
a one-step transition matrix representing the state of the system,
Figure RE-DEST_PATH_IMAGE020
representing the process error covariance matrix.
5. The dynamic adaptive extended Kalman filter fault tolerant algorithm of claim 1,
it is characterized in that the step S4 specifically includes:
calculating a Kalman filtering gain matrix:
Figure RE-RE-DEST_PATH_IMAGE021
（4）
wherein,
Figure RE-DEST_PATH_IMAGE022
an equivalent covariance matrix representing the observation vector,
Figure RE-RE-DEST_PATH_IMAGE023
a matrix of the gain of the kalman filter is represented,
Figure RE-394756DEST_PATH_IMAGE018
which represents the prediction of the system state vector,
Figure RE-RE-DEST_PATH_IMAGE024
representing an observation matrix.
6. The dynamic adaptive extended Kalman filter fault tolerant algorithm of claim 1,
it is characterized in that the step S5 specifically includes:
constructing dynamically adjusted adaptation factors
Figure RE-DEST_PATH_IMAGE025
：
Figure RE-RE-DEST_PATH_IMAGE026
（5）
Figure RE-RE-DEST_PATH_IMAGE027
Wherein,
Figure RE-DEST_PATH_IMAGE028
it is indicated that the adaptive factor is adjusted dynamically,
Figure RE-RE-DEST_PATH_IMAGE029
represents the robust solution of the current epoch state parameter,
Figure RE-DEST_PATH_IMAGE030
representing custom parameters.
7. The dynamic adaptive extended Kalman filter fault tolerant algorithm of claim 1,
it is characterized in that the step S6 specifically includes:
and updating the system state:
Figure RE-RE-DEST_PATH_IMAGE031
（6）
wherein,
Figure RE-DEST_PATH_IMAGE032
which represents the estimate of the state vector of the system,
Figure RE-DEST_PATH_IMAGE034
the unit matrix is represented by a matrix of units,
Figure RE-RE-DEST_PATH_IMAGE035
a matrix of the gain of the kalman filter is represented,
Figure RE-RE-DEST_PATH_IMAGE036
representing the system output vector.
8. The dynamic adaptive extended Kalman filter fault tolerant algorithm of claim 1,
it is characterized in that the step S7 specifically includes:
system state update covariance:
Figure RE-DEST_PATH_IMAGE037
（7）
wherein,
Figure RE-132469DEST_PATH_IMAGE032
which represents the estimate of the state vector of the system,
Figure RE-443364DEST_PATH_IMAGE034
the unit matrix is represented by a matrix of units,
Figure RE-245098DEST_PATH_IMAGE035
a matrix of the gain of the kalman filter is represented,
Figure RE-RE-DEST_PATH_IMAGE038
a representation of an observation matrix is shown,
Figure RE-DEST_PATH_IMAGE039
representing the observed noise covariance matrix.
CN202010207331.8A 2020-03-23 2020-03-23 Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm Pending CN111290008A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010207331.8A CN111290008A (en) 2020-03-23 2020-03-23 Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010207331.8A CN111290008A (en) 2020-03-23 2020-03-23 Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm

Publications (1)

Publication Number Publication Date
CN111290008A true CN111290008A (en) 2020-06-16

Family

ID=71030283

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010207331.8A Pending CN111290008A (en) 2020-03-23 2020-03-23 Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm

Country Status (1)

Country Link
CN (1) CN111290008A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112671373A (en) * 2020-12-21 2021-04-16 北京科技大学 Kalman filtering self-adaptive filtering algorithm based on error control
CN115390096A (en) * 2022-08-29 2022-11-25 浙江大学 Low-orbit satellite real-time relative orbit determination method based on full-view satellite-borne GNSS (Global navigation satellite System) receiving system

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004198188A (en) * 2002-12-17 2004-07-15 Yokogawa Denshikiki Co Ltd Device and method for discriminating and predicting road surface condition
KR101502721B1 (en) * 2014-02-06 2015-03-24 군산대학교산학협력단 Method and apparatus for providing precise positioning information using adaptive interacting multiple model estimator
CN105549049A (en) * 2015-12-04 2016-05-04 西北农林科技大学 Adaptive Kalman filtering algorithm applied to GPS navigation
CN106767900A (en) * 2016-11-23 2017-05-31 东南大学 A kind of online calibration method of the optical fibre SINS system based on integrated navigation technology
CN108134549A (en) * 2017-12-25 2018-06-08 西安理工大学 A kind of method for improving permanent magnet synchronous motor speed estimate stability
CN110895146A (en) * 2019-10-19 2020-03-20 山东理工大学 Synchronous positioning and map construction method for mobile robot

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004198188A (en) * 2002-12-17 2004-07-15 Yokogawa Denshikiki Co Ltd Device and method for discriminating and predicting road surface condition
KR101502721B1 (en) * 2014-02-06 2015-03-24 군산대학교산학협력단 Method and apparatus for providing precise positioning information using adaptive interacting multiple model estimator
CN105549049A (en) * 2015-12-04 2016-05-04 西北农林科技大学 Adaptive Kalman filtering algorithm applied to GPS navigation
CN106767900A (en) * 2016-11-23 2017-05-31 东南大学 A kind of online calibration method of the optical fibre SINS system based on integrated navigation technology
CN108134549A (en) * 2017-12-25 2018-06-08 西安理工大学 A kind of method for improving permanent magnet synchronous motor speed estimate stability
CN110895146A (en) * 2019-10-19 2020-03-20 山东理工大学 Synchronous positioning and map construction method for mobile robot

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
刘射德 等: "基于MCKF容错算法的列车多源信息融合定位技术研究", 《铁道学报》, vol. 41, no. 8, 31 August 2019 (2019-08-31), pages 74 - 83 *
徐晓苏等: "一种新型自适应模型预测组合滤波在在线标定中的应用", 《中国惯性技术学报》, vol. 25, no. 06, 15 December 2017 (2017-12-15), pages 713 - 718 *
蒋帆 等: "基于模型预测扩展卡尔曼滤波的动力定位状态估计", 《船舶工程》, vol. 41, no. 7, 31 July 2019 (2019-07-31), pages 86 - 91 *
赵长胜: "《动态测量数据处理理论》", 31 December 2016, 测绘出版社, pages: 64 - 70 *
高伟 等: "CCD星敏感器辅助光纤陀螺在线标定技术", 《系统工程与电子技术》, vol. 34, no. 8, 31 August 2012 (2012-08-31), pages 1680 - 1684 *
高杜生 等: "抗差自适应模型预测及其在组合导航中的应用", 《中国惯性技术学报》, vol. 19, no. 6, 31 December 2011 (2011-12-31), pages 701 - 705 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112671373A (en) * 2020-12-21 2021-04-16 北京科技大学 Kalman filtering self-adaptive filtering algorithm based on error control
CN112671373B (en) * 2020-12-21 2024-04-26 北京科技大学 Kalman filtering self-adaptive filtering algorithm based on error control
CN115390096A (en) * 2022-08-29 2022-11-25 浙江大学 Low-orbit satellite real-time relative orbit determination method based on full-view satellite-borne GNSS (Global navigation satellite System) receiving system
CN115390096B (en) * 2022-08-29 2023-04-25 浙江大学 Low-orbit satellite real-time relative orbit determination method based on full-view satellite-borne GNSS receiving system

Similar Documents

Publication Publication Date Title
Hasan et al. A review of navigation systems (integration and algorithms)
Wendel et al. A performance comparison of tightly coupled GPS/INS navigation systems based on extended and sigma point Kalman filters
Hu et al. Robust unscented Kalman filtering with measurement error detection for tightly coupled INS/GNSS integration in hypersonic vehicle navigation
CN109029454A (en) A kind of abscissa system Strapdown Inertial Navigation System damping algorithm based on Kalman filtering
CN113203418B (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
Hiliuta et al. Fuzzy corrections in a GPS/INS hybrid navigation system
CN110514203B (en) Underwater integrated navigation method based on ISR-UKF
Goodall Improving usability of low-cost INS/GPS navigation systems using intelligent techniques
CN110567455B (en) Tightly-combined navigation method for quadrature updating volume Kalman filtering
Nourmohammadi et al. Design and experimental evaluation of indirect centralized and direct decentralized integration scheme for low-cost INS/GNSS system
Wang et al. Enhanced multi-sensor data fusion methodology based on multiple model estimation for integrated navigation system
CN114777812B (en) Inter-advancing alignment and attitude estimation method for underwater integrated navigation system
US20100256906A1 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN111290008A (en) Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm
CN116222551A (en) Underwater navigation method and device integrating multiple data
CN109974695A (en) The robust adaptive filtering method of surface ship navigation system based on the space Krein
Wang et al. Performance analysis of GNSS/INS loosely coupled integration systems under GNSS signal blocking environment
Rahman et al. Low-cost real-time PPP GNSS aided INS for CAV applications
CN115031728A (en) Unmanned aerial vehicle integrated navigation method based on tight combination of INS and GPS
Kubo et al. Nonlinear filtering methods for the INS/GPS in-motion alignment and navigation
Li et al. Realization of GNSS/INS tightly coupled navigation and reliability verification in intelligent driving system
Frykman Applied particle filters in integrated aircraft navigation
Campos-Vega et al. Navigation through the Processing of Android Data with a High-Order Kalman Filter
Nie et al. Comparison of nonlinear filtering approach in tightly-coupled GPS/INS navigation system
Zhao et al. Application for autonomous underwater vehicle initial alignment with divided difference filter

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20200616

RJ01 Rejection of invention patent application after publication