CN104539265A

CN104539265A - Self-adaptive UKF (Unscented Kalman Filter) algorithm

Info

Publication number: CN104539265A
Application number: CN201410691143.1A
Authority: CN
Inventors: 何俊; 张清华; 孙国玺; 肖明; 熊建斌; 丘海健
Original assignee: Guangdong University of Petrochemical Technology
Current assignee: Guangdong University of Petrochemical Technology
Priority date: 2014-11-25
Filing date: 2014-11-25
Publication date: 2015-04-22

Abstract

The invention discloses a self-adaptive UKF (Unscented Kalman Filter) algorithm. In the algorithm, a Kalman filtering algorithm based on a maximum likelihood criterion and the self-adaptive UKF algorithm based on maximum posteriori estimation are combined. Real-time estimation is performed on covariance through the two algorithms, and averaging is performed to obtain an estimated value with a better prior covariance true value tracking effect, so that the filtering accuracy is increased, and the filtering stability is enhanced.

Description

Self-adaptive UKF filtering algorithm

Technical Field

The invention relates to a self-adaptive UKF filtering algorithm, in particular to a self-adaptive UKF filtering algorithm combining maximum likelihood estimation and maximum posterior estimation.

Background

In order to solve the problem that the Kalman filtering and the subsequent extended Kalman filtering under the nonlinear condition have larger errors in estimation and even possibly cause Filter divergence, in recent years, Julie and Uhlman propose an Unscented Kalman Filter (UKF) method based on a multivariate function representative point idea, the accuracy of the UKF algorithm is obviously higher than that of the EKF algorithm, and the calculated amount is obviously smaller than that of the EKF algorithm, but the UKF algorithm has a main problem that when the noise statistical characteristic is unknown, the filtering accuracy of the UKF is reduced and even diverges, in order to solve the problem, various scholars propose a plurality of solving methods, wherein a document [7] proposes an algorithm of a suboptimal unbiased MAP noise time-varying estimator containing a fading factor according to an extremely large posterior estimation principle and exponential weighting, the algorithm has the characteristics of self-adaption capability, simple recurrence formula and easy engineering realization, but needs enough data, the ideal effect can be achieved.

Disclosure of Invention

The invention aims to overcome the defects of the prior art and provides a self-adaptive UKF filtering algorithm, which is an algorithm combining a Kalman filtering algorithm based on a maximum likelihood criterion and a self-adaptive UKF filtering algorithm based on maximum posterior estimation, estimates covariance in real time through the two algorithms, and then obtains an estimated value with better tracking effect on a priori covariance true value in average acquisition, thereby improving filtering precision and filtering stability. The specific technical scheme is as follows:

an adaptive UKF filtering algorithm comprising the steps of:

the first step is as follows: judging whether convergence has been stabilized

Q_k-Q_k-2≤H

R_k-R_k-2≤H

Wherein H is a small positive integer;

the second step is that: selecting a figure of merit

Let the estimated covariance of the two algorithms be Q1_k、R1_k、Q2_k、R2_kThen, then

{\hat{Q}}_{k} = \frac{{Q 1}_{k} + {Q 2}_{k}}{2}

{\hat{R}}_{k} = \frac{{R 1}_{k} + {R 2}_{k}}{2}

In the formula,is the covariance estimate.

Compared with the prior art, the invention has the beneficial effects that:

compared with the traditional UKF algorithm, the UKF adaptive algorithm based on the maximum posterior estimation solves the problem of filter divergence of the traditional UKF algorithm under the condition that the noise statistical characteristic is unknown, selects the UKF adaptive algorithm based on the optimal noise statistical characteristic estimation by combining the advantages and the disadvantages of the maximum likelihood estimation and the maximum posterior estimation, and can be seen to improve the filtering precision in a simulation result.

Drawings

FIG. 1 is a graph of adaptive estimation of noise mean for the process [7 ];

FIG. 2 is a graph of adaptive estimation of process noise covariance in [7 ];

FIG. 3 is a graph of the process noise covariance estimation of the present invention;

FIG. 4 is a graph of adaptive estimation of mean value of measured noise in document [7 ];

FIG. 5 document [7] adaptive estimation plot of the measured noise covariance;

FIG. 6 is a graph of adaptive estimation of measured noise covariance according to the present invention.

Detailed Description

In order to make the technical means, the creation characteristics, the achievement purposes and the effects of the invention easy to understand, the invention is further described with the specific examples.

1 problem statement

Consider a non-linear discrete system as shown below:

\{\begin{matrix} x_{k} = f (x_{k - 1}) + w_{k - 1} \\ z_{k} = h (x_{k}) + v_{k} \end{matrix} - - - (1)

wherein k is more than or equal to 0 and is a discrete time variable; f (x)_k-1) And h (x)_k) Respectively a system nonlinear state function and a measurement function; x is the number of_kAnd z_kRespectively an n-dimensional state vector and a l-dimensional measurement vector of the system; w is a_kAnd v_kN-dimensional system noise and l-dimensional measurement noise, respectively, and having the following statistical properties:

in many cases, the noise mean is non-zero mean, so the system noise and the measurement noise are not made to be non-zero mean gaussian noise and their means are q and r, respectively:

visible,. mu._kAndthe discrete nonlinear system (1) is zero-mean white gaussian noise which is uncorrelated with each other and the variance is Q and R respectively, so the discrete nonlinear system is equivalent to:

2-adaptive UKF filtering algorithm

2.1 UKF algorithm when noise mean is non-zero

Step 1. initialize system variables

{\hat{x}}_{0} = E (x_{0}) - - - (5)

P_{0} = E ((x_{0} - {\hat{x}}_{0}) {(x_{0} - {\hat{x}}_{0})}^{T}) - - - (6)

Step 2, sigma points and corresponding weights are calculated

From the determined sampling strategy, the obedient mean value is obtainedVariance is P_k-1Sigma point { X of_i,k-1}; where i is 0,1,2, …,2n, operatorRepresenting a symmetric Cholesky decomposition.

\{\begin{matrix} X_{0, k - 1} = x_{k - 1} \\ X_{i, k - 1} = x_{k - 1} + {(\sqrt{(n + 1) P_{k - 1}})}_{i}, i = 1,2, . . ., n \\ X_{i, k - 1} = x_{k - 1} - {(\sqrt{(n + 1) P_{K - 1}})}_{i}, i = n + 1, n + 2, . . ., 2 n \end{matrix} - - - (7)

The corresponding weight of the Sigma point is as follows

Wherein α describes the extent of the Sigma point around the mean; beta is an adjusting parameter, l ═ alpha²(n + k) -n, k describe the system distribution information.

Step 3. time update

Sigma Point X obtained according to step 2_i,k-1(i is 0,1,2, …,2n) and its corresponding weight, so that X is equal to_i,k-1After propagation through the nonlinear state function f (·) + q, X_i,k|k-1According to X_i,k|k-1One-step state prediction can be obtainedAnd its corresponding error covariance matrix P_k|k-1

X_i,k|k-1＝f(X_i,k-1)+q，i＝0,1,…,2n (9)

Step 4, sigma points and corresponding weights are calculated

Then obtaining the obedient mean value ofVariance is P_k|k-1Sigma point { X of_i,k|k-1},i＝0,…,2n

\{\begin{matrix} X_{0, k | k - 1} = x_{k | k - 1} \\ X_{i, k | k - 1} = x_{k | k - 1} + {(\sqrt{(n + 1) P_{k | k - 1}})}_{i}, i = 1,2, . . ., n \\ X_{i, k | k - 1} = x_{k | k - 1} - {(\sqrt{(n + 1) P_{k | k - 1}})}_{i}, i = n + 1, n + 2, . . ., 2 n \end{matrix} - - - (12)

The corresponding weight is consistent with the formula (8) in the step 2.

Step 5, updating measurement

Then according to newly obtained Sigam point X in step 4_i,k|k-1(i is 0, …,2n) and weight, let X_i,k|k-1Z is obtained after propagation through a nonlinear measurement function h (·) + r_i,k|k-1Then from z_i,k|k-1Deriving output predictionsAnd auto-covariance matrixSum cross covariance matrix

z_i,k|k-1＝h(X_i,k|k-1)+r，i＝0,1,…,2n (13)

Step 6, filtering updating

After time and measurement updates have passedThe filter gain matrix K is obtained by calculation_kState vector estimation at time kAnd its corresponding covariance matrix P_k|k-1

K_{k} = P_{{\hat{x}}_{k} {\hat{z}}_{k}} {(P_{{\hat{z}}_{k}})}^{- 1} - - - (18)

P_{k | k - 1} = P_{k | k - 1} - K_{k} P_{{\hat{z}}_{k}} {(K_{k})}^{T} - - - (20)

2.2 adaptive UKF Algorithm

2.2.1 constant noise estimator

From document [7], it is known that a suboptimal unbiased MAP constant noise statistical estimator is obtained based on the maximum a posteriori estimation principle:

wherein,respectively representing the mean value of system noise, mean value of measured noise, covariance value of system noise, and covariance value of measured noise at time kThe value is obtained.

2.2.2 evanescent factor

Assume weighting factor { beta }_iSatisfy: beta is a_i＝β_i-1b andwherein 0<b<And 1 is a forgetting factor.

Obtaining the weight coefficient and the time { beta ] according to an equal ratio series summation formula and the constraint condition of the weight coefficient_iThe related general expression is

2.2.3 adaptive UKF Algorithm with time-varying noise estimator

From document [10], it is known that the weight coefficient 1/k in equations (21) and (24) is replaced by an fading factor d (k), so as to obtain an fading memory time-varying noise statistical estimator:

accordingly, an adaptive UKF filtering algorithm can be derived by combining equations (5) and (20) and equations (26) and (29):

K_{k} = P_{{\hat{x}}_{k} {\hat{z}}_{k}} {(P_{{\hat{z}}_{k}})}^{- 1} - - - (37)

P_{k | k} = P_{k | k - 1} - K_{k} P_{{\hat{z}}_{k}} {(K_{k})}^{T} - - - (39)

3 maximum likelihood criterion and Q, R array online estimation adjustment

According to the document [4]]The proposed idea uses the combination of matrix derivative formula and UKF filter formula to deduce the problem of estimating and adjusting R and Q matrix based on maximum likelihood estimation criterion into 1 determinationAnd a ═ a (Q, R) (representing the noise variance matrix that needs to be estimated).

The system innovation variance is not set as:

in the formula, innovation:

the window size N is estimated.

The maximum likelihood estimation is to realize real-time estimation and adjustment of the Q, R array from the angle of maximum occurrence probability of system measurement, and is based on an objective function J (a) under the maximum likelihood condition:

in the formula, | · | represents a determinant.

The derivation of a converts the adaptive filtering problem into the derivation of innovation variance on a:

<math><mrow> <munderover> <mi>Σ</mi> <mrow> <mi>i</mi> <mo>=</mo> <mi>k</mi> <mo>-</mo> <mi>N</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>k</mi> </munderover> <mo>{</mo> <mi>tr</mi> <mo>{</mo> <msup> <msub> <mi>P</mi> <msub> <mi>V</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mfrac> <mrow> <mo>&PartialD;</mo> <msub> <mi>P</mi> <msub> <mi>V</mi> <mi>k</mi> </msub> </msub> </mrow> <mrow> <mo>&PartialD;</mo> <mi>a</mi> </mrow> </mfrac> <mo>}</mo> <mo>-</mo> <msubsup> <mi>ϵ</mi> <mi>i</mi> <mi>T</mi> </msubsup> <msup> <msub> <mi>P</mi> <msub> <mi>V</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mfrac> <mrow> <mo>&PartialD;</mo> <msub> <mi>P</mi> <msub> <mi>V</mi> <mi>k</mi> </msub> </msub> </mrow> <mrow> <mo>&PartialD;</mo> <mi>a</mi> </mrow> </mfrac> <msup> <msub> <mi>P</mi> <msub> <mi>V</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>ϵ</mi> <mi>i</mi> </msub> <mo>}</mo> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>43</mn> <mo>)</mo> </mrow> </mrow></math>

in the formula, tr represents a trace of the sampling matrix.

Obtaining according to a standard unscented Kalman filter equation:

3.1R array estimation

To obtain a real-time estimated expression for the R-array, the derivation of equation (44) a is reduced jointly with equation (43) without assuming that the Q-array is completely known:

taking the covariance matrix of the innovation sequence as a positive definite matrix, and obtaining the condition that the above formula is satisfied in the whole estimation window:

therefore, the real-time estimation value of the observed noise covariance:

3.2Q matrix estimation

In the same way, a real-time estimation expression of the Q array is obtained, and the R array is not assumed to be completely accurately known.

Let the state estimation error be:

then combine equations (40) and (42) in conjunction with the standard unscented kalman filter equation to obtain a real-time estimate of the process noise covariance:

4 improved self-adaptive UKF filtering algorithm

4.1 estimation Window Range

The determination of the estimation window has certain risks, which are reflected in that the estimation window is too large, the unbiased performance of the filtering result is better, but the dynamic performance of the system model is possibly poor; the estimation window is too small, and although the change of the system model can be better reflected, the information contained in the observed value cannot be fully applied, the unbiased property of the filtering process cannot be ensured, and the divergence of filtering even caused by serious conditions cannot be ensured.

4.2 Filtering the cause of divergence and the rules for determining the estimated values for selecting the optimal Q, R matrix

Due to the fact that the prior statistical property of the noise is unknown and time-varying, the actual covariance of the noise deviates from the true value, and therefore the error of state estimation is increased, and the actual covariance is the root cause of filtering divergence.

The UKF adopts a kalman filtering frame, kalman filtering is growth memory filtering, the initial stage is very large, measurement information can be fully utilized, and a strong correction effect is provided, so that an estimation value is fast and close to an estimation state, but the estimation value is gradually reduced along with the time, and the gain K of the filtering process is gradually reduced_kThe smaller the measurement data is, the weaker the correction effect of the new measurement data reflecting the real state is in the estimation, and the data saturation phenomenon is formed, which causes the filtering divergence.

According to the filtering performance analysis of the document [7] and the filtering divergence cause analysis, the optimal Q, R array estimation values are as close to converging on the true values as possible, so that the Q, R array estimation values which are closest to the true values and have more stable convergence are selected. The noise covariance is unknown and the initial value is not necessarily accurate, so the noise statistic estimator needs a certain time to estimate and adapt to achieve stability.

Therefore, we can complete the estimation of the Q, R matrix by the following steps:

the first step is as follows: judging whether convergence has been stabilized

Q_k-Q_k-2≤H (46)

R_k-R_k-2≤H (47)

Wherein H is a small positive integer

The second step is that: selecting a figure of merit

It is not assumed that the estimated covariance of the two algorithms is Q1_k、R1_k、Q2_k、R2_kThen, then

{\hat{Q}}_{k} = \frac{{Q 1}_{k} + {Q 2}_{k}}{2} - - - (48)

{\hat{R}}_{k} = \frac{{R 1}_{k} + {R 2}_{k}}{2} - - - (49)

In the formula,is the covariance estimate.

4.3 implementation of the improved adaptive UKF filtering algorithm:

1) classical UKF filtering is achieved by equations (5) and (20).

2) The estimation of the noise mean is achieved by equations (26) and (27).

3) Noise covariance estimation is performed by equations (44) and (46) and (28) and (29), and the best estimated covariance is selected as noise by comparing the results of the two according to the rule set forth in 4.2.

4) Replacing the corresponding noise statistical characteristics in 1) with the noise statistical characteristics obtained in 2) 3).

5 simulation analysis

The effectiveness of the improved adaptive UKF filtering algorithm is verified by adopting a strong nonlinear Gaussian system model of document [5 ]:

wherein w_kAnd v_kAre white gaussian noise and their constant statistical properties are as follows:

\{\begin{matrix} q = 1.2 \\ Q = 0.6 \end{matrix} - - - (51)

\{\begin{matrix} r = 1.0 \\ R = 0.8 \end{matrix} - - - (52)

setting the initial value of the nonlinear system sum as:

\{\begin{matrix} {\hat{x}}_{0} = 1.1 \\ P_{0} = 0.01 \end{matrix} - - - (53)

and isAnd w_kAnd v_kAre not related to each other.

Considering the strongly nonlinear gaussian system model presented above, it is assumed that the measurement noise statistics are accurately known (as in equation), while the process noise statistics are unknown or inaccurate, and the initial mean and covariance are taken as:

{\hat{q}}_{0} = 0.2, {\hat{Q}}_{0} = 0.5

in order to verify the superiority of the improved adaptive UKF filtering algorithm, the UKF algorithm proposed by the document [7] and the adaptive UKF algorithm proposed by the invention are respectively adopted to carry out adaptive estimation simulation on Q, and the simulation result is as follows:

still consider the above strong nonlinear gaussian system model, assuming that at this time, the system noise w and v are both gaussian noise and obey normal distribution, the process noise of the nonlinear system is accurately known (as shown in the equation) and the statistical characteristics of the measured noise are unknown or inaccurate, and the initial value mean and covariance are taken as:

{\hat{r}}_{0} = 0.3, {\hat{R}}_{0} = 0.6

in order to verify the superiority of the improved adaptive UKF filtering algorithm, the UKF algorithm proposed in the document [7] and the adaptive UKF algorithm proposed by the invention are respectively adopted to carry out adaptive estimation simulation on R

While the invention has been described with reference to a preferred embodiment, it will be understood by those skilled in the art that various changes may be made and equivalents may be substituted for elements thereof without departing from the scope of the invention.

Reference to the literature

[1] Lushubo, yang forever, worship loyalty. INS/GPS integrated navigation system simulation based on UKF [ J ]. science and technology and engineering, 2011,04: 773-.

[2] Field, new rock, a simplified SAGE-HUSA kalman filter [ J ] bulletin and arrow and guidance bulletin, 2011,01:75-77+84.

[3] Application of the Stone courage, Korea, Showa, adaptive UKF algorithm in target tracking [ J ]. automatic school newspaper, 2011,06: 755-.

[4] Yue Xiao Kui, Yuan Jianping an adaptive Kalman filtering algorithm [ J ] based on a maximum likelihood criterion, academic newspaper of northwest industry university, 2005,04: 469-.

[5] Wanglong, Liguangchun, QiaoXiangwei, Meng Meglong, horse wave, adaptive UKF algorithm [ J ] based on maximum likelihood criterion and maximum expectation algorithm, 2012,07:1200 + 1210.

[6] Von Johnua, Wu iron force, Marlon. high dynamics AUKF-based carrier tracking algorithm [ J ] computer engineering, 2012,16: 237-.

[7] Zhao Lin, Wang Xiao Xua, Sun Ming, D Cheng, Yan super adaptive UKF filter algorithm [ J ] based on maximum posterior estimate and exponential weighting, 2010,07: 1007-one 1019.

[8] Dengjiang Ann, Kongxiangwei, Sunxingpeng, Lucleng.an adaptive UKF filter is used to track maneuvering target [ J ]. firepower and command control, 2008, S1:56-59.

[9] The height is wide, which sea wave, Chenjinping, the self-adaptive UKF algorithm and the application thereof in the GPS/INS combined navigation [ J ]. the university of Beijing science and engineering, 2008,06:505 and 509.

[10] Hugaoge, high society, Zhao rock, a new adaptive UKF algorithm and its application in integrated navigation [ J ]. Chinese inertial technology bulletin, 2014,03: 357-.

[11] Liu, UKF algorithm and its improved algorithm study [ D ]. university of south and middle, 2009.

Claims

1. An adaptive UKF filtering algorithm, characterized by comprising the steps of: the first step is as follows: judging whether convergence has been stabilized

Q_k-Q_k-2≤H

R_k-R_k-2≤H

Wherein H is a small positive integer;

the second step is that: selecting a figure of merit

{\hat{Q}}_{k} = \frac{{Q 1}_{k} - {Q 2}_{k}}{2}

{\hat{R}}_{k} = \frac{{R 1}_{k} + {R 2}_{k}}{2}

In the formula,is the covariance estimate.