High-order extended Kalman filtering method for nonlinear system

Abstract

The invention discloses a high-order extended Kalman filtering method for a nonlinear system. The method comprises the following steps: S1, establishing a nonlinear system composed of a linear state model equation and a nonlinear measurement model equation; s2, establishing a calculation algorithm for solving the statistical characteristics of the Kronecker product type composite random variables; s3, analyzing the high-order characteristics of the state estimation error of the nonlinear system by using the algorithm established in the step S2; s4, combining the known measurement information with the high-order features of the errors, designing and optimizing extended Kalman filtering, and gradually estimating the next state in the nonlinear system; and S5, circulating the process of the step S4, estimating the state of the nonlinear system step by step until the state estimation of the set step number is completed, and obtaining the state estimation value of each step. According to the method, the estimation model in the filtering process is adaptively adjusted based on the high-order statistical characteristics of the system state estimation error and the known measurement information, so that the stability and the precision of the filter are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of Kalman filtering, and in particular relates to a high-order extended Kalman filtering method for nonlinear systems. Background Art

[0002] The extended Kalman filter (ERF) is commonly used for state estimation in nonlinear dynamic systems. However, when the system nonlinearity is strong, traditional EKFs may suffer from estimation bias, affecting filter performance. The EKF performs a first-order Taylor series expansion on the nonlinear state model function and measurement model function at the current estimation point, converting the nonlinear problem into a linear problem using the standard Kalman filter, and then performing filter design. It should be noted that the EKF uses a Gaussian distribution to approximate the posterior distribution of the estimated value, thus achieving good estimation performance when both system noise and measurement noise follow a Gaussian distribution. However, the EKF can only achieve a first-order linear approximation. Furthermore, for systems with strong nonlinearity and asymmetric distributions of estimated values, its filtering performance increases with increasing model nonlinearity, leading to increased roundoff error, which in turn degrades filter performance and can even lead to target tracking loss due to divergence in the filter estimation algorithm. Therefore, for strongly nonlinear systems, a filtering method that can improve state estimation accuracy is urgently needed to adapt to complex dynamic systems.

[0003] Patent publication number CN 119150557 A proposes a calculation method that leverages the statistical properties of higher-order characteristics of state prediction estimation errors in the Taylor expansion of a nonlinear measurement model, achieving higher-precision estimation results. However, this process utilizes a backward smoothing filter, requiring the knowledge of future measurement values. Then, based on the backward smoothing filter, the higher-order statistical properties of the state prediction estimation errors, higher than the second order, in the Taylor expansion of the nonlinear measurement model are gradually solved. This introduces the following new problems:

[0004] 1) The statistical characteristics of the high-order features contain future measurement information, which will greatly reduce the performance of the update phase.

[0005] 2) Since the errors in the statistical characteristics of high-order features include both process noise and measurement noise, as well as the errors in the statistical characteristics of low-order features, the correlation of these errors will also increase the complexity of the filter and reduce the accuracy of the filter.

[0006] 3) Most importantly, it destroys the recursive real-time performance that the Kalman filter is proud of. This is because a statistical characteristic calculation task is added to the filter design process, which uses future measurement information to smooth the high-order features that should have been completed in the prediction stage.

[0007] To this end, the present invention provides a new model for calculating high-order statistical information, and uses it to design a high-precision recursive real-time Kalman filter. Between the current moment and the future moment, that is, the prediction phase, the statistical characteristics of the high-order features in the nonlinear model expansion, namely the mean and variance, are calculated to form a state prediction estimator and a measurement prediction estimator; after the measurement value at the future moment arrives, the filter update process is immediately executed to calculate the optimal state estimate and the estimation error covariance matrix. Summary of the Invention

[0008] To overcome the problem of excessive deviation in the next-step state estimation using conventional extended Kalman filters in existing technologies for strongly nonlinear systems, the present invention provides an adaptive extended Kalman filter design method for strongly nonlinear dynamic systems. By combining a linear state model with a nonlinear measurement model, the method progressively identifies the high-order characteristics of the system state estimation error, thereby optimizing the filter performance. Based on the high-order statistical characteristics of the system state estimation error and known measurement information, the method adaptively adjusts the estimation model during the filtering process, thereby improving the filter's stability and accuracy.

[0009] The new method overcomes some of the shortcomings of the patent publication CN 119150557 A, namely noise correlation, non-real-time performance, and reduced performance during the update phase. The new method does not utilize future measurement information in the prediction phase, which does not reduce update performance and introduce noise correlation. High-order terms can be calculated before the actual future measurement arrives, thus preventing real-time performance degradation. A new Kalman filter with the same recursive real-time advantages as the traditional Kalman filter but with higher accuracy is designed. The method first calculates the predicted state estimate from the current moment; then calculates the statistical characteristics of the high-order terms in the Taylor expansion of the measurement model at the predicted estimate; and finally calculates the predicted state estimate for the future measurement. Upon arrival of the actual future measurement, the update process from the predicted state estimate to the optimal state estimate is immediately performed.

