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

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

Info

Publication number
CN112764345B
CN112764345B CN202011513880.4A CN202011513880A CN112764345B CN 112764345 B CN112764345 B CN 112764345B CN 202011513880 A CN202011513880 A CN 202011513880A CN 112764345 B CN112764345 B CN 112764345B
Authority
CN
China
Prior art keywords
target state
variable
model
hidden
parameter
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011513880.4A
Other languages
Chinese (zh)
Other versions
CN112764345A (en
Inventor
王金诚
孙晓辉
文成林
袁洢苒
徐晓滨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Dianzi University
Original Assignee
Hangzhou Dianzi University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Dianzi University filed Critical Hangzhou Dianzi University
Priority to CN202011513880.4A priority Critical patent/CN112764345B/en
Publication of CN112764345A publication Critical patent/CN112764345A/en
Application granted granted Critical
Publication of CN112764345B publication Critical patent/CN112764345B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)
  • Radar Systems Or Details Thereof (AREA)

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, but also has good characteristic of real-time recursion, thus being especially suitable for running on computer, and rapidly getting wide popularization and application 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 improvement, similar in concept to Taylor's 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 (0) (x)*f (1) (x)*f (2) (x)*…*f (r) (x) To simulate a designAnd obtaining the 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:
Figure BDA0002847131410000031
Figure BDA0002847131410000032
wherein x is j (k) Is the value of the target state and,
Figure BDA0002847131410000033
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 kj 1, otherwise, δ kj ＝0。a ij And h ij The 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 1 、x 2 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:
Figure BDA0002847131410000034
wherein,
Figure BDA0002847131410000035
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
Figure BDA0002847131410000041
And estimation error coordinationVariance 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)
Figure BDA0002847131410000042
And
Figure BDA0002847131410000043
and corresponding estimation error covariance matrix P α (k +1| k +1) and P x (k+1|k+1)。
Step (4-1) regarding parameter α (1) Kalman filter design of (k + 1):
assuming that the estimated value of the k time is known
Figure BDA0002847131410000044
And corresponding estimation error covariance matrix
Figure BDA0002847131410000045
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 (1) (k +1) state estimation value
Figure BDA0002847131410000046
Sum estimation error covariance matrix
Figure BDA0002847131410000047
(1) Designing hidden variable parameter alpha (1) (k +1) a Kalman filter;
(2) hidden variable parameter alpha (1) (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 α (2) Kalman filter design of (k + 1):
estimate of known k +1 time
Figure BDA0002847131410000048
And an estimate of time k
Figure BDA0002847131410000049
Figure BDA0002847131410000051
And estimating the error covariance matrix
Figure BDA0002847131410000052
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 (2) (k +1) state estimation value
Figure BDA0002847131410000053
Sum estimation error covariance matrix
Figure BDA0002847131410000054
(1) Designing hidden variable parameter alpha (2) (k +1) a Kalman filter;
(2) hidden variable parameter alpha (2) (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
Figure BDA0002847131410000055
And
Figure BDA0002847131410000056
and estimating the error covariance matrix
Figure BDA0002847131410000057
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)
Figure BDA0002847131410000058
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 (i) (x) Defined as an implicit variable parameter of the system, alpha (i) (k)：＝f (i) (x (k)), i ═ 0,1, …, r, and if α is further substituted (i) (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 (1) ,α (2) ,…,α (i-1) At time k and alpha (i) ,α (i+1) ,…,α (r) X estimate at time k-1, is used to estimate α (i) (k) The state and measurement model of (a) may be changed to a linear model; (3) when alpha is obtained (1) ,α (2) ,…,α (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.
Figure BDA0002847131410000061
Figure BDA0002847131410000062
Wherein x is j (k) Is the value of the target state and,
Figure BDA0002847131410000063
is a certain multiplier, w i (k) And v i (k +1) is a gaussian white noise sequence and has 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 kj 1, otherwise, δ kj ＝0。a ij And h ij The coefficients of the model and the measurement model are tracked for the target state.
Step (2) takes a two-dimensional target state tracking model and a measurement model as examples, defines a basic function as an implicit variable parameter of the target state tracking model and the measurement model, and simplifies a nonlinear model into a first-order product form of a state and the implicit 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 1 、x 2 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:
Figure BDA0002847131410000071
wherein,
Figure BDA0002847131410000072
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(α (1) (k),α (2) (k))x(k)+w (0) (k) (4)
Wherein
Figure BDA0002847131410000073
Figure BDA0002847131410000074
According to the formulae (3) and (4), the formula (2) can be obtained
y(k+1)＝H(α (1) (k+1),α (2) (k+1))x(k+1)+v(k+1) (5)
Wherein
Figure BDA0002847131410000075
Figure BDA0002847131410000076
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:
Figure BDA0002847131410000077
Figure BDA0002847131410000081
wherein
Figure BDA0002847131410000082
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
Figure BDA0002847131410000083
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
Figure BDA0002847131410000084
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(α (1) (k+1),α (2) (k+1))x(k+1)+v(k+1)
＝H(x(k+1),α (1) (k+1))α (2) (k+1)+v(k+1)
＝H(x(k+1),α (2) (k+1))α (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
Figure BDA0002847131410000085
Sum estimation error covariance matrix P α (k|k)，P x (k | k). Then at time k → k +1The 3-stage step Kalman filter is designed as follows: sequentially obtaining estimated values of alpha (k +1) and x (k +1)
Figure BDA0002847131410000091
And
Figure BDA0002847131410000092
and corresponding estimation error covariance matrix P α (k +1| k +1) and P x (k+1|k+1)。
Step (4-1) regarding parameter α (1) Kalman filter design of (k + 1):
assuming that the estimated value of the k time is known
Figure BDA0002847131410000093
And corresponding estimation error covariance matrix
Figure BDA0002847131410000094
And
Figure BDA0002847131410000095
then with the parameter α (1) The linear state model and the linearized observation model with (k +1) as the state variable are respectively
Figure BDA0002847131410000096
Figure BDA0002847131410000097
Wherein
Figure BDA0002847131410000098
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 (1) (k +1) state estimation value
Figure BDA0002847131410000099
Sum estimation error covariance matrix
Figure BDA00028471314100000910
(1) Designing hidden variable parameter alpha (1) (k +1) Kalman filter
Figure BDA00028471314100000911
(2) Hidden variable parameter alpha (1) (k +1) one-step predictive estimate
Figure BDA00028471314100000912
Prediction estimation error
Figure BDA00028471314100000913
And a prediction estimation error covariance matrix
Figure BDA00028471314100000914
Figure BDA00028471314100000915
Figure BDA00028471314100000916
Figure BDA00028471314100000917
(3) First stage predicted measured values containing parametric variables
Figure BDA00028471314100000918
And predicted estimation error
Figure BDA00028471314100000919
Figure BDA0002847131410000101
Figure BDA0002847131410000102
(4) Computing a gain matrix for a first stage
Figure BDA0002847131410000103
Figure BDA0002847131410000104
(5) Computing an estimation error covariance matrix for a first stage
Figure BDA0002847131410000105
Figure BDA0002847131410000106
Step (4-2) regarding parameter α (2) Kalman filter design of (k + 1):
estimate of known k +1 time
Figure BDA0002847131410000107
And an estimate of time k
Figure BDA0002847131410000108
Figure BDA0002847131410000109
And estimating the error covariance matrix
Figure BDA00028471314100001010
And P x (k | k), then for parameter α (2) The state of (k +1) and the observation model are linearized as follows
Figure BDA00028471314100001011
Figure BDA00028471314100001012
Wherein
Figure BDA00028471314100001013
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 (2) (k +1) state estimation value
Figure BDA00028471314100001014
Sum estimation error covariance matrix
Figure BDA0002847131410000111
(1) Designing hidden variable parameter alpha (2) (k +1) Kalman Filter
Figure BDA0002847131410000112
(2) Hidden variable parameter alpha (2) (k +1) State prediction value
Figure BDA0002847131410000113
Prediction estimation error
Figure BDA0002847131410000114
And a prediction estimation error covariance matrix
Figure BDA0002847131410000115
Figure BDA0002847131410000116
Figure BDA0002847131410000117
Figure BDA0002847131410000118
(3) Second stage predictive measurements of parametric variables
Figure BDA0002847131410000119
And predicted estimation error
Figure BDA00028471314100001110
Figure BDA00028471314100001111
Figure BDA00028471314100001112
(4) Computing a gain matrix for the second stage
Figure BDA00028471314100001113
Figure BDA00028471314100001114
(5) Calculating the covariance matrix of the estimation error in the second stage
Figure BDA00028471314100001115
Figure BDA0002847131410000121
Step (4-3) Kalman filter design on the target state variable x (k + 1):
assuming known estimates
Figure BDA0002847131410000122
And
Figure BDA0002847131410000123
and estimating the error covariance matrix
Figure BDA0002847131410000124
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(α (1) (k),α (2) (k))x(k)+w (0) (k) (29)
y(k+1)＝H x (α (1) (k+1|k+1),α (2) (k+1|k+1))x(k+1)+v(k+1) (30)
Wherein
Figure BDA0002847131410000125
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)
Figure BDA0002847131410000126
Sum estimation error covariance matrix P x (k+1|k+1)。
(1) Kalman filter for designing target state variable x (k +1)
Figure BDA0002847131410000127
(2) Target state prediction value
Figure BDA0002847131410000128
Prediction error
Figure BDA0002847131410000129
And prediction error covariance matrix P x (k+1|k)
Figure BDA00028471314100001210
Figure BDA00028471314100001211
Figure BDA00028471314100001212
(3) Third stage predictive measurements of parametric variables
Figure BDA00028471314100001213
And predicted estimation error
Figure BDA0002847131410000131
(4) Calculating the gain matrix K of the third stage x (k+1)
Figure BDA0002847131410000132
(5) Calculating the estimation error covariance matrix P of the third stage x (k+1|k+1)
Figure BDA0002847131410000133
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,k Respectively representing the displacement and the speed of the object at the moment k; 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 in the nonlinear system of case 3 is strongly nonlinearBoth the equation and the measurement equation are strongly non-linear. 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.
Figure BDA0002847131410000134
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.
Figure BDA0002847131410000135
The target state variables were estimated using the EKF and proposed MEKF, and the displacement and velocity estimates and the displacement and velocity estimation errors were compared. 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.
Figure BDA0002847131410000141
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
Figure BDA0002847131410000142
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 k N (0, 0.1). FIGS. 5 and 6 show the displacement variable x under two filtering methods, respectively 1 And the velocity variable x 2 FIG. 7 is an estimation error diagram, and Table 1 shows the estimated displacement variable x in two ways 1 And estimating the velocity variable x 2 The mean square error of (d).
Table 1.Error comparison between MEKF and EKF
Filter MSE of x 1 MSE of x 2 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 1 And the velocity variable x 2 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
Figure BDA0002847131410000151
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 k N (0, 0.1). FIGS. 8 and 9 show the displacement variable x under two filtering methods, respectively 1 And the velocity variable x 2 FIG. 10 is an estimation error map, and Table 2 is an estimated displacement variable x under two methods 1 And estimating the velocity variable x 2 The mean square error of (d).
Table 2.Error comparison between MEKF and EKF
Filter MSE of x 1 MSE of x 2 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 1 And the velocity variable x 2 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
Figure BDA0002847131410000152
Wherein process noise and measurementThe noise has the following characteristics w 1,k ～N(0,0.1)，w 2,k ～N(0,0.1)，v k N (0, 0.1). FIGS. 11 and 12 show the displacement variable x under two filtering methods, respectively 1 And the velocity variable x 2 FIG. 13 is an estimation error map, and Table 3 is an estimated displacement variable x under two methods 1 And estimating the velocity variable x 2 The mean square error of (d).
Table 3.Error comparison between MEKF and EKF
Filter MSE of x 1 MSE of x 2 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 1 And the velocity variable x 2 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:
Figure FDA0003572497150000011
Figure FDA0003572497150000012
wherein x is j (k) Is the target state value, f j (l) (x (k)) 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 kj 1, otherwise, δ kj ＝0；a ij And h ij Respectively are coefficients of a target state tracking model and a measurement model;
and (2) defining basic functions as hidden variable parameters of the target state tracking model and the measurement model for the two-dimensional target state tracking model and the measurement model, and simplifying the nonlinear target state tracking model into a first-order product form of the state and the hidden variable parameters of each order:
let m-n-r-2, two state variables x 1 、x 2 Respectively represent eyesThe target displacement and speed are subjected to pseudo-linear description on a target state tracking model and a measurement model by introducing hidden variable parameters, and are defined as follows:
Figure FDA0003572497150000013
wherein,
Figure FDA0003572497150000014
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
Figure FDA0003572497150000021
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)
Figure FDA0003572497150000022
And
Figure FDA0003572497150000023
and corresponding estimation error covariance matrix P α (k +1| k +1) and P x (k+1|k+1)；
Step (4-1) regarding parameter α (1) Kalman filter design of (k + 1):
assuming that the estimated value of the k time is known
Figure FDA0003572497150000024
And corresponding estimation error covariance matrix
Figure FDA0003572497150000025
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 (1) (k +1) state estimation value
Figure FDA0003572497150000026
Sum estimation error covariance matrix
Figure FDA0003572497150000027
(1) Designing hidden variable parameter alpha (1) (k +1) a Kalman filter;
(2) hidden variable parameter alpha (1) (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 α (2) Kalman filter design of (k + 1):
estimate of known k +1 time
Figure FDA0003572497150000031
And an estimate of time k
Figure FDA0003572497150000032
Figure FDA0003572497150000033
And estimating the error covariance matrix
Figure FDA0003572497150000034
Figure FDA0003572497150000035
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 (2) (k +1) state estimation value
Figure FDA0003572497150000036
Sum estimation error covariance matrix
Figure FDA0003572497150000037
(1) Designing hidden variable parameter alpha (2) (k +1) ofA Kalman filter;
(2) hidden variable parameter alpha (2) (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
Figure FDA0003572497150000038
And
Figure FDA0003572497150000039
and estimating the error covariance matrix
Figure FDA00035724971500000310
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)
Figure FDA00035724971500000311
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.
CN202011513880.4A 2020-12-21 2020-12-21 Strong nonlinear system Kalman filter design method based on target state tracking Active CN112764345B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011513880.4A CN112764345B (en) 2020-12-21 2020-12-21 Strong nonlinear system Kalman filter design method based on target state tracking

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011513880.4A CN112764345B (en) 2020-12-21 2020-12-21 Strong nonlinear system Kalman filter design method based on target state tracking

Publications (2)

Publication Number Publication Date
CN112764345A CN112764345A (en) 2021-05-07
CN112764345B true CN112764345B (en) 2022-08-02

Family

ID=75694517

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011513880.4A Active CN112764345B (en) 2020-12-21 2020-12-21 Strong nonlinear system Kalman filter design method based on target state tracking

Country Status (1)

Country Link
CN (1) CN112764345B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113630106A (en) * 2021-08-02 2021-11-09 杭州电子科技大学 High-order extended Kalman filter design method based on strong tracking filtering
CN116192095B (en) * 2023-05-04 2023-07-07 广东石油化工学院 Real-time filtering method for dynamic system additive interference and state estimation

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107290742B (en) * 2017-06-20 2019-06-11 武汉理工大学 Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system
CN108599737B (en) * 2018-04-10 2021-11-23 西北工业大学 Design method of nonlinear Kalman filter of variational Bayes
CN109829938B (en) * 2019-01-28 2020-12-08 杭州电子科技大学 Adaptive fault-tolerant volume Kalman filtering method applied to target tracking
CN111693984B (en) * 2020-05-29 2023-04-07 中国计量大学 Improved EKF-UKF moving target tracking method

Also Published As

Publication number Publication date
CN112764345A (en) 2021-05-07

Similar Documents

Publication Publication Date Title
CN112764345B (en) Strong nonlinear system Kalman filter design method based on target state tracking
Gwiazda et al. Structured population equations in metric spaces
CN108304612B (en) Iterative square root CKF (tracking of target) automobile radar target tracking method based on noise compensation
Jones et al. Recursive algorithm for computing the frequency response of a class of non-linear difference equation models
CN104992164B (en) A kind of dynamic oscillation signal model parameters discrimination method
CN104462015B (en) Process the fractional order linear discrete system state updating method of non-gaussian L é vy noises
CN112765921A (en) High-order extended Kalman filter design method of strong nonlinear dynamic system
CN105354860A (en) Box particle filtering based extension target CBMeMBer tracking method
CN107292410A (en) tunnel deformation prediction method and device
WO2020052213A1 (en) Iterative cubature unscented kalman filtering method
Schoukens et al. Identification of hammerstein-wiener systems
CN113032988A (en) High-order extended Kalman filter design method based on maximum correlation entropy
CN113630106A (en) High-order extended Kalman filter design method based on strong tracking filtering
Moradi et al. Parameters estimation of linear frequency modulated signal using Kalman filter and its extended versions
CN109687844A (en) A kind of intelligent maneuver method for tracking target
CN108228959A (en) Using the method for Random censorship estimating system virtual condition and using its wave filter
CN116595897A (en) Nonlinear dynamic system state estimation method and device based on message passing
CN115079573A (en) High-order expansion strong tracking filter of nonlinear system
CN116667815A (en) Filtering method of non-Gaussian multiplicative noise system based on maximum entropy Gaussian sum
Ichiba et al. Efficient estimation of one-dimensional diffusion first passage time densities via Monte Carlo simulation
CN116522055A (en) Parameter self-tuning Kalman filter design method based on expansion state
CN115685128A (en) Radar target tracking algorithm and electronic equipment under maneuvering target scene
CN114236524A (en) Doppler radar sequential smooth variable structure filtering method and device
Akansha Exploring Chebyshev Polynomial Approximations: Error Estimates for Functions of Bounded Variation
CN111274752B (en) Power amplifier behavior modeling method based on two-stage open loop and binary function

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant