Adaptive volume Kalman filter aircraft aerodynamic parameter identification method

Abstract

The invention discloses a method for identifying aerodynamic parameters of an adaptive volume Kalman filter aircraft, which is used for improving a traditional Kalman filter algorithm so as to solve the common problem of negative determination of a covariance matrix in practical application. Specifically, singular value decomposition is adopted to replace traditional Cholesky decomposition to decompose a covariance matrix, and the improvement can improve the stability and precision of the algorithm, so that the aerodynamic parameters of the missile are more reliably estimated; in the filtering process, the noise variance is corrected according to real-time measurement information, so that the state estimation error is reduced; according to the method, the adaptive capacity to the dynamic change of the system can be improved, and the precision and robustness of parameter estimation are further improved.

Description

Technical Field

[0001] The invention belongs to the technical field of parameter identification, and in particular relates to an adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method. Background Art

[0002] Parameter identification methods include offline and online methods. Offline methods include least squares method, recursive least squares method and least squares method with forgetting factor. Online methods usually use Kalman filter, extended Kalman filter and unscented Kalman filter. Compared with offline methods, online methods can estimate model parameters in real time, so they are widely used in control system models. Parameters can be revised at any time to improve system accuracy, so they are more common.

[0003] In the field of aircraft parameter identification, the extended Kalman filter (EKF) is the most common method. For example, some researchers have used EKF to identify the parameters of tactical missiles. However, for complex nonlinear systems, due to the difficulty in solving the Jacobian matrix and the Hessian matrix, the EKF has the problems of large computational complexity and low estimation accuracy. In order to solve these problems, an improved extended Kalman filter method has been proposed. Based on the iterative filtering theory, by re-expanding the Taylor series at the EKF estimation point and iterating, the linearization error is effectively reduced and the identification accuracy is improved. However, this does not fundamentally solve the problem of computational complexity.

[0004] Some scholars have proposed an online identification method for aerodynamic parameters of cruise missiles based on unscented Kalman filtering (UKF). Compared with EKF, UKF has higher accuracy and avoids the problem of solving Jacobian matrix and Hessian matrix. However, UKF has unstable performance when dealing with high-dimensional strong nonlinear systems. Moreover, in practical applications, these methods often face the problem of negative covariance matrix.

[0005] Based on this, the present invention proposes an adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method. Summary of the invention

[0006] In view of the above technical problems, the present invention provides an adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method.

[0007] The technical solution adopted by the present invention to solve the technical problem is:

[0008] An adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method, the method comprising the following steps:

[0009] S100: Obtain the flight parameters of the aircraft and establish the state equation and measurement equation of the tactical missile;

[0010] S200: adding the parameters to be identified into the state equation to establish the augmented state equation;

[0011] S300: Initialize the filter to determine the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the initial moment;

[0012] S400: Time update: predicting the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the current moment according to the augmented state equation, the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the previous moment, and the process noise covariance matrix;

[0013] S500: Measurement update: Calculate the updated state estimate and the updated covariance matrix according to the measurement equation, the augmented state variable estimate at the current moment, the augmented state variable error covariance matrix estimate, and the measurement noise covariance matrix;

[0014] S600: recursively estimate the noise variance, and return to S400 to iterate until a preset number of iterations is reached to obtain a final estimate of the parameter to be identified.

[0015] Preferably, the state equation in S100 is specifically:

[0016]

[0017] Among them, V x 、V y 、V z are the linear velocities of the three components of the axial, normal and lateral directions in the projectile coordinate system; ω x ,ω y ,ω z are the angular velocities in three directions respectively; I x ,I y ,I z are the moments of inertia of the roll, yaw and pitch channels respectively, X, Y, Z are the axial force, normal force and lateral force respectively, γ, ψ is the roll, yaw, and pitch angles, M x ,M y ,M z is the torque in each direction, and h is the flight height.

[0018] Preferably, the measurement equation in S100 is specifically:

[0019] ω x =ω x +v 1

[0020] ω y =ω y +v2

[0021] ω z =ω z +v 3

[0022]

[0023] ψ=ψ+v 8

[0024] γ=γ+v 9

[0025] h=h+v 10

[0026] α=arctg(-V y / V x )+v 11

[0027] β=arcsin(V z / V)+v 12