[0010] The present invention adopts the following technical solutions:

[0011] A high-order extended Kalman filter method for nonlinear systems includes the following steps:

[0012] S1, establish a system consisting of linear state model equations and nonlinear measurement model equations;

[0013] S2, establish a computational algorithm for solving the statistical properties of Kronecker product type composite random variables;

[0014] S3, using the algorithm established in step S2, analyzing the high-order characteristics of the system state estimation error to obtain the high-order statistical characteristics of the error;

[0015] S4, combining known measurement information with high-order features of the error, designing and optimizing an extended Kalman filter, and gradually estimating the next state in the nonlinear system;

[0016] S5, looping the process of step S4, gradually estimating the system state until the state estimation for a set number of steps is completed, and the state estimation value of each step is obtained.

[0017] Preferably, step S1 is as follows:

[0018] Assume that the estimated state value x(k+1) has the following relationship with the system's state vector x(k) and the observation vector y(k+1) of the state x(k+1):

[0019] x(k+1)=A(k)x(k)+w(k) (1)

[0020] y(k+1)=h(x(k+1))+v(k+1) (2)

[0021] where A(k) is the state transfer matrix, h(·) is a nonlinear measurement function with arbitrary-order continuous differentiability, x(k) is the n-dimensional state vector, w(k) is the n-dimensional state modeling error vector, y(k+1) is the m-dimensional measurement value vector, h(x(k+1)) is the m-dimensional measurement function vector, v(k+1) is the m-dimensional measurement modeling error vector, and k represents the kth moment of calculation.

[0022] Preferably, step S2 is as follows:

[0023] Suppose there is an n-dimensional vector x and an m-dimensional vector y:

[0024] x=[x1,…,x i ,…,x n ] T ,y=[y1,…,y j ,…,y m ] T (3)

[0025] Where T represents the transpose of the matrix;

[0026] x and y are statistically independent and have the following statistical characteristics:

[0027] x~N[0,P x ],y~N[0,P y ], Cov(x,y)=P xy (4)

[0028] Among them, N means that it obeys the normal distribution; P x represents the variance of the random variable x; P yRepresents the variance of random variable y; Cov(x,y) represents the variance of random variable x and random variable y; Pxy represents the specific numerical matrix corresponding to the variance of random variable x and random variable y;

[0029] Solution Statistical properties of:

[0030]

[0031] in, represents the Kronecker product; st represents arranging each element of the matrix into a vector according to the row number; E{·} represents finding the mean of the random variables in the brackets {·}; P represents the random variable variance;

[0032] make get:

[0033]

[0034] S21, convert Kronecker product into matrix multiplication;

[0035] Introduce an operation G:

[0036]

[0037] Among them, I y is the identity matrix with the same dimensions as y; x i represents the i-th element in vector x;

[0038] Use operation G to convert the Kronecker product into a matrix multiplication:

[0039]

[0040] Among them, := means defined as;

[0041] S22, P y Perform singular value decomposition and make further transformations:

[0042]

[0043] Among them, λ represents the matrix P y The singular value of the matrix P y Singular values ​​of ; ∑ represents summation;

[0044] S23, exchange Chinese symbol Position of elements on both sides:

[0045] Introducing an operation

[0046]

[0047] Among them, the matrix M- 1 represents the inverse matrix of matrix M;

[0048] Expand the matrix:

[0049]

[0050] Wherein, the ((j-1)×n+i)th row of the matrix M is equal to the ((i-1)*m+j)th row of the nm-dimensional identity matrix; i=1,…,n; j=1,…,m;

[0051] Using operations Do further conversion:

[0052]

[0053] S24, get Variance of:

[0054]

[0055] Preferably, step S3 includes the following steps:

[0056] S31, set formula (2) in The r-order Taylor expansion at is as follows:

[0057]

[0058] in, The initial value of is given according to the specific model. represents the system state estimation error The l-th order Kronecker product, H [l] (k+1) is h(x(k+1)) The l-order Jacobian matrix at , the superscript ^ represents the predicted value, and the superscript ~ represents the error between the predicted value and the true value; (k+1|k) represents the solution of the state value at the kth moment to the state value at the k+1th moment, the symbol | is followed by the condition, and the symbol before is the value to be calculated; k represents the kth moment;

[0059] S32, calculate the state prediction estimation error Statistical properties of:

