CN112764345A - Strong nonlinear system Kalman filter design method based on target state tracking - Google Patents

Abstract

The invention discloses a strong nonlinear system Kalman filter design method based on target state tracking. The method defines a basic function in a target state and a measurement equation as a hidden variable, so that an original target state model and a measurement model are rewritten into a pseudo-linear form; then, taking the hidden variables as parameter variables of the system, and establishing a dynamic linear model among the hidden variables, other hidden variables and target state variables; further rewriting the measurement model into a first-order linear product form between the target state estimation value at the current moment and the variable value of each parameter; and finally, gradually solving the parameter variables by means of a Kalman filter bank, and designing a gradually linearized high-order extended Kalman filter. The effectiveness of the invention is verified by the simulation test of the comparison of the three target state tracking cases.

Description

Strong nonlinear system Kalman filter design method based on target state tracking

Technical Field

The invention belongs to the field of target state tracking of strong nonlinear dynamic systems, and particularly relates to the field of target state tracking of strong nonlinear systems consisting of products of linear functions and a plurality of separable basic functions.

Background

The state estimation theory has been widely applied in various fields and plays an important role, such as space monitoring, wireless communication, target tracking and the like. The design of the core filter of the state estimation theory is subjected to the continuous processes of lifting, improving, re-lifting and re-improving from the beginning to the end, the performance of the filter is stronger and the application range is wider and wider. However, when a modeling error of a modern filter represented by linear KF is non-white noise or lacks effective statistical characteristics, the model has strong nonlinear characteristics, and other problems, a bottleneck problem to be solved still exists.

In 1942, Weiner et al proposed Wiener filtering (Wiener filtering) based on minimum mean square error, which opened the precedent of filter design. But since wiener filtering is only applicable to stationary stochastic processes, it is difficult to apply it to a wide range of non-stationary stochastic systems.

Kalman Filtering (KF) was proposed by Kalman et al in 1960. KF is not only suitable for non-stationary random process, and has the excellent characteristic of real-time recursion, so it is especially suitable for running on computer, and is rapidly popularized and applied in various fields, especially in the field of national defense. KF is an optimal filter designed by taking the minimum mean square error as a standard on the premise that the model is linear and the modeling error is white noise. However, in the face of many practical projects, the statistical characteristics of modeling noise are difficult to obtain, and particularly in the face of practical dynamic system applications in which the model is nonlinear, the conventional KF is difficult to exert excellent filtering performance.

In 1969, Bucy proposed a filter suitable for nonlinear systems using taylor expansion: extended Kalman Filtering (EKF). The EKF carries out first-order Taylor expansion on a nonlinear state model function and a measurement model function under the state of a current estimation point, approximately converts the nonlinear problem into a linear problem under standard KF, and designs a filter. However, EKF can only implement first order linear approximation and can cause an increase in rounding error as the degree of model nonlinearity increases, thereby causing a decrease in filter performance and even a loss of tracking target due to divergence of the filter estimation algorithm.

In order to improve the approximation capability of a nonlinear function model, the UKF and the CKF are designed in sequence. A large number of examples prove that no matter the UKF or the CKF can achieve the second-order approximation of the nonlinear system at most, the filtering effect is poor due to the fact that high-order information is lost greatly.

In 1991, Kowalski et al proposed Polynomial Extended Kalman Filtering (PEKF) based on Carleman adaptation, similar in concept to Taylor expansion, but far more complex in form than EKF. With the improvement of the nonlinear approximation capability, the number of the variables of the system to be expanded is rapidly expanded in a geometric series form. For a system with higher original variable dimension, a filter algorithm which can recur in real time on the existing computer is difficult to design for a system with dimension expansion, and meanwhile, the problems of limited approximation capability and rounding errors still exist. In order to solve the problem, Zidong Wang carries out new modification aiming at rounding errors, so that the performance of the filter is further improved. But because more dimension-expanding variables are introduced when rounding errors are expanded, the complexity of a filter implementation algorithm is higher compared with PEKF.