[0028] Where X, Y, and Z are axial force, normal force, and lateral force, respectively; Nx, Ny, and Nz are overloads of axial force, normal force, and lateral force, respectively; g 0 is the acceleration due to gravity; V is the velocity; v 1 ~v 12 are the noise of each observation, are Gaussian noise and follow Gaussian distribution; α, β are the angle of attack and sideslip angle, respectively.

[0029] Preferably, the parameters to be identified in S200 are

[0030]

[0031] Among them, C x ,C y ,C z are the aerodynamic forces of axial force, normal force and lateral force respectively, C x0 , is the axial force coefficient; C y0 , is the normal force coefficient; C z0 , is the lateral force coefficient;

[0032] The axial force, normal force and lateral force are as follows:

[0033]

[0034] Among them, q represents dynamic pressure, which indicates the energy density of aerodynamic flow. The calculation formula is: S m It is a characteristic area of ​​the missile;

[0035] S200 is specifically:

[0036]

[0037] z(k)=h(x(k),u(k))+v(k)

[0038] in, is the augmented state vector, where the augmented state vector is z(k) is the measurement vector, and the measurement vector is u(k) is the control input, and the control input parameter is δ x , δ y , δ z , w(k), v(k) are system noises.

[0039] Preferably, S300 specifically includes:

[0040]

[0041] in, represents the estimated value of the initial state, P 0 represents the error covariance matrix.

[0042] Preferably, S400 includes:

[0043] S410: Use SVD to calculate the covariance matrix P k-1 Decompose and further obtain volume points, specifically:

[0044]

[0045] Where S is a diagonal matrix, S = diag{s 1 ,s 2 ,…,s n}After SVD decomposition of the covariance matrix, U is the left singular matrix and V is the right singular matrix;

[0046] P k-1 =U k-1 S k-1 V T k-1

[0047]

[0048] Among them, X i,k-1 represents the i-th volume point at time k-1, U i,k-1 Denotes the covariance matrix P k-1 The i-th column in the decomposed left singular vector matrix, Denotes the covariance matrix P k-1 The square root of the i-th singular value in the decomposed singular value matrix, represents the mean value of the state prediction at time k-1; i = 1, 2, m represents the number of volume sampling points. When the third-order volume rule is used, the total number of volume sampling points is twice the state dimension, that is, m = 2n; [1] i represents the i-th point in the point set [1], where the symbol [1] represents a complete and fully symmetric point set;

[0049] [1] represents a set of points in n-dimensional space, namely:

[0050]

[0051] S420: Calculate the propagation volume points through the nonlinear state equation:

[0052] X * i,k =f(X i,k-1 ,u k-1 )

[0053] Among them, X i,k-1 represents the i-th volume point at time k-1, f represents the state transition, X * i,k represents the i-th state prediction point after transformation by the state transfer function f; u k-1 is the control input or external input at time k-1;

[0054] S430: Calculate the state prediction value and the value of the error covariance matrix at time k-1:

[0055]

[0056] Among them, Q k-1 is the process noise covariance matrix.

[0057] Preferably, S500 includes:

[0058] S510: P k|k-1 Perform SVD decomposition, specifically:

[0059] P k|k-1 =U k|k-1 S k|k-1 V T k|k-1

[0060]

[0061] S520: Calculate the propagated volume point by using a nonlinear measurement equation, specifically:

[0062] Zi,k|k-1 =h(X i,k|k-1 )

[0063] Among them, h is the measurement equation, Z i,k|k-1 It is the predicted value of the observation obtained by mapping the state into the observation space through the measurement equation;

[0064] S530: Estimate the observation prediction value at time k, and calculate the measurement covariance matrix and the cross covariance matrix of the state and the measurement, specifically:

[0065]

[0066] in, is the predicted measurement mean obtained by numerical integration of volume points, P zz,k|k-1 is the measurement covariance matrix, R k is the measurement noise covariance matrix, which describes the statistical characteristics of the measurement noise, P xz,k|k-1 is the cross-covariance matrix of state and measurement, describing the covariance between state and measurement;

[0067] S540: Estimate the filter gain and the state estimation value and covariance matrix at time k, specifically:

[0068] W k =P xz,k|k-1 P xz,k|k-1 -1

[0069]

[0070] Among them, W k is the Kalman gain matrix, which is used to combine the predicted state with the measurement information; is the updated state estimate, z k is the actual measured value, is the predicted measured mean, P k is the updated covariance matrix.

[0071] Preferably, the noise variance is estimated recursively in S600, specifically:

[0072]

