CN107632959A - A kind of multi-model self calibration kalman filter method - Google Patents

Abstract

The present invention provides a multi-model self-calibration Kalman filter method, the steps are as follows: one: establish the basic equation of the system; two: perform filter initialization to the system composed of formula (1), formula (2) and formula (3); Three: time update the system; four: iterative variable update; five: measurement update; six: iterative calculation; the present invention introduces multi-model estimation theory into the problem that the system state equation is disturbed by unknown input, based on self-calibration Kalman Filtering and standard Kalman filtering, the complete process of the multi-model self-calibrating Kalman filtering method is obtained; it not only solves the problem of standard Kalman filtering divergence in unknown input non-zero segment filtering, but also significantly improves the self-calibrating Kalman filtering in unknown The filtering accuracy of the input is zero segment; the invention improves the filtering accuracy of the unknown input of zero segment and non-zero segment at the same time, the scope of application is further expanded, and the system robustness is further improved on the basis of the self-calibration Kalman filter method promote.

Description

A multi-model self-calibration Kalman filter method

【Technical field】

The invention provides a multi-model self-calibration Kalman filtering method, which belongs to the technical field of robust Kalman filtering.

【Background technique】

Kalman filtering is a method of estimating the system state by using the system state equation and measurement equation. Since it was proposed in 1960, it has been widely used in signal processing, navigation, deep space exploration and fault diagnosis and other fields. The standard Kalman filter method is only for linear systems. On this basis, researchers have proposed Extended Kalman Filter (Extended Kalman Filter, EKF), Unscented Kalman Filter (Unscented Kalman Filter, UKF) and Particle Filter (Particle Filter). ,PF) and a series of methods, they extend the scope of application of the Kalman filtering method to nonlinear systems, and further improve the filtering accuracy.

However, whether the above filtering methods are aimed at linear systems or nonlinear systems, the system equations are required to be accurate. However, in engineering practice, due to the influence of environmental factors, improper selection of models and parameters, etc., the system state equation is often disturbed by unknown inputs, which reduces the filtering accuracy and even leads to filtering divergence. In response to this problem, the document "Self-calibration Kalman filter method [J]. Aerodynamics Journal. 2014, 29 (06): 1363-1368" proposed a self-calibration Kalman filter method (Self-calibration Kalman Filter, SKF), The method estimates the unknown input items while performing iterative operations according to the original state equation, so that the influence of the unknown input is automatically compensated.

But due to the uncertainty of the system, the unknown input of the system state equation may also be zero. In this case, since the self-calibration Kalman filter method introduces an unknown input estimation term, although this term will gradually converge to zero as the filtering progresses, considering the volatility, time delay and error of the convergence process , its filtering accuracy is not as good as the standard Kalman filtering method. Therefore, while maintaining the accuracy of the unknown input non-zero segment filter, improving the accuracy of the unknown input to the zero segment is the direction for further improvement of the self-calibration Kalman filter method.

Multiple-Model Estimation (MME) provides an effective way to solve this problem. The basic principle is: since artificial modeling cannot be completely accurate, multiple dynamic models are established to describe a system at the same time. Yees' theorem iteratively calculates the conditional probability corresponding to each dynamic model when the measured value is known, and then obtains the final state estimation value through weighted fusion.

【Content of invention】

The purpose of the present invention is to provide a multi-model self-calibration Kalman filter method (Multiple-model Self-calibration Kalman Filter, MSKF), it uses self-calibration Kalman filter and standard Kalman filter to carry out iterative operation simultaneously, and in each The weights corresponding to the filtering results of the two methods are obtained at all times, and their respective advantages at different stages are brought into play through weighted fusion. A large number of simulation results show that the invention can ensure good filtering results no matter whether the system is affected by unknown input or not.

A kind of multi-model self-calibration Kalman filter method of the present invention, it comprises following six steps:

Step 1: Establish the basic equations of the system

The multi-model self-calibration Kalman filter adopts two methods of self-calibration Kalman filter and standard Kalman filter, so the system contains two state equations, the first is the self-calibration state equation, and the second is the standard state equation, Its specific expression is

Z _k ＝H _k X _k +V _k (3)

where X _k represents the state vector of the system, with Corresponding to the dynamic model with unknown input and the standard dynamic model, Z _k represents the system measurement vector, Φ _k and H _k are the state transition matrix and measurement matrix, respectively, b _k represents the unknown input, W _k and V _k are the system noise vector and the measurement noise vector respectively, and their variance matrices are Q _k and R _k respectively, and satisfy