Regardless of EKF or PEKF, the original model of the approximate approximation state and measurement near the expansion point is in the form of polynomial series centered on the state latest estimated value. The method has the advantages of consistent format, neatness and uniformity and easy understanding. However, in practical application, the following disadvantages still exist: rewriting each order polynomial in the expansion equation by taking the estimated value as the center at each moment; the coefficients of the polynomials of the orders that are updated at the same time are also recalculated. Formally not only complex, but certainly with increasing computational load as the model dimensions or polynomial expansion order increase. But in practice, when a complex target state tracking system is encountered, the state model and the measurement model are both nonlinear functions composed of linear functions, nonlinear functions, products of the linear functions and nonlinear basic functions, and the like in a summation form. And each part has its own specific physical meaning, especially each primitive function implies a more profound physical meaning. The existing approximate approximation of the nonlinear state model and the measurement model in the form of local expansion polynomial series inevitably destroys the original physical meaning contained in the basic function, and in addition, the rounding error generated in the expansion process of the basic function inevitably reduces the performance of the designed filter and even diverges along with the increase of the nonlinear degree.

Disclosure of Invention

Aiming at the defects of the prior art, the invention designs a novel step-by-step linearized high-order extended Kalman filter aiming at a strong nonlinear target state tracking system consisting of a linear function and a plurality of separable basic function products. The invention targets a target state tracking system, such as f, which is a strong nonlinear state model and a measurement model both composed of a linear function and a plurality of separable basic function products⁽⁰⁾(x)*f⁽¹⁾(x)*f⁽²⁾(x)*…*f^(r)(x) And designing high-order extended Kalman filtering suitable for the strong nonlinear system.

The invention comprises the following steps:

the method comprises the following steps of (1) providing a nonlinear target state tracking model and a measurement model:

wherein x is_j(k) Is the value of the target state and,

is a multiplier, w_i(k) And v_i(k +1) is a gaussian white noise sequence with E { w (k) } ═ 0, E { w (k) w^T(j)}＝Q(k)δ_kj；E{v(k+1)}＝0，E{v(k+1)v^T(j+1)}＝R(k+1)δ_kj. When k is j, delta _kj1, otherwise, δ_kj＝0。a_ijAnd h_ijThe coefficients of the target state tracking model and the measurement model are respectively.

And (2) taking a two-dimensional target state tracking model and a measurement model as examples, defining a basic function as hidden variable parameters of the target state tracking model and the measurement model, and simplifying a nonlinear target state tracking model into a first-order product form of a state and the hidden variable parameters of each order:

for example, two state variables x, with m ═ n ═ r ═ 2₁、x₂Respectively representing the displacement and the speed of the target, carrying out pseudo-linearization description on a target state tracking model and a measurement model by introducing hidden variable parameters, and defining:

wherein the content of the first and second substances,

is a hidden variable parameter relative to the original target state variable x (k).

And (3) taking the hidden variable parameters as parameter variables, namely new variables of the target state tracking model and the measurement model, establishing a dynamic linear model among all the hidden variables, other hidden variables and the target state variables, and further rewriting the measurement model into a first-order linear product form between the target state estimation value at the current moment and the variable values of all the parameters.

Combining a given quasi-linear representation model of the target state model with respect to the original target state variables x (k) and a given respective alpha^(l)(k +1) with respect to the original target state variable x (k) and other parameters α^(u)(k) The linear correlation model between the original target state variable x (k) and all the hidden variable function parameters alpha is realized^(u)(k) Joint step linearization representation of (1).

Based on the joint step-by-step linearization representation of the target state variable and all hidden variable parameters, giving out a linearization representation model only aiming at the original target state variable;