[0060] According to the state prediction estimation error The definition of is:

[0061]

[0062] Find the expected mean:

[0063]

[0064] Then the covariance matrix is:

[0065]

[0066] in, is the variance of x(k), The initial value is given according to the specific model, := means defined as; The superscript [1,1] indicates that the covariance is the error of the first-order state prediction estimation error. Error with first-order state prediction estimate error The product of is averaged; mid-subscript Denotes that the covariance is the error of the state prediction estimation error Error with state prediction estimation error The product of is averaged;

[0067] Thus, we get Statistical characteristics:

[0068]

[0069] S33, will Find the expectation and get the estimated value of the second-order state prediction error of state x(k+1):

[0070]

[0071] in, is the Kronecker product, and st is to arrange each element of the matrix into a vector according to the row number;

[0072] S34, calculate the covariance matrix of the second-order state prediction estimation error:

[0073]

[0074] Then we get The solution formula is:

[0075]

[0076] S35, using the algorithm established in step S2 to obtain

[0077]

[0078] in, yes abbreviation of ;

[0079] S36, substitute formula (23) into formula (22) to obtain The solution formula is:

[0080]

[0081] S37, will Find the expectation and get the estimated value of the third-order state prediction error of state x(k+1):

[0082]

[0083] Among them, 0 represents a vector whose elements are all 0;

[0084] S38, using the algorithm established in step S2 to obtain the third-order state prediction estimation error covariance matrix

[0085] in, yes The abbreviation of yes abbreviation of;

[0086] S39, will Find the expectation and get the estimated value of the odd 2m+1 order state prediction error of state x(k+1): Since m+1 and m must have an odd number, that is:

[0087]

[0088] Among them, or means either;

[0089] Find the expected mean:

[0090]

[0091] S310, using the algorithm established in step S2 to calculate

[0092]

[0093] in, yes The abbreviation of yes abbreviation of ;

[0094] S311, will Find the expectation and get the estimated value of the even 2m+2 order state prediction error of state x(k+1):

[0095]

[0096] Among them, st means arranging each element of the matrix into a vector according to the row number;

[0097] S312, calculate the even 2m+2 order state prediction estimation error The covariance matrix of :

[0098]

[0099] Then we get The solution formula is:

[0100]

[0101] S313, calculate using the algorithm established in step S2

[0102]

[0103] Among them, P [m+1] for abbreviation of ;

[0104] Then we get The solution formula is:

[0105]

[0106] Among them, P [m+1] for abbreviation of ;

[0107] S314, substitute formula (32) into formula (31) to obtain The solution formula is:

[0108]

[0109] Among them, P [m+1] for abbreviation of .

[0110] Preferably, step S4 includes the following steps:

[0111] S41, use formula (1) to obtain the state prediction estimate of state x(k+1):

[0112]

[0113] S42, use formula (34) to calculate the state prediction estimation error value of state x(k+1):

[0114]

[0115] S43, use formula (1) and (35) to obtain the state prediction estimation error covariance matrix of state x(k+1):

[0116]

[0117] Where Q(k) is the variance of the state prediction error w(k) in formula (1);

[0118] S44, using formula (15) to obtain the r-order prediction estimate of the measurement y(k+1)

[0119]

[0120] S45, using formula (15) and (37) to calculate the r-order prediction estimation error value of measurement y(k+1)

[0121]

[0122] S46, using formula (38) to obtain the r-order prediction estimation error covariance matrix P of the measurement y(k+1) yy (k+1|k):

[0123]

[0124] Where R(k+1) is the variance of the measurement prediction error v(k+1) in formula (2); P yy The subscript yy in (k+1|k) indicates that the covariance is the r-order prediction error of the measurement y(k+1) The r-order prediction error with the measurement y(k+1) Take the mean of the product of ;

[0125] S47, use formula (35) and (38) to obtain the cross-covariance matrix P between the state and the measurement xy (k+1|k):

[0126]

[0127] Among them, P xy The subscript xy in (k+1|k) indicates that the covariance is the prediction error of state x(k+1) The r-order prediction error with the measurement y(k+1) Take the mean of the product of ;

[0128] S48, using the orthogonality between the state estimation error and the measurement value, the filter gain matrix K(k+1) is obtained:

[0129]

[0130] Where R(k+1) is the variance of the measurement prediction error v(k+1) in formula (2);

[0131] S49, using the value of the filter gain matrix K(k+1) and the state prediction estimate obtained by formula (34) to calculate the next state value:

[0132]

[0133] S410, using the state prediction estimation error covariance matrix P(k+1|k) and the filter gain matrix K(k+1), update the estimation error covariance matrix P(k+1|k+1):