In the formula, Cov[ ] is covariance, E[ ] is mathematical expectation, δ _kj is δ function, when k=j, δ _kj =1, when k≠j, δ _kj =0;

Step 2: Perform filter initialization on the system composed of formula (1), formula (2) and formula (3)

Set the initial value of the state estimation and estimation error variance matrix to be

At the same time, in order to complete the fusion of the estimated results of the two models, it is also necessary to set the initial value of the probability of the two models

Pr(1|Z ₃ )=Pr(2|Z ₃ )=0.5 (7)

And the probability initial values Pr _max and Pr _min for iterative calculation; the reasons for initializing Pr _max and Pr _min are as follows:

In the process of multi-model estimation calculation, some models will be eliminated because the corresponding probability gradually approaches zero, so the number N of models participating in the calculation is constantly decreasing, which will reduce the adaptability of the system to complex environments; The solution to this problem is to set a probability lower limit for the system. When the probability value of a certain model is lower than this lower limit, the probability value will remain at this lower limit and will not change; the advantage of this treatment is that it does not affect the estimation results. At the same time, the diversity of the model is maintained, but the disadvantage is also obvious, that is, if the model at the lower limit of the probability is to function again, its probability value needs a long time of iterative operation to recover, which has a great impact on the real-time performance of Kalman filtering. influence; in order to overcome this shortcoming, only two dynamic models are selected for the method of this paper and the prior estimation value is selected by the highest probability method, so it is only necessary to qualitatively analyze the probability of the two models without accurately calculating the characteristics of the probability value. The multi-model self-calibration Kalman filter method no longer uses the calculated conditional probability value to iterate, but sets two determined initial probability values Pr _min and Pr _max =1-Pr _min ; before each step of filtering, by comparing The size of the probability result in the previous step is assigned to the two models respectively, and the model probability at the current moment is updated with them as initial values; since Pr _min is not a minimum value like the lower limit of the probability, the speed of probability recovery can be guaranteed, so that The real-time performance of Kalman filtering is guaranteed;

Step 3: Update the system time

When k=1,2, state one-step forecast value

One-step forecast error variance matrix

P _1/0 ＝Φ ₀ P ₀ Φ ₀ ^T +Q ₀ (10)

P _2/1 ＝Φ ₁ P ₁ Φ ₁ ^T +Q ₁ (11)

When k>2, state one-step forecast value

One-step forecast error variance matrix

In the formula

J＝argmax _j Pr(j|Z _k ) (14)

where the function argmax[f(x)] returns the value of x when f(x) is maximum, and

S ₁ =P ₁ (22)

In the above calculation process, the formula (15) provides the one-step prediction result of the self-calibration state equation, and the formula (16) provides the one-step prediction result of the standard state equation calculation, and then through the comparison of the probability in the formula (12), the formula ( 14) Complete the final one-step prediction value screening; the calculation of the probability depends on the Bayesian principle, and the comparison is the conditional probability of the two models when the measured value at the current moment is known, such as formula (19) and formula As shown in (20), other calculation processes come from the multi-model estimation theory; the schematic diagram of the time update process is shown in Figure 2;

Step 4: Iterative variable update

In step (3), there are many intermediate variables that need to be updated in real time, so it is necessary to obtain their recursive formulas to ensure the smooth progress of the entire filtering process; these iterative variables include:

Each model measurement update

Conditional probability reset for each model

Pr(J|Z _k )＝Pr _max (28)

Pr[(3-J)|Z _k ]＝Pr _min (29)

;

Step 5: Measurement update

filter gain matrix

state estimate

Since the calculation formula of this step does not involve the basic assumptions of the self-calibration technology and the relevant content of the multi-model estimation theory, the calculation process is consistent with the posterior estimation formula of the standard Kalman filtering method; The calibration Kalman filter method is calculated by the fusion of two model results, so it is not necessary to estimate the transfer of the error variance matrix between adjacent moments in the calculation process, so the calculation formula of _Pk is not given in the calculation step ;

Step 6: Iterative calculation

State estimates obtained from the two models at time k and the error variance matrix P _k , repeat steps (3), (4) and (5), and then obtain the estimated value of the state at k+1 time, and iterate back and forth until the filtering process ends.