implicit variable parameter alpha for introduction^(l)(k +1), equivalently rewritten with the original target state variable x (k +1) and other hidden variable function parameters α^(u)(k +1), u is 1, and 2 is a linearized representation model of the observation matrix parameters. Therefore, the joint step-by-step linear representation of the state variable of the target to be estimated and all the hidden variable parameters is combined, and the step-by-step observation linear representation of the corresponding state of the target to be estimated and the hidden variable function parameters is realized.

And (4) designing a high-order extended Kalman filter bank consisting of r +1 Kalman filtering by means of introducing a target state tracking model and a measurement model of hidden variable parameters, solving step by step and linearizing an original target state tracking model and the measurement model.

Suppose that the observed target state values y (1), y (2), …, y (k) and the estimated values of the target state tracking model and the measurement model at the moment k have been obtained

Sum estimation error covariance matrix P_α(k|k)，P_x(k | k). Then the 3-phase step Kalman filter at time k → k +1 is designed to: sequentially obtaining estimated values of alpha (k +1) and k (k +1)

And

and corresponding estimation error covariance matrix P_α(k +1| k +1) and P_x(k+1|k+1)。

Step (4-1) regarding parameter α⁽¹⁾Kalman filter design of (k + 1):

assuming that the estimated value of the k time is known

And corresponding estimation error covariance matrix

And P_x(k|k)；

The design of the first stage is as follows: under the condition of known observation target state value y (k +1), designing and solving a hidden parameter variable alpha⁽¹⁾(k +1) state estimation value

Sum estimation error covariance matrix

(1) Designing hidden variable parameter alpha⁽¹⁾(k +1) a Kalman filter;

(2) hidden variable parameter alpha⁽¹⁾(k +1) predicting an estimated value, a predicted estimation error and a predicted estimation error covariance matrix in one step;

(3) the first stage comprises the predicted measured value and the predicted estimation error of the parameter variable;

(4) calculating a gain matrix of the first stage;

(5) a first stage estimation error covariance matrix is calculated.

Step (4-2) regarding parameter α⁽²⁾Kalman filter design of (k + 1):

estimate of known k +1 time

And an estimate of time k

And estimating the error covariance matrix

And P_x(k|k)；

The design of the second stage is as follows: under the condition of known observation target state value y (k +1), designing and solving a hidden parameter variable alpha⁽²⁾(k +1) state estimation value

Sum estimation error covariance matrix

(1) Designing hidden variable parameter alpha⁽²⁾(k +1) a Kalman filter;

(2) hidden variable parameter alpha⁽²⁾(k +1) a state prediction value, a prediction estimation error, and a prediction estimation error covariance matrix;

(3) the second stage comprises a predicted measured value and a predicted estimation error of the parameter variable;

(4) calculating a gain matrix of the second stage;

(5) calculating an estimation error covariance matrix of the second stage;

step (4-3) Kalman filter design for the target state variable x (k + 1):

assuming known estimates

And

and estimating the error covariance matrix

And P_x(k|k)。

The design of the third stage is as follows: under the condition of knowing the observed target state value y (k +1), the design obtains the target state estimation value of the target state variable x (k +1)

Sum estimation error covariance matrix P_x(k+1|k+1)。

(1) Designing a Kalman filter of a target state variable x (k + 1);

(2) a target state prediction value, a prediction error and a prediction error covariance matrix;

(3) the third stage comprises a predicted measured value and a predicted estimation error of the parameter variable;

(4) calculating a gain matrix of the third stage;

(5) and calculating an estimation error covariance matrix of the third stage.

The main technical contributions of the invention are as follows: (1) r basic functions f⁽ⁱ⁾(x) Defined as an implicit variable parameter of the system, alpha⁽ⁱ⁾(k)：＝f⁽ⁱ⁾(x (k)), i ═ 0,1, …, r, and if α is further substituted⁽ⁱ⁾(k) The target state tracking model can be simplified formally by regarding the time-varying parameters of the target state tracking model and the measurement model; (2) suppose that alpha has been obtained⁽¹⁾,α⁽²⁾,…,α^(i-1)At time k and alpha⁽ⁱ⁾,α⁽ⁱ⁺¹⁾,…,α^(r)X estimate at time k-1, is used to estimate α⁽ⁱ⁾(k) The state and measurement model of (a) may be changed to a linear model; (3) when alpha is obtained⁽¹⁾,α⁽²⁾,…,α^(r)After the estimated value of the time k, the original state and the measurement model for estimating x are approximately equivalently rewritten into a linear form of the state variable at the time, and finally, the state variable of the displacement and the speed is updated in real time by means of KF.

