CN116361616A - Robust Kalman filtering method - Google Patents

Robust Kalman filtering method Download PDF

Info

Publication number
CN116361616A
CN116361616A CN202310344333.5A CN202310344333A CN116361616A CN 116361616 A CN116361616 A CN 116361616A CN 202310344333 A CN202310344333 A CN 202310344333A CN 116361616 A CN116361616 A CN 116361616A
Authority
CN
China
Prior art keywords
kalman filtering
time
updating
covariance
sampling frequency
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
CN202310344333.5A
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.)
Hefei Zhongke Shengu Technology Development Co ltd
Original Assignee
Hefei Zhongke Shengu Technology Development Co ltd
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 Hefei Zhongke Shengu Technology Development Co ltd filed Critical Hefei Zhongke Shengu Technology Development Co ltd
Publication of CN116361616A publication Critical patent/CN116361616A/en
Pending legal-status Critical Current

Links

Images

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/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G06F18/243—Classification techniques relating to the number of classes
    • G06F18/2433—Single-class perspective, e.g. one-against-all classification; Novelty detection; Outlier detection
    • 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
    • Y02E—REDUCTION OF GREENHOUSE GAS [GHG] EMISSIONS, RELATED TO ENERGY GENERATION, TRANSMISSION OR DISTRIBUTION
    • Y02E40/00—Technologies for an efficient electrical power generation, transmission or distribution
    • Y02E40/40—Arrangements for reducing harmonics