In summary: step (1) establishes two dynamic models (state equations) required by the multi-model estimation theory, which provides prerequisites for the subsequent fusion of estimation results; step (2) ensures the normal startup of the filtering method Step (3) as the core step of the present invention has completed the fusion of two model estimation results, so that the present invention can adapt to different complex environments; Step (4) is an important guarantee that step (3) can be carried out smoothly. For the update of intermediate variables, step (3) can be used normally at every moment; step (5) provides a posteriori estimation of the state by introducing measurement information, which further improves the filtering accuracy; step (6) The normal operation of the filtering is guaranteed, so that it can run to the actual required end time; that is to say, the present invention introduces the multi-model estimation theory into the problem that the system state equation is disturbed by unknown input, based on self-calibration Kalman filter and standard Karl Mann filter, the complete process of the multi-model self-calibration Kalman filter method is derived theoretically; this invention not only solves the problem of standard Kalman filter divergence in unknown input non-zero segment filtering, but also significantly improves the self-calibration Kalman filter The filtering accuracy when the unknown input is zero segment; the invention improves the filtering accuracy of the unknown input being zero segment and non-zero segment at the same time, the scope of application is further expanded, and the system robustness is also improved on the basis of the self-calibration Kalman filter method further improvement.

Advantage and positive effect of the present invention are:

(1) The multi-model estimation theory is introduced into the problem that the system state equation is disturbed by unknown input. Based on the self-calibration Kalman filter and the standard Kalman filter, the complete process of the multi-model self-calibration Kalman filter method is theoretically derived.

(2) The invention combines the advantages of various methods, which not only solves the problem of standard Kalman filter divergence when the unknown input is non-zero, but also significantly improves the filtering accuracy of the self-calibration Kalman filter when the unknown input is zero.

(3) The invention simultaneously improves the filtering accuracy of the unknown input being zero segment and non-zero segment, the scope of application is further expanded, and the robustness of the system is further improved on the basis of the self-calibration Kalman filter method.

【Description of drawings】

Fig. 1 is a schematic flow chart of the method of the present invention.

FIG. 2 is a schematic diagram of the time updating process in step (3).

The serial numbers, symbols and codes in the invention are explained as follows:

X _k : the state vector of the system

State vector for a kinetic model with unknown inputs

The state vector of the standard kinetic model

Z _k : system measurement vector

Φ _k : state transition matrix

H _k : measurement matrix

b _k : unknown input

W _k : System noise vector

V _k : measurement noise vector

Q _k : System noise vector variance matrix

R _k : measurement noise vector variance matrix

Cov[ ]: covariance

E[·]: Mathematical Expectation

δ _kj : δ function

Pr(1|Z _k ), Pr(2|Z _k ): the initial value of the probability of iterative calculation

Pr _max , Pr _min : the initial value of the probability used for iterative calculation

P _k/k-1 : one-step forecast error variance matrix

argmax[f(x)]: function maximum return function

r _k : residual

T _k : measurement estimation error variance matrix

K _k : filter gain matrix

state estimation

【detailed description】

The present invention will be described in detail below in conjunction with the accompanying drawings.

The present invention proposes a kind of multi-model self-calibration Kalman filter method, its flow chart as shown in Figure 1, time updating flow chart as shown in Figure 2, it comprises following six steps:

Step 1: Establish the basic equations of the system

Z _k =H _k X _k +V _k (34)

where X _k represents the state vector of the system, with Corresponding to the dynamic model with unknown input and the standard dynamic model, Z _k represents the system measurement vector, Φ _k and H _k are the state transition matrix and measurement matrix, respectively, b _k represents the unknown input, W _k and V _k are the system noise vector and the measurement noise vector respectively, and their variance matrices are Q _k and R _k respectively, and satisfy

In the formula, Cov[ ] is covariance, E[ ] is mathematical expectation, δ _kj is δ function, when k=j, δ _kj =1, when k≠j, δ _kj =0.

Step 2: Filter initialization

Set the initial value of the state estimation and estimation error variance matrix to be

At the same time, set the initial value of the probability of the two models

Pr(1|Z ₃ )=Pr(2|Z ₃ )=0.5 (38)

And the probability initial values Pr _max and Pr _min for iterative calculation.

Step 3: Time update

When k=1,2, state one-step forecast value

One-step forecast error variance matrix

P _1/0 ＝Φ ₀ P ₀ Φ ₀ ^T +Q ₀ (41)

P _2/1 ＝Φ ₁ P ₁ Φ ₁ ^T +Q ₁ (42)

When k>2, state one-step forecast value

One-step forecast error variance matrix

In the formula

J＝argmax _j Pr(j|Z _k ) (45)

where the function argmax[f(x)] returns the value of x when f(x) is maximum, and