Drawings

FIG. 1 is a flow chart of the present design;

FIG. 2 is a first stage filter design;

FIG. 3 is a second stage filter design;

FIG. 4 is a third stage filter design;

FIG. 5 is the displacement estimate for case 1;

FIG. 6 is the velocity estimate for case 1;

fig. 7 is the estimation error for case 1;

FIG. 8 is the displacement estimate for case 2;

FIG. 9 is the velocity estimate for case 2;

fig. 10 is the estimation error of case 2;

FIG. 11 is the displacement estimate for case 3;

FIG. 12 is the velocity estimate for case 3;

fig. 13 is the estimation error of case 3.

Detailed Description

The invention comprises the following steps:

the general forms of a nonlinear target state tracking model and a measurement model are given in the step (1):

consider a class of nonlinear target state tracking models and measurement models consisting of products of linear functions and several separable basic multipliers.

Wherein x is_j(k) Is the value of the target state and,

is a certain multiplier, w_i(k) And v_i(k +1) is a gaussian white noise sequence with E { w (k) } ═ 0, E { w (k) w^T(j)}＝Q(k)δ_kj；E{v(k+1)}＝0，E{v(k+1)v^T(j+1)}＝R(k+1)δ_kj. When k is j, delta _kj1, otherwise, δ_kj＝0。a_ijAnd h_ijThe coefficients of the model and the measurement model are tracked for the target state.

And (2) taking a two-dimensional target state tracking model and a measurement model as examples, defining a basic function as hidden variable parameters of the target state tracking model and the measurement model, and simplifying a nonlinear model into a first-order product form of a state and the hidden variable parameters of each order:

for the sake of complexity of the problem, an exemplary description will be given by taking m-n-r-2 as an example.

Two state variables x₁、x₂Respectively representing the displacement and the speed of the target, and carrying out pseudo-linearization description on a target state tracking model and a measurement model by introducing hidden variable parameters to define:

wherein the content of the first and second substances,

is a hidden variable parameter relative to the original target state variable x (k).

Then the formula (1) can be obtained

x(k+1)＝A(α⁽¹⁾(k),α⁽²⁾(k))x(k)+w⁽⁰⁾(k) (4)

Wherein

According to the formulae (3) and (4), the formula (2) can be obtained

y(k+1)＝H(α⁽¹⁾(k+1),α⁽²⁾(k+1))x(k+1)+v(k+1) (5)

Wherein

Step (3) taking the hidden variable parameters as parameter variables, namely new variables of the target state tracking model and the measurement model, establishing a dynamic linear model among the hidden variable parameters, other hidden variable parameters and the target state variables, and further rewriting the measurement model into a first-order linear product form among the estimation value of the target state at the current moment and the variable values of the parameters:

wherein

In the formula (6), the l-th hidden variable parameter α^(l)(k +1) and the u-th hidden variable parameter alpha^(u)(k) The correlation matrix S between^(lu)(k) Middle parameter

The identification can be performed according to the input and output information of the original target state tracking model. But without any a priori information, it is set as follows

Therefore, a quasi-linear representation model of the target state tracking model given by equation (4) with respect to the original target state variables x (k) and each α given by equation (6) are combined^(l)(k +1) with respect to the original target state variable x (k) and other parameters α^(u)(k) The linear correlation model between the original target state variable x (k) and all the hidden variable parameters alpha is realized^(u)(k) Joint step linearization representation of (1).