[0073] in, In order to enhance the effect of new observation information, the gradual memory exponent is introduced, and a=(1-b k ) / (1-b) is substituted into the equation, where b is the forgetting factor and 0 is the <b<1;

[0074] In practical applications, the minus sign in the equation may cause the noise variance matrix to lose positive definiteness and lead to filter divergence. Using a noise biased estimator can prevent filter divergence:

[0075]

[0076] Therefore, the following improved noise estimator is proposed:

[0077]

[0078] Where z is any non-zero vector, usually a standard basis vector or a random vector is selected as a test vector to determine the matrix and Whether it is positive or not;

[0079] In the process of estimating the noise variance, when is non-semidefinite or If it is not positive definite, use the second formula to correct it;

[0080] If not satisfied is non-semidefinite or Since it is non-positive definite, the first formula is used for estimation;

[0081] If time efficiency is important and one is willing to sacrifice filtering accuracy to some extent, the third formula is used to perform biased estimation of the noise.

[0082] The above-mentioned adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method combines the Kalman filter algorithm based on singular value decomposition and the Sage-Husa adaptive filter, realizes the singular value decomposition of the covariance matrix, and corrects the noise variance according to the real-time measurement information, thereby improving the stability, accuracy and adaptability of parameter estimation. This innovative technology can be widely used in fields such as aircraft parameter identification, providing a more robust and accurate solution for real-time parameter estimation, and providing an important guarantee for improving system performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] Figure 1 It is a flow chart of an adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method in one embodiment of the present invention;

[0084] Figure 2 The present invention is a flowchart of an adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method in another embodiment of the present invention. DETAILED DESCRIPTION

[0085] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.

[0086] In one embodiment, Figure 1 and 2As shown, an adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method, the method comprising the following steps:

[0087] S100: Obtain the flight parameters of the aircraft and establish the state equation and measurement equation of the tactical missile;

[0088] S200: adding the parameters to be identified into the state equation to establish the augmented state equation;

[0089] S300: Initialize the filter to determine the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the initial moment;

[0090] S400: Time update: predicting the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the current moment according to the augmented state equation, the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the previous moment, and the process noise covariance matrix;

[0091] S500: Measurement update: Calculate the updated state estimate and the updated covariance matrix according to the measurement equation, the augmented state variable estimate at the current moment, the augmented state variable error covariance matrix estimate, and the measurement noise covariance matrix;

[0092] S600: recursively estimate the noise variance, and return to S400 to iterate until a preset number of iterations is reached to obtain a final estimate of the parameter to be identified.

[0093] Specifically, the above-mentioned adaptive volumetric Kalman filter aerodynamic parameter identification method for aircraft improves the traditional Kalman filter algorithm to solve the common problem of negative covariance matrix in practical applications. Specifically, singular value decomposition is used to decompose the covariance matrix instead of the traditional Cholesky decomposition. This improvement can improve the stability and accuracy of the algorithm, making it more reliable when estimating the aerodynamic parameters of the missile; in the filtering process, the noise variance is corrected according to the real-time measurement information, thereby reducing the state estimation error. This method can improve the adaptability to the dynamic changes of the system and further improve the accuracy and robustness of parameter estimation.

[0094] In one embodiment, the state equation in S100 is specifically:

[0095]

[0096] Among them, V x 、V y 、V z are the linear velocities of the three components of the axial, normal and lateral directions in the projectile coordinate system; ω x ,ω y ,ωz are the angular velocities in three directions respectively; I x ,I y ,I z are the moments of inertia of the roll, yaw and pitch channels respectively, X, Y, Z are the axial force, normal force and lateral force respectively, γ, ψ is the roll, yaw, and pitch angles, M x ,M y ,M z is the torque in each direction, and h is the flight height.

[0097] In one embodiment, the measurement equation in S100 is specifically:

[0098] ω x =ω x +v 1

[0099] ω y =ω y +v 2

[0100] ω z =ω z +v 3

[0101]

[0102] ψ=ψ+v 8

[0103] γ=γ+v 9

[0104] h=h+v 10

[0105] α=arctg(-V y / V x )+v 11

[0106] β=arcsin(V z / V)+v 12

[0107] Where X, Y, and Z are axial force, normal force, and lateral force, respectively; Nx, Ny, and Nz are overloads of axial force, normal force, and lateral force, respectively; g 0 is the acceleration due to gravity; V is the velocity; v 1 ~v 12 are the noise of each observation, are Gaussian noise and follow Gaussian distribution; α, β are the angle of attack and sideslip angle, respectively.

[0108] Specifically, the core task of the system identification process is to extract the most accurate and reliable information from the measurement data, while eliminating the interference of noise, so as to more accurately estimate the state of the system.

[0109] In one embodiment, the parameters to be identified in S200 are specifically:

[0110]

[0111] Among them, C x ,C y ,C z are the aerodynamic forces of axial force, normal force and lateral force respectively, C x0 , is the axial force coefficient; C y0 , is the normal force coefficient; C z0 , is the lateral force coefficient;

[0112] The axial force, normal force and lateral force are as follows:

[0113]

[0114] Among them, q represents dynamic pressure, which indicates the energy density of aerodynamic flow. The calculation formula is: S m It is a characteristic area of ​​the missile;

[0115] S200 is specifically:

[0116]

[0117] z(k)=h(x(k),u(k))+v(k)

[0118] in, is the augmented state vector, where the augmented state vector is z(k) is the measurement vector, and the measurement vector is u(k) is the control input, and the control input parameter is δ x , δ y , δ z , w(k), v(k) are system noises.

[0119] In one embodiment, S300 specifically includes:

[0120]

[0121] in, represents the estimated value of the initial state, P 0 represents the error covariance matrix.

[0122] Furthermore, time updating is the first step of the Kalman filter algorithm, which predicts the system state at the current moment based on the system's dynamic model and the state estimate at the previous moment.

[0123] In one embodiment, S400 includes:

[0124] S410: Use SVD to calculate the covariance matrix P k-1 Decompose and further obtain volume points, specifically:

[0125]

[0126] Where S is a diagonal matrix, S = diag{s 1 ,s 2 ,…,s n}After SVD decomposition of the covariance matrix, U is the left singular matrix and V is the right singular matrix;

[0127] P k-1 =U k-1 S k-1 V T k-1

[0128]

[0129] Among them, X i,k-1 represents the i-th volume point at time k-1, U i,k-1 Denotes the covariance matrix P k-1 The i-th column in the decomposed left singular vector matrix, Denotes the covariance matrix P k-1 The square root of the i-th singular value in the decomposed singular value matrix, represents the mean value of the state prediction at time k-1 (prior state estimate); i = 1, 2, m represents the number of volume sampling points. When the third-order volume rule is used, the total number of volume sampling points is twice the state dimension, that is, m = 2n; [1] i represents the i-th point in the point set [1], where the symbol [1] represents a complete and fully symmetric point set;

[0130] [1] represents a set of points in n-dimensional space, namely:

[0131]

[0132] S420: Calculate the propagation volume points through the nonlinear state equation:

[0133] X * i,k =f(X i,k-1 ,u k-1 )

[0134] Among them, X i,k-1 represents the i-th volume point at time k-1, f represents the state transition, X * i,k represents the i-th state prediction point after transformation by the state transfer function f; u k-1 is the control input or external input at time k-1; it can be an external observation or other control variable in the environment. Further, the transformed volume points are used for numerical integration to calculate the mean and covariance of the predicted state;

[0135] S430: Calculate the state prediction value and the value of the error covariance matrix at time k-1:

[0136]

[0137] Among them, Q k-1 is the process noise covariance matrix.

[0138] Furthermore, measurement update (also called update step) is another important step in the Kalman filter algorithm, which uses the observation value at the current moment to correct the prior state estimate obtained in the time update step.

[0139] In one embodiment, S500 includes:

[0140] S510: P k|k-1 Perform SVD decomposition, specifically:

[0141] P k|k-1 =U k|k-1 S k|k-1 V T k|k-1

[0142]

[0143] S520: Calculate the propagated volume point by using a nonlinear measurement equation, specifically:

[0144] Z i,k|k-1 =h(X i,k|k-1 )

[0145] Among them, h is the measurement equation, Z i,k|k-1 It is the predicted value of the observation obtained by mapping the state into the observation space through the measurement equation;

[0146] S530: Estimate the observation prediction value at time k, and calculate the measurement covariance matrix and the cross covariance matrix of the state and the measurement, specifically:

[0147]

[0148] in, is the predicted measurement mean obtained by numerical integration of volume points, P zz,k|k-1 is the measurement covariance matrix, R k is the measurement noise covariance matrix, which describes the statistical characteristics of the measurement noise, P xz,k|k-1 is the cross-covariance matrix of state and measurement, describing the covariance between state and measurement;

[0149] S540: Estimate the filter gain and the state estimation value and covariance matrix at time k, specifically:

[0150] W k =P xz,k|k-1 P xz,k|k-1 -1

[0151]

[0152] Among them, W k is the Kalman gain matrix, which is used to combine the predicted state with the measurement information; is the updated state estimate, z k is the actual measured value, is the predicted measured mean, P k is the updated covariance matrix.

[0153] In one embodiment, the noise variance is estimated recursively in S600, specifically:

[0154]

[0155] in, In order to enhance the effect of new observation information, the gradual memory exponent is introduced, and a=(1-b k ) / (1-b) is substituted into the equation, where b is the forgetting factor and 0 is the <b<1;

[0156] In practical applications, the minus sign in the equation may cause the noise variance matrix to lose positive definiteness and lead to filter divergence. Using a noise biased estimator can prevent filter divergence:

[0157]

[0158] Therefore, the following improved noise estimator is proposed:

[0159]

[0160] Where z is any non-zero vector, usually a standard basis vector or a random vector is selected as a test vector to determine the matrix and Whether it is positive or not;

[0161] In the process of estimating the noise variance, when is non-semidefinite or If it is not positive definite, use the second formula to correct it;

[0162] If not satisfied is non-semidefinite or Since it is non-positive definite, the first formula is used for estimation;

[0163] If time efficiency is important and one is willing to sacrifice filtering accuracy to some extent, the third formula is used to perform biased estimation of the noise.

[0164] Specifically, as long as the initially set system noise variance is positive definite, this positive definiteness will be maintained in the subsequent filtering process.

[0165] The above-mentioned adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method combines the Kalman filter algorithm based on singular value decomposition and the Sage-Husa adaptive filter, realizes the singular value decomposition of the covariance matrix, and corrects the noise variance according to the real-time measurement information, thereby improving the stability, accuracy and adaptability of parameter estimation. This innovative technology can be widely used in fields such as aircraft parameter identification, providing a more robust and accurate solution for real-time parameter estimation, and providing an important guarantee for improving system performance.

[0166] The above is a detailed introduction to an adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method provided by the present invention. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of ​​the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. An adaptive volumetric Kalman filter aircraft aerodynamic parameter identification method, characterized in that: The method comprises the following steps: S100: Obtain the flight parameters of the aircraft and establish the state equation and measurement equation of the tactical missile; S200: adding the parameters to be identified into the state equation to establish the augmented state equation; S300: Initialize the filter to determine the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the initial moment; S400: Time update: predicting the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the current moment according to the augmented state equation, the estimated value of the augmented state variable and the estimated value of the augmented state variable error covariance matrix at the previous moment, and the process noise covariance matrix; S500: Measurement update: Calculate the updated state estimate and the updated covariance matrix according to the measurement equation, the augmented state variable estimate at the current moment, the augmented state variable error covariance matrix estimate, and the measurement noise covariance matrix; S600: recursively estimate the noise variance, and return to S400 to iterate until a preset number of iterations is reached to obtain a final estimate of the parameter to be identified.

2. The method according to claim 1, characterized in that The state equation in S100 is: Among them, V x 、V y 、V z are the linear velocities of the three components of the axial, normal and lateral directions in the projectile coordinate system; ω x ,ω y ,ω z are the angular velocities in three directions respectively; I x ,I y ,I z are the moments of inertia of the roll, yaw and pitch channels respectively, X, Y, Z are the axial force, normal force and lateral force respectively, γ, ψ is the roll, yaw, and pitch angles, M x ,M y ,M z is the moment in each direction, and h is the flight height.

3. The method according to claim 2, characterized in that The measurement equation in S100 is as follows: oh x =ω x +v1 oh y =ω y +v2 oh z =ω z +v3 ψ=ψ+v8 γ=γ+v9 h=h+v 10 α=arctg(-V y / V x )+v 11 β=arcsin(V z / V)+v 12 Where X, Y, and Z are axial force, normal force, and lateral force, respectively; Nx, Ny, and Nz are overloads of axial force, normal force, and lateral force, respectively; g0 is the acceleration due to gravity; V is the velocity; v1~v 12 are the noise of each observation, are Gaussian noise and follow Gaussian distribution; α, β are the angle of attack and sideslip angle, respectively.

4. The method according to claim 3, characterized in that: The parameters to be identified in S200 are specifically: Among them, C x ,C y ,C z are the aerodynamic forces of axial force, normal force and lateral force respectively, C x0 , is the axial force coefficient; C y0 , is the normal force coefficient; C z0 , is the lateral force coefficient; The axial force, normal force and lateral force are as follows: Among them, q represents dynamic pressure, which indicates the energy density of aerodynamic flow. The calculation formula is: S m It is a characteristic area of ​​the missile; S200 is specifically: z(k)=h(x(k),u(k))+v(k) in, is the augmented state vector, where the augmented state vector is z(k) is the measurement vector, and the measurement vector is u(k) is the control input, and the control input parameter is δ x , δ y , δ z , w(k), v(k) are system noises.

5. The method according to claim 4, characterized in that S300 is specifically: in, represents the estimated value of the initial state, and P0 represents the error covariance matrix.

6. The method according to claim 5, characterized in that S400 includes: S410: Use SVD to calculate the covariance matrix P k-1 Decompose and further obtain volume points, specifically: Where S is a diagonal matrix, S = diag{s1,s2,…,s n }After SVD decomposition of the covariance matrix, U is the left singular matrix and V is the right singular matrix; P k-1 =U k-1 S k-1 V T k-1 Among them, X i,k-1 represents the i-th volume point at time k-1, U i,k-1 Denotes the covariance matrix P k-1 The i-th column in the decomposed left singular vector matrix, Denotes the covariance matrix P k-1 The square root of the i-th singular value in the decomposed singular value matrix, represents the mean value of the state prediction at time k-1; i = 1, 2, m represents the number of volume sampling points. When the third-order volume rule is used, the total number of volume sampling points is twice the state dimension, that is, m = 2n; [1] i represents the i-th point in the point set [1], where the symbol [1] represents a complete and fully symmetric point set; [1] represents a set of points in n-dimensional space, namely: S420: Calculate the propagation volume points through the nonlinear state equation: X * i,k =f(X i,k-1 ,u k-1 ) Among them, X i,k-1 represents the i-th volume point at time k-1, f represents the state transition, X * i,k represents the i-th state prediction point after transformation by the state transfer function f; u k-1 is the control input or external input at time k-1; S430: Calculate the state prediction value and the value of the error covariance matrix at time k-1: Among them, Q k-1 is the process noise covariance matrix.

7. The method according to claim 6, characterized in that S500 includes: S510: P k|k-1 Perform SVD decomposition, specifically: P k|k-1 =U k|k-1 S k|k-1 V T k|k-1 S520: Calculate the propagated volume point by using a nonlinear measurement equation, specifically: Z i,k|k-1 =h(X i,k|k-1 ) Among them, h is the measurement equation, Z i,k|k-1 It is the predicted value of the observation obtained by mapping the state into the observation space through the measurement equation; S530: Estimate the observation prediction value at time k, and calculate the measurement covariance matrix and the cross covariance matrix of the state and the measurement, specifically: in, is the predicted measurement mean obtained by numerical integration of volume points, P zz,k|k-1 is the measurement covariance matrix, R k is the measurement noise covariance matrix, which describes the statistical characteristics of the measurement noise, P xz,k|k-1 is the cross-covariance matrix of state and measurement, describing the covariance between state and measurement; S540: Estimate the filter gain and the state estimation value and covariance matrix at time k, specifically: W k =P xz,k|k-1 P xz,k|k-1 -1 Among them, W k is the Kalman gain matrix, which is used to combine the predicted state with the measurement information; is the updated state estimate, z k is the actual measured value, is the predicted measured mean, P k is the updated covariance matrix.

8. The method according to claim 7, characterized in that The noise variance is recursively estimated in S600, specifically: in, In order to enhance the effect of new observation information, the gradual memory exponent is introduced, and a=(1-b k ) / (1-b) is substituted into the equation, where b is the forgetting factor and 0 is the <b<1; In practical applications, the minus sign in the equation may cause the noise variance matrix to lose positive definiteness and lead to filter divergence. Using a noise biased estimator can prevent filter divergence: Therefore, the following improved noise estimator is proposed: Where z is any non-zero vector, usually a standard basis vector or a random vector is selected as a test vector to determine the matrix and Whether it is positive or not; In the process of estimating the noise variance, when is non-semidefinite or If it is not positive definite, use the second formula to correct it; If not satisfied is non-semidefinite or Since it is non-positive definite, the first formula is used for estimation; If time efficiency is important and one is willing to sacrifice filtering accuracy to some extent, the third formula is used to perform biased estimation of the noise.

Images