S ₁ =P ₁ (53)

The schematic diagram of the time update process is shown in Figure 2;

Step 4: Iterative variable update

Each model measurement update

Conditional probability reset for each model

Pr(J|Z _k )＝Pr _max (59)

Pr[(3-J)|Z _k ]＝Pr _min (60)

;

Step 5: Measurement update

filter gain matrix

state estimate

;

Step 6: Iterative calculation

k=k+1 (65)

Repeat steps 3, 4 and 5 until the filtering process ends.

Claims (1)

1. A kind of 1. multi-model self calibration kalman filter method, it is characterised in that：It includes following six step：

   Step 1：Establish system fundamental equation

   Multi-model self calibration Kalman filtering is transported using two methods of self calibration Kalman filtering and standard Kalman filtering Calculate, therefore system includes two state equations, first is self calibration state equation, and second state equation for standard, it has Body expression formula is

   <mrow> <msubsup> <mi>X</mi> <mi>k</mi> <mn>1</mn> </msubsup> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>+</mo> <msub> <mi>b</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>W</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msubsup> <mi>X</mi> <mi>k</mi> <mn>2</mn> </msubsup> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <msub> <mi>W</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

   Z_k=H_kX_k+V_k·············(3)

   In formula, X_kThe state vector of expression system,WithCorresponding kinetic model and standard containing Unknown worm is dynamic respectively Mechanical model, Z_kRepresent system measurements vector, Φ_kAnd H_kRespectively state-transition matrix and measurement matrix, b_kRepresent unknown defeated Enter, W_kWith V_kRespectively system noise vector sum measures noise vector, and its variance matrix is respectively Q_kAnd R_k, and meet

   <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>C</mi> <mi>o</mi> <mi>v</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>W</mi> <mi>j</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <msubsup> <mi>W</mi> <mi>j</mi> <mi>T</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <msub> <mi>&amp;delta;</mi> <mrow> <mi>k</mi> <mi>j</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>C</mi> <mi>o</mi> <mi>v</mi> <mo>&amp;lsqb;</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>V</mi> <mi>j</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> <msubsup> <mi>V</mi> <mi>j</mi> <mi>T</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <msub> <mi>&amp;delta;</mi> <mrow> <mi>k</mi> <mi>j</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>C</mi> <mi>o</mi> <mi>v</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>V</mi> <mi>j</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <msubsup> <mi>V</mi> <mi>j</mi> <mi>T</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>...</mn> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>

   In formula, Cov [] is covariance, and E [] is mathematic expectaion, δ_kjFor δ functions, as k=j, δ_kj=1, as k ≠ j, δ_kj=0；

   Step 2：Initialization is filtered to the system being made up of formula (1), formula (2) and formula (3)

   Set the initial value of state estimation and estimation error variance matrix as

   <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>X</mi> <mn>0</mn> </msub> <mo>&amp;rsqb;</mo> <mn>...</mn> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msub> <mi>P</mi> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> <mn>...</mn> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>

   Meanwhile in order to complete the fusion of two model estimated results, it is also necessary to set the probability initial value of two kinds of models

   Pr(1|Z₃)=Pr (2 | Z₃)=0.5 (7)

   And the probability initial value Pr for iterative calculation_maxAnd Pr_min；Initialize Pr_maxAnd Pr_minThe reason for it is as follows：

   In multiple-model estimator calculating process, some models can be eliminated due to the gradual convergence of corresponding probability for zero, therefore be joined Model quantity N with computing is constantly reducing, and this can reduce adaptability of the system to complex environment；Only chosen for the present invention Two kinetic models and use maximum probability method selection priori estimates, therefore only need the probability of two kinds of models of qualitative analysis big It is small without accurate the characteristics of calculating probable value, multi-model self calibration kalman filter method does not use the bar being calculated Part probable value is iterated, but sets the probability initial value Pr of two determinations_minAnd Pr_max=1-Pr_min；Filtered in each step Before, it is assigned to two models respectively by comparing the size of previous step probability results, and worked as using them as initial value renewal The model probability at preceding moment；Due to Pr_minIt is not minimum as probability lower limit, therefore the speed that guarantee probability recovers, from And the real-time of Kalman filtering is set to be guaranteed；

   Step 3：Time renewal is carried out to system

   Work as k=1, when 2, state one-step prediction value

   <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mn>1</mn> <mo>/</mo> <mn>0</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mn>0</mn> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mn>2</mn> <mo>/</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mn>1</mn> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>

   One-step prediction varivance matrix

   <mrow> <msub> <mi>P</mi> <mrow> <mn>1</mn> <mo>/</mo> <mn>0</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mn>0</mn> </msub> <msub> <mi>P</mi> <mn>0</mn> </msub> <msubsup> <mi>&amp;Phi;</mi> <mn>0</mn> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mn>0</mn> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>

   P_2/1=Φ₁P₁Φ₁ ^T+Q₁············(11)

   As k ＞ 2, state one-step prediction value

   <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>J</mi> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>

   One-step prediction varivance matrix

   <mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>J</mi> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>

   In formula

   J=argmax_jPr(j|Z_k)···········(14)

   <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>-</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <msubsup> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;times;</mo> <msubsup> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <msubsup> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;times;</mo> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <msup> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>

   Wherein, function argmax [f (x)] returns to the value of the x when f (x) is maximum, and

   <mrow> <mi>Pr</mi> <mrow> <mo>(</mo> <mi>j</mi> <mo>|</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mi>d</mi> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>|</mo> <mi>j</mi> <mo>)</mo> </mrow> <mi>Pr</mi> <mrow> <mo>(</mo> <mi>j</mi> <mo>|</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>p</mi> <mi>d</mi> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>|</mo> <mi>i</mi> <mo>)</mo> </mrow> <mi>Pr</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>|</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> <mn>...</mn> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <mi>p</mi> <mi>d</mi> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>|</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>&amp;ap;</mo> <mfrac> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <mo>-</mo> <msubsup> <mi>r</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msubsup> <mi>T</mi> <mi>k</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>r</mi> <mi>k</mi> </msub> <mo>/</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <mrow> <mo>(</mo> <mn>2</mn> <mi>&amp;pi;</mi> <mo>)</mo> </mrow> <mrow> <mi>q</mi> <mo>/</mo> <mn>2</mn> </mrow> </msup> <mo>|</mo> <msub> <mi>T</mi> <mi>k</mi> </msub> <msup> <mo>|</mo> <mrow> <mn>1</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> </mrow> </mfrac> <mn>...</mn> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msub> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>3</mn> </mrow> </msub> <msubsup> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>3</mn> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> <mn>....</mn> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>

   S₁=P₁··············(22)

   <mrow> <msub> <mi>r</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msub> <mi>T</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow>

   In above-mentioned calculating process, formula (15) provides the one-step prediction result of self calibration state equation, and formula (16) provides standard shape The one-step prediction result that state equation calculates, then by the comparison of probability size in formula (12), final one is completed by formula (14) Walk predicted value screening；The conditional probability of two kinds of models, as shown in formula (19) and formula (20)；

   Step 4：Iteration variable updates

   There are many intermediate variables to need real-time update in step (3), it is therefore necessary to their recurrence formula is obtained, and then Ensure being smoothed out for whole filtering；These iteration variables include：

   Each model measurement renewal

   <mrow> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mn>...</mn> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>+</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mn>...</mn> <mrow> <mo>(</mo> <mn>26</mn> <mo>)</mo> </mrow> </mrow>

   <mrow> <msubsup> <mi>P</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msub> <mi>R</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mn>......</mn> <mrow> <mo>(</mo> <mn>27</mn> <mo>)</mo> </mrow> </mrow>

   Each Model Condition probability is reset

   P_r(J|Z_k)=Pr_max············(28)

   Pr[(3-J)|Z_k]=Pr_min···········(29)

   ；

   Step 5：Measure renewal

   Filtering gain matrix

   <mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mn>...</mn> <mrow> <mo>(</mo> <mn>30</mn> <mo>)</mo> </mrow> </mrow>

   State estimation

   <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>...</mn> <mrow> <mo>(</mo> <mn>31</mn> <mo>)</mo> </mrow> </mrow>

   The calculation formula of this step due to not being related to the basic assumption of self-calibration technique and the related content of Multiple model estimation theory, So calculating process and the Posterior estimator formula of standard Kalman filtering method are consistent；It should be noted simultaneously that due to Multi-model self calibration kalman filter method is obtained by the fusion calculation of two model results, therefore in calculating process And transmission of the estimation error variance matrix between adjacent moment is not needed, so not providing P in calculation procedure_kCalculating it is public Formula；

   Step 6：Iterative calculation

   The state estimation respectively obtained according to the two of the k moment modelsWith varivance matrix P_k, repeat step (three), (4) and (five), and then the state estimation at k+1 moment is obtained, reciprocal iteration, until filtering terminates.