Meanwhile, joint step-by-step linearized expression of the target state variables and all hidden variable parameters is given based on the expressions (4) and (6), and a linearized expression model only aiming at the original target state variables is given by the expression (5); for hidden variable parameter alpha^(l)(k +1), equation (8) can be equivalently rewritten with the original target state variable x (k +1) and other hidden variable function parameters α^(u)(k +1), u-1, 2 is a linearized representation model of the parameters of the observation matrix

y(k+1)＝H(α⁽¹⁾(k+1),α⁽²⁾(k+1))x(k+1)+v(k+1)

＝H(x(k+1),α⁽¹⁾(k+1))α⁽²⁾(k+1)+v(k+1)

＝H(x(k+1),α⁽²⁾(k+1))α⁽¹⁾(k +1) + v (k +1) (8), so that the joint step-by-step linear representation of the state variable to be estimated and all hidden variable parameters given by combining the formulas (4) and (6) also realizes the step-by-step observation linear representation of the corresponding state variable to be estimated and hidden variable function parameters.

And (4) designing a high-order extended Kalman filter group consisting of r +1 Kalman filtering by means of introducing a target state tracking model and a measurement model of hidden variable parameters, solving step by step and linearizing an original target state tracking model and the measurement model.

Suppose that the estimated values of the observation target state values y (1), y (2), …, y (k) and the times of equations (3) to (7) k have been obtained

Sum estimation error covariance matrix P_α(k|k)，P_x(k | k). Then the 3-phase step Kalman filter at time k → k +1 is designed to: sequentially obtaining estimated values of alpha (k +1) and x (k +1)

And

and corresponding estimation error covariance matrix P_α(k +1| k +1) and P_x(k+1|k+1)。

Step (4-1) regarding parameter α⁽¹⁾Kalman filter design of (k + 1):

assuming that the estimated value of the k time is known

And corresponding estimation error covariance matrix

And

then with the parameter α⁽¹⁾The linear state model and the linearized observation model with (k +1) as the state variable are respectively

Wherein

The goal of the first stage is: under the condition of known observation target state value y (k +1), designing and solving an implicit variable parameter alpha⁽¹⁾(k +1) state estimation value

Sum estimation error covariance matrix

(1) Designing hidden variable parameter alpha⁽¹⁾(k +1) Kalman filter

(2) Hidden variable parameter alpha⁽¹⁾(k +1) one-step predictive estimate

Prediction estimation error

And a prediction estimation error covariance matrix

(3) First stage predicted measured values containing parametric variables

And predicted estimation error

(4) Computing a gain matrix for a first stage

(5) Computing an estimation error covariance matrix for a first stage

Step (4-2) regarding parameter α⁽²⁾Kalman filter design of (k + 1):

estimate of known k +1 time

And an estimate of time k

And estimating the error covariance matrix

And P_x(k | k), then for parameter α⁽²⁾The state of (k +1) and the observation model are linearized as follows

Wherein

The goals of the second stage are: under the condition of known observation target state value y (k +1), designing and solving an implicit variable parameter alpha⁽²⁾(k +1) state estimation value

Sum estimation error covariance matrix

(1) Designing hidden variable parameter alpha⁽²⁾(k +1) Kalman filter

(2) Hidden variable parameter alpha⁽²⁾(k +1) State prediction value

Prediction estimation error

And a prediction estimation error covariance matrix

(3) Second stage predictive measurements of parametric variables

And predicted estimation error

(4) Computing a gain matrix for the second stage

(5) Calculating the covariance matrix of the estimation error in the second stage

Step (4-3) Kalman filter design for the target state variable x (k + 1):

assuming known estimates

And

and estimating the error covariance matrix

And P_x(k | k), the state of the target state variable x (k +1) and the linearized form of the observation model

x(k+1)＝A(α⁽¹⁾(k),α⁽²⁾(k))x(k)+w⁽⁰⁾(k) (29)

y(k+1)＝H_x(α⁽¹⁾(k+1|k+1),α⁽²⁾(k+1|k+1))x(k+1)+v(k+1) (30)

Wherein

The targets of the third stage are: under the condition of knowing the observed target state value y (k +1), the design obtains the estimation value of the target state variable x (k +1)

Sum estimation error covariance matrix P_x(k+1|k+1)。

(1) Kalman filter for designing target state variable x (k +1)

(2) Target state prediction value

Prediction error

And prediction error covariance matrix P_x(k+1|k)

(3) Third stage predictive measurements of parametric variables

And predicted estimation error

(4) Calculating the gain matrix K of the third stage_x(k+1)

(5) Calculating the estimation error covariance matrix P of the third stage_x(k+1|k+1)

In order to verify the effectiveness of the method provided by the invention, three target state tracking cases are used for carrying out a comparative simulation experiment. In which two state variables x_1,k、x_2,kRespectively representing the displacement and the speed of the object at the k moment; the state equation in the nonlinear system of case 1 is strongly nonlinear, the measurement equation is linear, the state equation and the measurement equation in the nonlinear system of case 2 are strongly nonlinear, and the state equation and the measurement equation in the nonlinear system of case 3 are strongly nonlinear. Compared with case 1, the state model in case 2 increases the number of basic multipliers; compared with case 1, case 3 not only increases the number of the multiplicative latent variable parameters, but also increases the nonlinear complexity of the motion system model.

Case 1 considers a target state tracking system in which the state equation is strongly nonlinear and the measurement equation is linear.

The target state variables were estimated using EKF and the proposed MEKF, comparing displacement and velocity estimates and displacement and velocity estimation errors. The accuracy of EKF and MEKF are respectively calculated and compared.

Case 2 considers a target state tracking system where both the state equation and the measurement equation are strongly non-linear.

The target state variables were estimated using EKF and the proposed MEKF, comparing displacement and velocity estimates and displacement and velocity estimation errors. The accuracy of EKF and MEKF are respectively calculated and compared.

Case 3 considers that both the state equation and the measurement equation are strongly nonlinear target state tracking systems.

The target state variables were estimated using EKF and the proposed MEKF, comparing displacement and velocity estimates and displacement and velocity estimation errors. The accuracy of EKF and MEKF are respectively calculated and compared.

The above embodiments are further described with reference to the accompanying drawings.

As shown in fig. 1, fig. 2, fig. 3, and fig. 4, the present invention proposes a novel step-by-step linearized high-order extended kalman filter designed for a strong nonlinear target tracking model composed of a linear function and a product of several separable basis functions.

Case 1:

consider the following non-linear target state tracking system: the equation of state is strongly nonlinear, and the measurement equation is linear

Wherein the process noise and the measurement noise have the following characteristics w_1,k～N(0,0.1)，w_2,k～N(0,0.1)，v_kN (0, 0.1). FIGS. 5 and 6 show the displacement variable x under two filtering methods, respectively₁And the velocity variable x₂FIG. 7 is an estimation error diagram, and Table 1 shows the estimated displacement variable x in two ways₁And estimating the velocity variable x₂The mean square error of (d).

Table 1.Error comparison between MEKF and EKF

Filter	MSE of x1	MSE of x2	MSE
				EKF	0.0155	0.0230	0.0192
MEKF	0.0143	0.0123	0.0133
				Improved	7.74％	46.5％	30.7％

As can be seen from the simulation of fig. 5, 6 and 7 and table 1, the proposed MEKF algorithm allows the displacement variable x to be estimated, compared to EKF₁And the velocity variable x₂The accuracy rates of the method are respectively improved by 7.74 percent and 46.5 percent, the state tracking level is integrally improved by 30.7 percent, and the effectiveness of the method is verified.

Case 2:

consider the following non-linear target state tracking system: both the state and the measurement equation are strongly non-linear