[0134] P(k+1|k+1)=[IK(k+1)H(k+1)]P xx (k+1|k) (43)

[0135] Among them, P(k+1|k+1) is the next state I represents the identity matrix.

[0136] Preferably, step S5 is as follows:

[0137] Using formula (43) we get Substitute into formula (36) and (41) to get P(2|1) and K(2), and then use formula (34) to get Substitute into formula (42) to obtain the state prediction value Substituting into formula (43) we get By analogy, we get Until

[0138] Compared with the prior art, the present invention has the following significant technical effects:

[0139] The present invention adaptively adjusts the estimation model during the filtering process based on the high-order statistical characteristics of the system state estimation error and known measurement information, thereby improving the stability and accuracy of the filter. The present invention defines a new model for calculating the mean and variance of the high-order statistical characteristics of random variables. The present method overcomes several shortcomings of the patent publication CN119150557A, namely noise correlation, non-real-time performance, and reduced performance during the update phase. The present method does not utilize future measurement information in the prediction phase, thus avoiding performance degradation during the update phase and introducing no noise correlation. High-order terms can be calculated before the actual future measurement arrives, thus preventing real-time performance degradation. The present invention designs a novel Kalman filter that shares the same recursive real-time advantages of a traditional Kalman filter but with higher accuracy. The method first calculates a predicted state estimate from the current moment; then calculates the statistical characteristics of the high-order terms in the Taylor expansion of the measurement model at the predicted estimate; and finally calculates a predicted estimate for the future measurement. Upon arrival of the actual future measurement, the update process from the predicted state estimate to the optimal state estimate is immediately performed. BRIEF DESCRIPTION OF THE DRAWINGS

[0140] Figure 1 This is a flow chart of a high-order extended Kalman filtering method for nonlinear systems according to a preferred embodiment of the present invention;

[0141] Figure 2 This is the mean square error comparison chart of group A;

[0142] Figure 3 This is a comparison chart of the true value and filtered value of X1 in group A;

[0143] Figure 4 This is the mean square error comparison chart of group B;

[0144] Figure 5 This is a comparison chart of the true value and filtered value of X1 in group B;

[0145] Figure 6 This is the mean square error comparison chart of group C;

[0146] Figure 7 This is a comparison chart of the true value and filtered value of X1 in group C. DETAILED DESCRIPTION

[0147] The present invention is further described in detail below with reference to preferred embodiments.

[0148] like Figure 1 As shown, this embodiment discloses a high-order extended Kalman filtering method for nonlinear systems, which specifically includes the following steps:

[0149] S1, establish a system consisting of nonlinear state model equations and linear measurement model equations;

[0150] S2, establish a computational algorithm for solving the statistical properties of Kronecker product type composite random variables;

[0151] S3, using the algorithm established in step S2, analyzing the high-order characteristics of the system state estimation error to obtain the high-order statistical characteristics of the error;

[0152] S4, combining known measurement information with high-order features of the error, designing and optimizing an extended Kalman filter, and gradually estimating the next state in the nonlinear system;

[0153] S5, looping the process of step S4, gradually estimating the system state until the state estimation for a set number of steps is completed, and the state estimation value of each step is obtained.

[0154] In this embodiment, step S1 is specifically as follows:

[0155] Assume that the estimated state value x(k+1) has the following relationship with the system's state vector x(k) and the observation vector y(k+1) of the state x(k+1):

[0156] x(k+1)=A(k)x(k)+w(k) (1)

[0157] y(k+1)=h(x(k+1))+v(k+1) (2)

[0158] where A(k) is the state transfer matrix, h(·) is a nonlinear measurement function with arbitrary-order continuous differentiability; x(k) is the n-dimensional state vector, w(k) is the n-dimensional state modeling error vector; y(k+1) is the m-dimensional measurement value vector, h(x(k+1)) is the m-dimensional measurement function vector, and v(k+1) is the m-dimensional measurement modeling error vector.

[0159] In this embodiment, step S2 is specifically as follows:

[0160] Suppose there is an n-dimensional vector x and an m-dimensional vector y:

[0161] x=[x1,…,x i ,…,x n ] T ,y=[y1,…,y j ,…,y m ] T (3)

[0162] Where T represents the transpose of the matrix;

[0163] x and y are statistically independent and have the following statistical characteristics:

[0164] x~N[0,P x],y~N[0,P y ], Cov(x,y)=P xy (4)

[0165] Among them, N means that it obeys the normal distribution; P x represents the variance of the random variable x; P y represents the variance of the random variable y; Cov(x,y) represents the variance of the random variable x and the random variable y; P xy Represents the specific numerical matrix corresponding to the variance of random variables x and y;

[0166] Solution Statistical properties of:

[0167]