Landscapes

  • Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Probability & Statistics with Applications (AREA)
  • Operations Research (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Complex Calculations (AREA)

Abstract

The invention relates to a robust Kalman filtering method, which comprises the steps of setting preset sampling times N, initializing filtering, and defining an initial value and a covariance matrix of a state vector; updating the observation vector and the calculation criterion index; detecting whether an abnormal value exists or not, if so, updating parameters by adopting a robust Kalman filtering algorithm, and detecting whether the abnormal value exists or not again after updating the parameters; if the abnormal value is not found, adopting a standard Kalman filtering algorithm to recursively update; repeating the sampling, and ending the program when the sampling times reach the preset sampling times. According to the scheme provided by the invention, when the abnormal value is detected, the parameters are updated by using the robust Kalman filtering algorithm, so that the efficiency and the application range are improved.

Description

Robust Kalman filtering method
Technical Field
The invention relates to the field of signal processing, in particular to a robust Kalman filtering method.
Background
In the field of optimal estimation, parameters and/or states are considered as random variables. In general, the statistical information of random variables is entirely captured by a probability distribution function or a density function. However, in most practical applications, it is almost impossible and not necessary to know the exact distribution or density function. The first two moments of the random variable, mean and covariance, are often more interesting and easier to obtain. The Kalman filter is the best known real-time linear estimator of mean and covariance and has a very wide range of applications. The positioning system can effectively reduce noise and smooth pose accuracy based on the extended Kalman frame, but the observed value in the Kalman system can have abnormal value, so that iteration update based on Kalman can lead to pose failure; standard kalman is the optimal estimator only if certain preconditions are met, e.g. gaussian distributed observation noise with precisely known mean and covariance. However, in many practical applications, observation noise may be that the actual observation is susceptible to outliers. The positioning system based on the Kalman frame can effectively reduce noise and smooth pose accuracy, but the observed value in the Kalman system can have abnormal value, so that iteration update based on Kalman can lead to pose failure;
outliers can be processed using a method called a robust filter or estimator, the most straightforward of which is the data auditing strategy, i.e. to reject observations that are "too large" of the residual. However, this simple strategy is inefficient and will result in a lack of continuity in covariance estimation. At present, a rank deficiency observation model Kalman filter based on M estimation is provided, and the initial state estimation is calculated only by using the current observation value at any moment through robust M estimation, so that the robustness is ensured. This initial state estimate is then used to modify the a priori estimate of covariance, thereby ensuring adaptivity. Thus, in this approach, the observation equation must be overdetermined, or more precisely, include no outlier observations, and the number of remaining observations should not be less than the dimension of the state. Therefore, this adaptive and robust approach can only be applied in limited situations.
Disclosure of Invention
The invention aims to provide a robust Kalman filtering method which can be used for solving the technical problems.
The technical scheme adopted by the invention is as follows.
A robust kalman filtering method, comprising the operations of:
s10: setting a preset sampling frequency N, initializing filtering, and defining an initial value and a covariance matrix of a state vector;
s20: updating the observation vector and the calculation criterion index;
s30: detecting whether an abnormal value exists, if so, executing step S31, and if not, executing step S32;
s31: updating parameters by adopting a robust Kalman filtering algorithm, and executing the step S30 again after updating the parameters;
s32: recursively updating by adopting a standard Kalman filtering algorithm;
s40: comparing the current sampling frequency with the preset sampling frequency N, if the current sampling frequency is less than the preset sampling frequency N, executing step S20, and if the current sampling frequency=the preset sampling frequency N, ending the operation.
Updating parameters using a robust Kalman filtering algorithm includes updating one or more of a scaling factor, a empirical estimation covariance, and a chi-square distribution.
In step S10: k is the sampling number, k=1, 2, … N, defining the initial value of the N-dimensional state vector of the system at time k=0 as x 0 The noise covariance matrix is P 0 。
The detailed scheme is that the specific operation in step S20 is as follows:
s21: carrying out state prediction;
Figure BDA0004159059480000021
Figure BDA0004159059480000022
wherein x is k An n-dimensional state vector sampled at time k;
Figure BDA0004159059480000023
is x k Is a priori estimated of (a); />
Figure BDA0004159059480000024
Is x k-1 Is a posterior estimate of (1); f (F) k-1 Is an n x n-dimensional propagation matrix in k=0 times; />
Figure BDA0004159059480000025
Is a priori estimate of the covariance matrix at time k; />
Figure BDA0004159059480000026
Is a posterior estimate of the covariance matrix at time k-1; />
Figure BDA0004159059480000027
Is F k-1 Is a transposed matrix of (a); q (Q) k-1 Is a range excitation noise covariance matrix at k-1 time;
s22: calculating innovation vectors;
Figure BDA0004159059480000028
Figure BDA0004159059480000029
wherein y is k Is the m-dimensional observation vector at time k;
Figure BDA00041590594800000210
is y k Is a priori estimated of (a); />
Figure BDA00041590594800000211
Is->
Figure BDA00041590594800000212
An associated covariance matrix; h k Is an m x n-dimensional observation matrix at k moment; />
Figure BDA00041590594800000213
Is H k Is a transposed matrix of (a); r is R k Is the observation equation noise covariance at time k;
s23: introducing a judgment index gamma k ；
λ k Let k time initial scaling factor be 1, i.e. lambda, for k time scaling factor k (0)＝1；
Figure BDA0004159059480000031
The specific operation of step S30 is as follows:
if it is
Figure BDA0004159059480000032
χ α For the alpha-quantile x of chi-square distribution, the updating is performed according to the following formula:
Figure BDA0004159059480000033
Figure BDA0004159059480000034
Figure BDA0004159059480000035
wherein: k (K) k Is the Kalman filter gain at k;
Figure BDA0004159059480000036
is x k Is a posterior estimate of (1); />
Figure BDA0004159059480000037
Is y k Is a measurement of the observed value of (2); />
Figure BDA0004159059480000038
Is covariance P k Is a posterior estimate of (2); />
Figure BDA0004159059480000039
Is covariance P k Is a priori estimated value of (2);
after updating, if the number of samples is not reached, k=k+1, starting a new sampling from the state prediction;
if it is not
Figure BDA00041590594800000310
Then the scaling factor lambda k Starting iteration;
Figure BDA00041590594800000311
where i represents the i-th iteration of the scaling factor,
Figure BDA00041590594800000312
representing the actual judgment index->
Figure BDA00041590594800000313
Is an innovation vector;
Figure BDA00041590594800000314
r is then amplified by a scaling factor k Observing a noise covariance;
Figure BDA00041590594800000315
determination of
Figure BDA00041590594800000316
Continuing with χ α And judging.
According to the scheme provided by the invention, when the abnormal value is detected, the corresponding observed value is updated by using the robust Kalman filtering algorithm, so that the continuity of sampling is ensured, and the accuracy of the method is quite stable and insensitive to modeling errors no matter how large the abnormal value is, therefore, the calculation accuracy of Kalman filtering is improved, and the application range is wider than that of the common Kalman filtering.
Drawings
FIG. 1 is a schematic flow chart of the present invention.
Fig. 2 is a position estimation error of KF.
FIG. 3 is a position estimation error of RKF.
Fig. 4 shows the velocity estimation error of KF.
Fig. 5 is a velocity estimation error of RKF.
Detailed Description
The present invention will be specifically described with reference to examples below in order to make the objects and advantages of the present invention more apparent. It should be understood that the following text is intended to describe only one or more specific embodiments of the invention and does not limit the scope of the invention strictly as claimed.
As shown in FIG. 1, a robust method for identifying abnormal values in an EKF observation system based on a Markov distance method comprises the steps of setting a preset sampling frequency N, initializing filtering, and defining an initial value and a covariance matrix of a state vector; updating the observation vector and the calculation criterion index; detecting whether an abnormal value exists or not, if the abnormal value is detected, updating parameters by adopting a robust Kalman filtering algorithm, wherein the parameters comprise updating a scaling factor, a test estimation covariance and a chi-square distribution, and detecting whether the abnormal value exists or not again after updating the parameters; if no abnormal value is detected, recursively updating by adopting a standard Kalman filtering algorithm; comparing the current sampling frequency with the preset sampling frequency N, if the current sampling frequency is less than the preset sampling frequency N, sampling again, and if the previous sampling frequency=the preset sampling frequency N, ending the whole program.
Example 1
k is the sampling number, k=1, 2, … N, defining the initial value of the N-dimensional state vector of the system at time k=0 as x 0 The noise covariance matrix is P 0 N is the preset sampling times.
Carrying out state prediction;
Figure BDA0004159059480000041
Figure BDA0004159059480000042
wherein x is k An n-dimensional state vector sampled at time k;
Figure BDA0004159059480000043
is x k Is a priori estimated of (a); />
Figure BDA0004159059480000044
Is x k-1 Is a posterior estimate of (1); f (F) k-1 Is an n x n-dimensional propagation matrix in k=0 times; />
Figure BDA0004159059480000045
Is a priori estimate of the covariance matrix at time k; />
Figure BDA0004159059480000047
Is a posterior estimate of the covariance matrix at time k-1; />
Figure BDA0004159059480000046
Is F k-1 Is a transposed matrix of (a); q (Q) k-1 Is a range excitation noise covariance matrix at k-1 time;
calculating innovation vectors;
Figure BDA0004159059480000051
Figure BDA0004159059480000052
wherein y is k Is the m-dimensional observation vector at time k;
Figure BDA0004159059480000053
is y k Is a priori estimated of (a); />
Figure BDA0004159059480000054
Is->
Figure BDA0004159059480000055
An associated covariance matrix; h k Is an m x n-dimensional observation matrix at k moment; />
Figure BDA0004159059480000056
Is H k Is a transposed matrix of (a); r is R k Is the observation equation noise covariance at time k;
introducing a judgment index gamma k ；
λ k Let k time initial scaling factor be 1, i.e. lambda, for k time scaling factor k (0)＝1；
Figure BDA0004159059480000057
If it is
Figure BDA0004159059480000058
χ α For the alpha-quantile x of chi-square distribution, the updating is performed according to the following formula:
Figure BDA0004159059480000059
Figure BDA00041590594800000510
Figure BDA00041590594800000511
wherein: k (K) k Is the Kalman filter gain at k;
Figure BDA00041590594800000512
is x k Is a posterior estimate of (1); />
Figure BDA00041590594800000513
Is y k Is a measurement of the observed value of (2); />
Figure BDA00041590594800000514
Is covariance P k Is a posterior estimate of (2); />
Figure BDA00041590594800000515
Is covariance P k Is a priori estimated value of (2);
after updating, if the number of samples is not reached, k=k+1, starting a new sampling from the state prediction;
if it is not
Figure BDA00041590594800000516
Then the scaling factor lambda k Starting iteration;
Figure BDA00041590594800000517
where i represents the i-th iteration of the scaling factor,
Figure BDA00041590594800000518
representing the actual judgment index->
Figure BDA00041590594800000519
Is an innovation vector;
Figure BDA00041590594800000520
r is then amplified by a scaling factor k Observing a noise covariance;
Figure BDA00041590594800000521
determination of
Figure BDA00041590594800000522
Continuing with χ α And judging.
The kinematic positioning problem using the constant velocity model, the kinematic model along the north axis in the continuous time domain is as follows:
Figure BDA00041590594800000523
wherein pN (t) and vN (t) are the north position and north speed of time t, respectively, the real-time estimation, north acceleration aN (t) is modeled as random noise, converted into discrete time form;
x k ＝Fx k-1 +w k
the positions along the three axes are observed, so the observation equation is:
y k ＝Hx k +η k
where H is the observation matrix and ηk is the observation noise.
Acceleration in the state process equation is considered to be Gaussian-distributed white noise, and the nominal standard deviation is 0.01m/s 2 The observed noise is considered to be gaussian distributed white noise with a standard deviation of 0.1m, and these parameters are used together with the state process equation and the observation equation to perform KF and Robust KF (RKF). A positional anomaly value of 2m was artificially introduced every 30 minutes. The position estimation error of KF is shown in fig. 2, the position estimation error of RKF is shown in fig. 3, and the velocity estimation error is shown in fig. 4 and 5. From the illustrations of fig. 2, 3, 4, 5, it is apparent that, when actually observedWhen an outlier exists in the values, the proposed RKF provides superior performance in both position estimation and velocity estimation compared to the standard KF, and the proposed RKF can effectively resist the effects of the outlier.
The foregoing is merely a preferred embodiment of the present invention and it should be noted that modifications and adaptations to those skilled in the art may be made without departing from the principles of the present invention, which are intended to be comprehended within the scope of the present invention. Structures, mechanisms, and methods of operation not specifically described and illustrated in the present invention are implemented by conventional means in the art unless specifically described and limited.

Claims (5)

1. A robust kalman filtering method, comprising the operations of:
s10: setting a preset sampling frequency N, initializing filtering, and defining an initial value and a covariance matrix of a state vector;
s20: updating the observation vector and the calculation criterion index;
s30: detecting whether an abnormal value exists, if so, executing step S31, and if not, executing step S32;
s31: updating parameters by adopting a robust Kalman filtering algorithm, and executing the step S30 again after updating the parameters;
s32: recursively updating by adopting a standard Kalman filtering algorithm;
s40: comparing the current sampling frequency with the preset sampling frequency N, if the current sampling frequency is less than the preset sampling frequency N, executing the step S20, and if the current sampling frequency=the preset sampling frequency N, ending the operation.
2. The robust kalman filtering method according to claim 1, wherein updating parameters with a robust kalman filtering algorithm includes updating one or more of a scaling factor, a empirical estimation covariance, and a chi-square distribution.
3. Robust card according to claim 1 or 2The kalman filtering method is characterized in that in step S10: k is the sampling number, k=1, 2, … N, defining the initial value of the N-dimensional state vector of the system at time k=0 as x 0 The noise covariance matrix is P 0 。
4. A robust kalman filtering method according to claim 3, wherein step S20 is specifically performed as follows:
s21: carrying out state prediction;
Figure FDA0004159059470000011
Figure FDA0004159059470000012
wherein x is k An n-dimensional state vector sampled at time k;
Figure FDA0004159059470000013
is x k Is a priori estimated of (a); />
Figure FDA0004159059470000014
Is x k-1 Is a posterior estimate of (1); f (F) k-1 Is an n-dimensional propagation matrix in the k=0 moment; />
Figure FDA0004159059470000015
Is a priori estimate of the covariance matrix at time k; />
Figure FDA0004159059470000016
Is a posterior estimate of the covariance matrix at time k-1; />
Figure FDA0004159059470000017
Is F k-1 Is a transposed matrix of (a); q (Q) k-1 Is a range excitation noise covariance matrix at k-1 time;
s22: calculating innovation vectors;
Figure FDA0004159059470000018
Figure FDA0004159059470000019
wherein y is k Is the m-dimensional observation vector at time k;
Figure FDA00041590594700000110
is y k Is a priori estimated of (a); />
Figure FDA00041590594700000111
Is->
Figure FDA00041590594700000112
An associated covariance matrix; h k Is an m x n-dimensional observation matrix at k moment; />
Figure FDA0004159059470000021
Is H k Is a transposed matrix of (a); r is R k Is the observation equation noise covariance at time k;
s23: introducing a judgment index gamma k ；
λ k Let k time initial scaling factor be 1, i.e. lambda, for k time scaling factor k (0)＝1；
Figure FDA0004159059470000022
5. The robust kalman filtering method according to claim 4, wherein the step S30 specifically operates as:
if it is
Figure FDA0004159059470000023
χ α The alpha-quantile χ distributed for chi-square is updated according to the following formula:
Figure FDA0004159059470000024
Figure FDA0004159059470000025
Figure FDA0004159059470000026
wherein: k (K) k Is the Kalman filter gain at k;
Figure FDA0004159059470000027
is x k Is a posterior estimate of (1); />
Figure FDA0004159059470000028
Is y k Is a measurement of the observed value of (2); />
Figure FDA0004159059470000029
Is covariance P k Is a posterior estimate of (2); />
Figure FDA00041590594700000210
Is covariance P k Is a priori estimated value of (2);
after updating, if the number of samples is not reached, k=k+1, starting a new sampling from the state prediction;
if it is not
Figure FDA00041590594700000211
Then the scaling factor lambda k Starting iteration;
Figure FDA00041590594700000212
where i represents the i-th iteration of the scaling factor,
Figure FDA00041590594700000213
representing the actual judgment index->
Figure FDA00041590594700000214
Is an innovation vector;
Figure FDA00041590594700000215
r is then amplified by a scaling factor k Observing a noise covariance;
Figure FDA00041590594700000216
determination of
Figure FDA00041590594700000217
Continuing with χ α And judging.
CN202310344333.5A 2022-12-16 2023-03-31 Robust Kalman filtering method Pending CN116361616A (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202211621888 2022-12-16
CN2022116218881 2022-12-16

Publications (1)

Publication Number Publication Date
CN116361616A true CN116361616A (en) 2023-06-30

Family

ID=86931240

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310344333.5A Pending CN116361616A (en) 2022-12-16 2023-03-31 Robust Kalman filtering method

Country Status (1)

Country Link
CN (1) CN116361616A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117109593A (en) * 2023-10-20 2023-11-24 中国石油大学(华东) Submersible robot positioning method and system based on robust Kalman filtering
CN117556180A (en) * 2023-11-14 2024-02-13 中国科学院地质与地球物理研究所 Height estimation method for semi-aviation electromagnetic method

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117109593A (en) * 2023-10-20 2023-11-24 中国石油大学(华东) Submersible robot positioning method and system based on robust Kalman filtering
CN117109593B (en) * 2023-10-20 2024-01-30 中国石油大学(华东) Submersible robot positioning method and system based on robust Kalman filtering
CN117556180A (en) * 2023-11-14 2024-02-13 中国科学院地质与地球物理研究所 Height estimation method for semi-aviation electromagnetic method

Similar Documents

Publication Publication Date Title
CN116361616A (en) Robust Kalman filtering method
Lorenzen et al. Robust MPC with recursive model update
Khodarahmi et al. A review on Kalman filter models
Li et al. Approximate Gaussian conjugacy: parametric recursive filtering under nonlinearity, multimodality, uncertainty, and constraint, and beyond
Lee et al. Extended Kalman filter based nonlinear model predictive control
US20130246006A1 (en) Method for kalman filter state estimation in bilinear systems
García-Fernández et al. Iterated posterior linearization smoother
Zhao et al. Fault detection and diagnosis of multiple-model systems with mismodeled transition probabilities
Chowdhary et al. Bayesian nonparametric adaptive control of time-varying systems using Gaussian processes
Alanwar et al. Data-driven set-based estimation using matrix zonotopes with set containment guarantees
Särkkä et al. Levenberg-Marquardt and line-search extended Kalman smoothers
Gning et al. A box particle filter for stochastic and set-theoretic measurements with association uncertainty
CN115455670B (en) Non-Gaussian noise model building method based on Gaussian mixture model
CN111998854B (en) Cholesky decomposition calculation-based accurate expansion Stirling interpolation filtering method
Zabolotnii et al. Semi-parametric estimation of the change-point of parameters of non-Gaussian sequences by polynomial maximization method
Kogel et al. Low latency output feedback model predictive control for constrained linear systems
Huang et al. Bearing-only target tracking using a bank of MAP estimators
Huang et al. Analytically-selected multi-hypothesis incremental MAP estimation
Carlson et al. Identification of LTV dynamical models with smooth or discontinuous time evolution by means of convex optimization
Speyer et al. Multi-step prediction optimal control for a scalar linear system with additive cauchy noise
Chitralekha et al. Comparison of expectation-maximization based parameter estimation using particle filter, unscented and extended Kalman filtering techniques
Munoz-Minjares et al. Jitter representation in SCNA breakpoints using asymmetric exponential power distribution
Merlinge et al. Online failure probability estimation under state estimation error and its application to angle of attack control of a reentry vehicle
Garrido et al. State estimation for nonlinear systems using restricted genetic optimization
JP6683498B2 (en) Information processing system, information processing method, and information processing program

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