Wherein the process noise and the measurement noise have the following characteristics w_1,k～N(0,0.1)，w_2,k～N(0,0.1)，v_kN (0, 0.1). FIGS. 8 and 9 show the displacement variable x under two filtering methods, respectively₁And the velocity variable x₂FIG. 10 is an estimation error map, and Table 2 is an estimated displacement variable x under two methods₁And estimating the velocity variable x₂The mean square error of (d).

Table 2.Error comparison between MEKF and EKF

Filter	MSE of x1	MSE of x2	MSE
				EKF	0.0391	0.0174	0.0283
MEKF	0.0361	0.0108	0.0234
				Improved	7.67％	37.9％	17.3％

Compared with case 1, the state model in case 2 increases the number of basic multipliers, and as can be seen from simulation fig. 8, 9 and 10 and table 2, compared with EKF, the proposed MEKF algorithm can make the displacement variable x to be estimated₁And the velocity variable x₂The accuracy rates of the method are respectively improved by 7.67% and 37.9%, the state tracking level is integrally improved by 17.3%, and the effectiveness of the method is further illustrated.

Case 3:

consider the following non-linear target state tracking system: both the state and the measurement equation are strongly non-linear

Wherein the process noise and the measurement noise have the following characteristics w_1,k～N(0,0.1)，w_2,k～N(0,0.1)，v_kN (0, 0.1). FIGS. 11 and 12 show the displacement variable x under two filtering methods, respectively₁And the velocity variable x₂FIG. 13 is an estimation error map, and Table 3 is an estimated displacement variable x under two methods₁And estimating the velocity variable x₂The mean square error of (d).

Table 3.Error comparison between MEKF and EKF

Filter	MSE of x1	MSE of x2	MSE
				EKF	0.0616	0.0163	0.0389
MEKF	0.0421	0.0099	0.0256
				Improved	31.66％	39.26％	34.19％

Compared with case 1, in case 3, the number of the multiplicative latent variable parameters is increased, and the nonlinear complexity of the target state tracking model and the measurement model is increased. As can be seen from the simulation of fig. 11, 12 and 13 and table 3, the proposed MEKF algorithm allows the displacement variable x to be estimated, compared to EKF₁And the velocity variable x₂The accuracy of the method is respectively improved by 31.66 percent and 39.26 percent, and the state tracking level is integrally improved by 34.19 percent, thereby illustrating the effectiveness and the adaptability of the method.

Claims (1)

1. The strong nonlinear system Kalman filter design method based on target state tracking is characterized by comprising the following steps:

the method comprises the following steps of (1) providing a nonlinear target state tracking model and a measurement model:

wherein x is_j(k) Is the value of the target state and,

is a multiplier, w_i(k) And v_i(k +1) is a gaussian white noise sequence with E { w (k) } ═ 0, E { w (k) w^T(j)}＝Q(k)δ_kj；E{v(k+1)}＝0，E{v(k+1)v^T(j+1)}＝R(k+1)δ_kj(ii) a When k is j, delta_kj1, otherwise, δ_kj＝0；a_ijAnd h_ijRespectively are coefficients of a target state tracking model and a measurement model;

and (2) taking a two-dimensional target state tracking model and a measurement model as examples, defining a basic function as hidden variable parameters of the target state tracking model and the measurement model, and simplifying a nonlinear target state tracking model into a first-order product form of a state and the hidden variable parameters of each order:

for example, two state variables x, with m ═ n ═ r ═ 2₁、x₂Respectively representing the displacement and the speed of the target, carrying out pseudo-linearization description on a target state tracking model and a measurement model by introducing hidden variable parameters, and defining:

wherein the content of the first and second substances,

is a hidden variable parameter relative to the original target state variable x (k);

step (3) taking the hidden variable parameters as parameter variables, namely new variables of the target state tracking model and the measurement model, establishing dynamic linear models among the hidden variables, other hidden variables and the target state variables, and further rewriting the measurement model into a first-order linear product form between the target state estimation value at the current moment and the variable values of the parameters;

combining a given quasi-linear representation model of the target state model with respect to the original target state variables x (k) and a given respective alpha^(l)(k +1) with respect to the original target state variable x (k) and other parameters α^(u)(k) The linear correlation model between the original target state variable x (k) and all the hidden variable function parameters alpha is realized^(u)(k) Joint step-wise linearization representation of (1);

based on the joint step-by-step linearization representation of the target state variable and all hidden variable parameters, giving out a linearization representation model only aiming at the original target state variable;

implicit variable parameter alpha for introduction^(l)(k +1), equivalently rewritten with the original target state variable x (k +1) and other hidden variable function parameters α^(u)(k +1), u ═ 1,2 is a linearized representation model of the parameters of the observation matrix; therefore, the joint step-by-step linear representation of the state variable of the target to be estimated and all the hidden variable parameters is combined, and the step-by-step observation linear representation of the corresponding target state to be estimated and the hidden variable function parameters is realized;

designing a high-order extended Kalman filter group consisting of r +1 Kalman filtering by means of introducing a target state tracking model and a measurement model of hidden variable parameters, solving step by step and linearizing an original target state tracking model and the measurement model;

suppose that the observed target state values y (1), y (2), …, y (k) and the estimated values of the target state tracking model and the measurement model at the moment k have been obtained

Sum estimation error covariance matrix P_α(k|k)，P_x(k | k); then the 3-phase step Kalman filter at time k → k +1 is designed to: sequentially obtaining estimated values of alpha (k +1) and x (k +1)

And

and corresponding estimation error covariance matrix P_α(k +1| k +1) and P_x(k+1|k+1)；

Step (4-1) regarding parameter α⁽¹⁾Kalman filter design of (k + 1):

assuming that the estimated value of the k time is known

And corresponding estimation error covariance matrix

And P_x(k|k)；

The design of the first stage is as follows: under the condition of known observation target state value y (k +1), designing and solving a hidden parameter variable alpha⁽¹⁾(k +1) state estimation value

Sum estimation error covariance matrix

(1) Designing hidden variable parameter alpha⁽¹⁾(k +1) a Kalman filter;

(2) hidden variable parameter alpha⁽¹⁾(k +1) predicting an estimated value, a predicted estimation error and a predicted estimation error covariance matrix in one step;

(3) the first stage comprises the predicted measured value and the predicted estimation error of the parameter variable;

(4) calculating a gain matrix of the first stage;

(5) calculating a first-stage estimation error covariance matrix;

step (4-2) regarding parameter α⁽²⁾Kalman filter design of (k + 1):

estimate of known k +1 time

And an estimate of time k

And estimating the error covariance matrix

And P_x(k|k)；

The design of the second stage is as follows: under the condition of known observation target state value y (k +1), designing and solving a hidden parameter variable alpha⁽²⁾(k +1) state estimation value

Sum estimation error covariance matrix

(1) Designing hidden variable parameter alpha⁽²⁾(k +1) a Kalman filter;

(2) hidden variable parameter alpha⁽²⁾(k +1) a state prediction value, a prediction estimation error, and a prediction estimation error covariance matrix;

(3) the second stage comprises a predicted measured value and a predicted estimation error of the parameter variable;

(4) calculating a gain matrix of the second stage;

(5) calculating an estimation error covariance matrix of the second stage;

step (4-3) Kalman filter design for the target state variable x (k + 1):

assuming known estimates

And

and estimating the error covariance matrix

And P_x(k|k)；

The design of the third stage is as follows: under the condition of knowing the observed target state value y (k +1), the design obtains the target state estimation value of the target state variable x (k +1)

Sum estimation error covariance matrix P_x(k+1|k+1)；

(1) Designing a Kalman filter of a target state variable x (k + 1);

(2) a target state prediction value, a prediction error and a prediction error covariance matrix;

(3) the third stage comprises a predicted measured value and a predicted estimation error of the parameter variable;

(4) calculating a gain matrix of the third stage;

(5) and calculating an estimation error covariance matrix of the third stage.

Images