[0168] in, represents the Kronecker product; st represents arranging each element of the matrix into a vector according to the row number; E{·} represents finding the mean of the random variables in the brackets {·}; P represents the random variable variance;

[0169] make get:

[0170]

[0171] S21, convert Kronecker product into matrix multiplication;

[0172] Introduce an operation G:

[0173]

[0174] Among them, I y is the identity matrix with the same dimensions as y; x i represents the i-th element in vector x;

[0175] Use operation G to convert the Kronecker product into a matrix multiplication:

[0176]

[0177] Among them, := means defined as;

[0178] S22, P y Perform singular value decomposition and make further transformations:

[0179]

[0180] Among them, λ represents the matrix P y The singular value of the matrix P y Singular values ​​of ; ∑ represents summation;

[0181] S23, exchange Chinese symbol Position of elements on both sides:

[0182] Introducing an operation

[0183]

[0184] Among them, the matrix M- 1 represents the inverse matrix of matrix M;

[0185] Expand the matrix:

[0186]

[0187] The ((j-1)×n+i)th row of the matrix M is equal to the ((i-1)*m+j)th row of the nm-dimensional identity matrix; i = 1,…,n; j = 1,…,m; using the operation Do further conversion:

[0188]

[0189] S24, get Variance of:

[0190]

[0191] In this embodiment, step S3 includes the following steps:

[0192] S31, set formula (2) in The r-order Taylor expansion at is as follows:

[0193]

[0194] in, The initial value of is given according to the specific model. represents the system state estimation error The l-th order Kronecker product, H [l] (k+1) is h(x(k+1)) The l-order Jacobian matrix at , the superscript ^ represents the predicted value, and the superscript ~ represents the error between the predicted value and the true value; (k+1|k) represents the solution of the state value at the kth moment to the state value at the k+1th moment, the symbol | is followed by the condition, and the symbol before is the value to be calculated; k represents the kth moment;

[0195] S32, calculate the state prediction estimation error Statistical properties of:

[0196] According to the state prediction estimation error The definition of is:

[0197]

[0198] Find the expected mean:

[0199]

[0200] Then the covariance matrix is:

[0201]

[0202] in, is the variance of x(k), The initial value is given according to the specific model, := means defined as; The superscript [1,1] indicates that the covariance is the error of the first-order state prediction estimation error. Error with first-order state prediction estimate error The product of is averaged; mid-subscript Denotes that the covariance is the error of the state prediction estimation error Error with state prediction estimation error The product of is averaged;

[0203] Thus, we get Statistical characteristics:

[0204]

[0205] S33, will Find the expectation and get the estimated value of the second-order state prediction error of state x(k+1):

[0206]

[0207] in, is the Kronecker product, and st is to arrange each element of the matrix into a vector according to the row number;

[0208] S34, calculate the covariance matrix of the second-order state prediction estimation error:

[0209]

[0210] Then we get The solution formula is:

[0211]

[0212] S35, using the algorithm established in step S2 to obtain

[0213]

[0214] in, yes abbreviation of ;

[0215] S36, substitute formula (23) into formula (22) to obtain The solution formula is:

[0216]

[0217] S37, will Find the expectation and get the estimated value of the third-order state prediction error of state x(k+1):

[0218]

[0219] Among them, 0 represents a vector whose elements are all 0;

[0220] S38, using the algorithm established in step S2 to obtain the third-order state prediction estimation error covariance matrix

[0221] in, yes The abbreviation of yes abbreviation of ;

[0222] S39, will Find the expectation and get the estimated value of the odd 2m+1 order state prediction error of state x(k+1): Since m+1 and m must have an odd number, that is:

[0223]

[0224] Among them, or means either;

[0225] Find the expected mean:

[0226]

[0227] S310, using the algorithm established in step S2 to calculate

[0228]

[0229] in, yes The abbreviation of yes abbreviation of ;

[0230] S311, will Find the expectation and get the estimated value of the even 2m+2 order state prediction error of state x(k+1):

[0231]

[0232] Among them, st means arranging each element of the matrix into a vector according to the row number;

[0233] S312, calculate the even 2m+2 order state prediction estimation error The covariance matrix of :

[0234]

[0235] Then we get The solution formula is:

[0236]

[0237] S313, calculate using the algorithm established in step S2

[0238]

[0239] Among them, P [m+1] for abbreviation of ;

[0240] Then we get The solution formula is:

[0241]

[0242] Among them, P [m+1] for abbreviation of ;

[0243] S314, substitute formula (32) into formula (31) to obtain The solution formula is:

[0244]

[0245] Among them, P [m+1] for abbreviation of .

[0246] In this embodiment, step S4 includes the following steps:

[0247] S41, use formula (1) to obtain the state prediction estimate of state x(k+1):

[0248]

[0249] S42, use formula (34) to calculate the state prediction estimation error value of state x(k+1):

[0250]

[0251] S43, use formula (1) and (35) to obtain the state prediction estimation error covariance matrix of state x(k+1):

[0252]

[0253] Where Q(k) is the variance of the state prediction error w(k) in formula (1);

[0254] S44, using formula (15) to obtain the r-order prediction estimate of the measurement y(k+1)

[0255]

[0256] S45, using formula (15) and (37) to calculate the r-order prediction estimation error value of measurement y(k+1)

[0257]

[0258] S46, using formula (38) to obtain the r-order prediction estimation error covariance matrix P of the measurement y(k+1) yy (k+1|k):

[0259]

[0260] Where R(k+1) is the variance of the measurement prediction error v(k+1) in formula (2); P yy The subscript yy in (k+1|k) indicates that the covariance is the r-order prediction error of the measurement y(k+1) The r-order prediction error with the measurement y(k+1) Take the mean of the product of ;

[0261] S47, use formula (35) and (38) to obtain the cross-covariance matrix P between the state and the measurement xy (k+1|k):

[0262]

[0263] Among them, P xy The subscript xy in (k+1|k) indicates that the covariance is the prediction error of state x(k+1) The r-order prediction error with the measurement y(k+1) Take the mean of the product of ;

[0264] S48, using the orthogonality between the state estimation error and the measurement value, the filter gain matrix K(k+1) is obtained:

[0265]

[0266] Where R(k+1) is the variance of the measurement prediction error v(k+1) in formula (2);

[0267] S49, using the value of the filter gain matrix K(k+1) and the state prediction estimate obtained by formula (34) to calculate the next state value:

[0268]

[0269] S410, using the state prediction estimation error covariance matrix P(k+1|k) and the filter gain matrix K(k+1), update the estimation error covariance matrix P(k+1|k+1):

[0270] P(k+1|k+1)=[IK(k+1)H(k+1)]P xx (k+1|k) (43)

[0271] Among them, P(k+1|k+1) is the next state I represents the identity matrix.

[0272] In this embodiment, step S5 is specifically as follows:

[0273] Using formula (43) we get Substitute into formula (36) and (41) to get P(2|1) and K(2), and then use formula (34) to get Substitute into formula (42) to obtain the state prediction value Substituting into formula (43) we get By analogy, we get Until

[0274] The effectiveness of the method of the present invention is verified by simulation experiments below:

[0275]

[0276] Here, it is assumed that the process noise w(k) is a set of independent zero-mean Gaussian white noise sequences with variances Q = 0.4. Similarly, the process noise v(k+1) is a set of independent zero-mean Gaussian white noise sequences with variances R = 0.4. Initial state value Initial variance P0 = [1, 0; 0, 1]. Simulation data are based on 500 Monte Carlo statistics, and the state prediction is done for n = 100 steps. The parameter is set to ΔT = 1. Figures 2 to 7 The data graph shown in the figure is used to obtain the parameter data tables of three groups A, B, and C, as shown in Table 1 and Table 2 below:

[0277] Table 1 Parameter data of three groups A, B and C

[0278] parameter A B C α 0.002 0.02 0.2 β 0.003 0.03 0.3

[0279] Table 2 Comparison of filter accuracy corresponding to each group of parameters

[0280] method The mean square error of A The mean square error of B Mean square error of C Extended Kalman Filter 0.00119 0.00191 0.00525 Second-order extended Kalman filter 0.00039 0.00062 0.00099 Improvements to the Extended Kalman Filter 67.23％ 67.54％ 81.14％ Third-order extended Kalman filter 0.00021 0.00031 0.00050 Improvements to the Extended Kalman Filter 82.35％ 83.77％ 90.48％

[0281] Note: Improvement rate = (error of existing method - error of the method of the present invention) / error of existing method * 100%

[0282] Compared to the traditional extended Kalman filter (existing method), the second-order extended Kalman filter method of the present invention for strongly nonlinear systems improves the identification accuracy of the state value x1 in the system models of Groups A, B, and C by 67.23%, 67.54%, and 81.14%, respectively. The third-order extended Kalman filter method improves the identification accuracy of the state value x1 in the system models of Groups A, B, and C by 82.35%, 83.77%, and 90.48%, respectively. The relative accuracy continues to improve with increasing nonlinearity, demonstrating the significant effectiveness of the present method for strongly nonlinear systems. These data demonstrate the effectiveness of the present high-order extended Kalman filter method.

[0283] exist Figure 2 、 Figure 4 and Figure 6 In the figure, the green line represents the extended Kalman filter in Table 2, the red line represents the second-order extended Kalman filter, the blue line represents the third-order extended Kalman filter, the green extended Kalman filter is the existing invention, the red line represents the second-order extended Kalman filter, and the blue line represents the third-order extended Kalman filter. The present invention is high-order, which means all orders greater than the first order. The higher the order, the better the effect. As shown in the chart, the third order is better than the second order, and both are better than the extended Kalman filter.

[0284] Figure 3 、 Figure 5 and Figure 7 It is used to intuitively demonstrate the effectiveness of the present invention and the existing methods. The red second-order extended Kalman filter and the blue third-order extended Kalman filter are the methods of the present invention, the green line extended Kalman filter is the existing invention, and the black line is the true value. The closer to the true value, the better the effect. After comparison, it can be clearly seen that the filtering effect of the method of the present invention is better than that of the existing method.

[0285] The above description is only a detailed description of the preferred embodiments and principles of the present invention. For ordinary technicians in this field, based on the ideas provided by the present invention, there may be changes in the specific implementation methods, and these changes should also be considered as the scope of protection of the present invention.

Claims

1. A high-order extended Kalman filter method for nonlinear systems, characterized by: The steps include: S1, establish a nonlinear system consisting of a linear state model equation and a nonlinear measurement model equation; S2, establish a computational algorithm for solving the statistical properties of Kronecker product type composite random variables; S3, using the algorithm established in step S2, analyzing the high-order characteristics of the nonlinear system state estimation error; S4, combining known measurement information with high-order features of the error, designing and optimizing an extended Kalman filter, and gradually estimating the next state in the nonlinear system; S5, looping the process of step S4, gradually estimating the state of the nonlinear system until the state estimation for a set number of steps is completed, and the state estimation value of each step is obtained.

2. The high-order extended Kalman filter method for nonlinear systems according to claim 1, characterized in that: Step S1 is specifically as follows: Assume that the state to be estimated x(k+1) has the following relationship with the system's state vector x(k) and the observation vector y(k+1) of the state x(k+1): x(k+1)=A(k)x(k)+w(k) (1) y(k+1)=h(x(k+1))+v(k+1) (2) Where A(k) is the state transfer matrix, h(·) is a nonlinear measurement function with arbitrary order continuous differentiability; x(k) is the n-dimensional state vector, w(k) is the n-dimensional state model error vector; y(k+1) is the m-dimensional measurement value vector, h(x(k+1)) is the m-dimensional measurement function vector, v(k+1) is the m-dimensional measurement model error vector, and k represents the kth moment of calculation.

3. The high-order extended Kalman filter method for nonlinear systems according to claim 2, characterized in that: Step S2 is specifically as follows: Suppose there is an n-dimensional vector x and an m-dimensional vector y: x=[x1,…,x i ,…,x n ] T ,y=[y1,…,y j ,…,and m ] T (3) Where T represents the transpose of the matrix; x and y are statistically independent and have the following statistical characteristics: x~N[0,P x ],y~N[0,P y ],Cov(x,y)=P xy (4) Among them, N means that it obeys the normal distribution; P x represents the variance of the random variable x; P y represents the variance of the random variable y; Cov(x,y) represents the variance of the random variable x and the random variable y; P xy Represents the specific numerical matrix corresponding to the variance of random variables x and y; Solution Statistical properties of: in, represents the Kronecker product; st represents arranging each element of the matrix into a vector according to the row number; E{·} represents finding the mean of the random variables in the brackets {·}; P represents the random variable variance; make get: S21, convert Kronecker product into matrix multiplication; Introduce an operation G: Among them, I y is the identity matrix with the same dimensions as y; x i represents the i-th element in vector x; Use operation G to convert the Kronecker product into a matrix multiplication: Among them, := means defined as; S22, P y Perform singular value decomposition and make further transformations: Among them, λ represents the matrix P y The singular value of the matrix P y Singular values ​​of ; Σ represents summation; S23, exchange Chinese symbol Position of elements on both sides: Introducing an operation Among them, the matrix M -1 represents the inverse matrix of matrix M; Expand the matrix: Wherein, the ((j-1)×n+i)th row of the matrix M is equal to the ((i-1)*m+j)th row of the nm-dimensional identity matrix; i=1,…,n; j=1,…,m; Using operations Do further conversion: S24, get Variance of:

4. The high-order extended Kalman filter method for nonlinear systems according to claim 3, characterized in that: Step S3 includes the following steps: S31, set formula (2) in The r-order Taylor expansion at is as follows: in, The initial value of is given according to the specific model. represents the system state estimation error The l-th order Kronecker product, H [l] (k+1) is h(x(k+1)) The l-order Jacobian matrix at , the superscript ^ represents the predicted value, and the superscript ~ represents the error between the predicted value and the true value; (k+1|k) represents the solution of the state value at the kth moment to the state value at the k+1th moment, the symbol | is followed by the condition, and the symbol before is the value to be calculated; k represents the kth moment; S32, calculate the state prediction estimation error Statistical properties of: According to the state prediction estimation error The definition of is: Find the expected mean: Then the covariance matrix is: in, is the variance of x(k), The initial value is given according to the specific model, := means defined as; The superscript [1,1] indicates that the covariance is the error of the first-order state prediction estimation error. Error with first-order state prediction estimate error The product of is averaged; mid-subscript Denotes that the covariance is the error of the state prediction estimation error Error with state prediction estimation error The product of is averaged; Thus, we get Statistical characteristics: S33, will Find the expectation and get the estimated value of the second-order state prediction error of state x(k+1): in, is the Kronecker product, and st is to arrange each element of the matrix into a vector according to the row number; S34, calculate the covariance matrix of the second-order state prediction estimation error: Then we get The solution formula is: S35, using the algorithm established in step S2 to obtain in, yes abbreviation of ; S36, substitute formula (23) into formula (22) to obtain The solution formula is: S37, will Find the expectation and get the estimated value of the third-order state prediction error of state x(k+1): Among them, 0 represents a vector whose elements are all 0; S38, using the algorithm established in step S2 to obtain the third-order state prediction estimation error covariance matrix in, yes The abbreviation of yes abbreviation of ; S39, will Find the expectation and get the estimated value of the odd 2m+1 order state prediction error of state x(k+1): Since m+1 and m must have an odd number, that is: Among them, or means either; Find the expected mean: S310, using the algorithm established in step S2 to calculate in, yes The abbreviation of yes abbreviation of ; S311, will Find the expectation and get the estimated value of the even 2m+2 order state prediction error of state x(k+1): Among them, st means arranging each element of the matrix into a vector according to the row number; S312, calculate the even 2m+2 order state prediction estimation error The covariance matrix of : Then we get The solution formula is: S313, calculate using the algorithm established in step S2 Among them, P [m+1] for abbreviation of ; Then we get The solution formula is: Among them, P [m+1] for abbreviation of ; S314, substitute formula (32) into formula (31) to obtain The solution formula is: Among them, P [m+1] for abbreviation of .

5. The high-order extended Kalman filter method for nonlinear systems according to claim 4, characterized in that: Step S4 includes the following steps: S41, use formula (1) to obtain the state prediction estimate of state x(k+1): S42, use formula (34) to calculate the state prediction estimation error value of state x(k+1): S43, use formula (1) and (35) to obtain the state prediction estimation error covariance matrix of state x(k+1): Where Q(k) is the variance of the state prediction error w(k) in formula (1); S44, using formula (15) to obtain the r-order prediction estimate of the measurement y(k+1) S45, using formula (15) and (37) to calculate the r-order prediction estimation error value of measurement y(k+1) S46, using formula (38) to obtain the r-order prediction estimation error covariance matrix P of the measurement y(k+1) yy (k+1|k): Where R(k+1) is the variance of the measurement prediction error v(k+1) in formula (2); P yy The subscript yy in (k+1|k) indicates that the covariance is the r-order prediction error of the measurement y(k+1) The r-order prediction error with the measurement y(k+1) Take the mean of the product of ; S47, use formula (35) and (38) to obtain the cross-covariance matrix P between the state and the measurement xy (k+1|k): Among them, P xy The subscript xy in (k+1|k) indicates that the covariance is the prediction error of state x(k+1) The r-order prediction error with the measurement y(k+1) Take the mean of the product of ; S48, using the orthogonality between the state estimation error and the measurement value, the filter gain matrix K(k+1) is obtained: Where R(k+1) is the variance of the measurement prediction error v(k+1) in formula (2); S49, using the value of the filter gain matrix K(k+1) and the state prediction estimate obtained by formula (34) to calculate the next state value: S410, using the state prediction estimation error covariance matrix P(k+1|k) and the filter gain matrix K(k+1), update the estimation error covariance matrix P(k+1|k+1): P(k+1|k+1)=[I-K(k+1)H(k+1)]P xx (k+1|k) (43) Among them, P(k+1|k+1) is the next state I represents the identity matrix.

6. The high-order extended Kalman filter method for nonlinear systems according to claim 5, characterized in that: Step S5 is specifically as follows: Using formula (43) we get Substitute into formula (36) and (41) to get P(2|1) and K(2), and then use formula (34) to get Substitute into formula (42) to obtain the state prediction value Substituting into formula (43) we get By analogy, we get Until

Images