Attitude estimation method of rotating projectile correction assembly based on maximum correlation entropy and strong tracking filtering

Abstract

The invention discloses an attitude estimation method of a rotating projectile correction assembly based on maximum correlation entropy and strong tracking filtering. The method comprises the following steps: establishing a three-axis geomagnetic sensor and MEMS gyroscope combined attitude estimation system model; a rotating projectile attitude estimation algorithm based on square root cubature Kalman filtering is realized on the basis of the constructed system model; based on the maximum correlation entropy criterion of the Cauchy kernel function, correction of the measurement noise of the sensor is realized; based on a strong tracking filtering principle, correction of an innovation covariance matrix is realized; and calculating a rotating projectile correction assembly state vector. Compared with the prior art, the method can effectively reduce the influence of the rapid change of the motion state of the correction assembly in the flight process of the projectile and the external interference on the state estimation, and improves the attitude measurement precision of the rotary projectile correction assembly.

Description

Technical Field

[0001] The invention relates to the field of sensors, and in particular to a posture estimation method of a rotating projectile correction component based on maximum correlation entropy and strong tracking filtering. Background Art

[0002] The correction component of the rotating projectile changes the aerodynamic force on the whole projectile by controlling the deflection of the rudder, and then adjusts the attitude of the projectile to achieve the correction of the trajectory. Obtaining accurate attitude angle information during the flight of the projectile is the prerequisite for realizing rudder deflection control and then realizing accurate guidance of the projectile. Due to the relatively complex motion characteristics and structural limitations of the rotating projectile, the attitude measurement sensors that can be adapted at present are limited, including micro-electromechanical (MEMS) gyroscopes, geomagnetic sensors, etc. When the attitude is estimated by the above means, due to the influence of the carrier motion state change and environmental noise, the traditional Kalman filter model has the problem of large error and easy divergence, which seriously affects the result of attitude estimation. Especially for the split correction component with fixed control rudder and component, the traditional Kalman filter model is difficult to achieve rapid tracking of the attitude change of the correction component. Therefore, new methods are urgently needed to improve the performance of the attitude estimation system of the rotating projectile.

[0003] The idea of ​​strong tracking filter is borrowed in the square root cubature Kalman filter of the correction component attitude estimation. The Kalman filter gain is adjusted by calculating the fading factor through the orthogonal criterion of the measurement residual vector, so as to realize the adaptive tracking of the model uncertainty and the change of the statistical characteristics of the noise. When the motion state of the correction component changes rapidly, the attitude estimation algorithm after introducing the strong tracking filter can quickly adjust the parameters to maintain an accurate estimation of the system state. However, the attitude measurement sensor in the missile-borne environment is easily affected by the complex environmental noise. Since the strong tracking filter needs to use the statistical characteristics of the measurement residual vector in the calculation process, the strong tracking filter is more sensitive to the interference of non-Gaussian measurement noise and measurement wild values. Complex measurement noise will seriously affect the accuracy of the strong tracking filter. Summary of the invention

[0004] In order to solve the above problems, the purpose of the present invention is to provide a posture estimation method for a rotating projectile correction component based on maximum correlation entropy and strong tracking filtering, which combines maximum correlation entropy and strong tracking filtering to suppress the interference of modeling inaccuracy and sensor noise, improve the robustness of the filtering model, and realize rapid tracking and high-precision estimation of the flight attitude of the rotating projectile correction component.

[0005] The technical solution of the present invention is: a method for estimating the attitude of a rotating projectile correction component based on maximum correlation entropy and strong tracking filtering, the method is based on known quantities: the output information of the measurement sensor used includes: three-axis gyroscope information, three-axis geomagnetic sensor information. The three-axis gyroscope information is represents the gyroscope information of the x-axis at time k, represents the gyroscope information of the y-axis at time k, represents the gyroscope information of the z-axis at time k; the three-axis geomagnetic sensor information is Represents the geomagnetic sensor information of the x-axis at time k, Represents the geomagnetic sensor information of the y-axis at time k, The method comprises the following steps:

[0006] S1: Establish a three-axis geomagnetic sensor and MEMS gyroscope combined attitude estimation system model;

[0007] S2: Implement the rotating projectile attitude estimation algorithm based on the square root volumetric Kalman filter based on the constructed system model;

[0008] S3: Based on the maximum correlation entropy criterion of the Cauchy kernel function, the sensor measurement noise is corrected to obtain the estimated value of the measurement noise variance matrix

[0009] S4: Based on the principle of strong tracking filtering, the measurement error covariance matrix is ​​realized and the one-step prediction cross-correlation covariance matrix amendments;

[0010] S5: Calculate the attitude estimation state vector of the rotating projectile correction component.

[0011] Compared with the prior art, the advantages of the present invention are:

[0012] (1) The present invention introduces a strong tracking filter into the square root volumetric Kalman filter model of the combined attitude estimation of the rotating projectile correction assembly, solves the estimation error problem caused by the rapid change of the carrier motion state, and improves the accuracy of the attitude estimation of the rotating projectile correction assembly;

[0013] (2) The present invention introduces the maximum correlation entropy criterion based on the Cauchy kernel function into the square root cubature Kalman filter model, solves the problem of filter divergence caused by non-Gaussian measurement errors and measurement wild values, and improves the anti-interference ability of attitude estimation.

[0014] (3) The present invention combines the maximum correlation entropy criterion and the strong tracking filter in the square root cubature Kalman filter model, so that the attitude estimation algorithm of the rotating projectile correction component can still achieve good tracking of the rapid changes in the carrier's motion state in a complex projectile-borne noise environment, thereby improving the robustness and applicability of attitude estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1Design a flow chart for the rotating projectile pose estimation method.

[0016] Figure 2 These are the pitch angle θ, roll angle γ, yaw angle ψ and time curves of the rotating projectile.

[0017] Figure 3 The graphs are the measurement data of the gyroscope sensor and the measurement data of the three-axis geomagnetic sensor versus time.

[0018] Figure 4 These are the attitude estimation error and time curves of the method proposed in the present invention and the comparative method, including (a) the pitch angle error curve, (b) the yaw angle error curve, and (c) the roll angle error curve. DETAILED DESCRIPTION

[0019] The present invention provides a method for estimating the attitude of a rotating projectile correction component based on maximum correlation entropy and strong tracking filtering. The method is based on known quantities: the output information of the measurement sensor used includes: three-axis gyroscope information and three-axis geomagnetic sensor information. The three-axis gyroscope information is represents the gyroscope information of the x-axis at time k, represents the gyroscope information of the y-axis at time k, represents the gyroscope information of the z-axis at time k; the three-axis geomagnetic sensor information is Represents the geomagnetic sensor information of the x-axis at time k, Represents the geomagnetic sensor information of the y-axis at time k, shows the geomagnetic sensor information of the z-axis at time k; Figure 1 As shown, the method comprises the following steps:

[0020] First, the present invention provides a posture estimation method of a rotating projectile correction component based on maximum correlation entropy and strong tracking filtering, the steps of which include:

[0021] S1: Establish a three-axis geomagnetic sensor and MEMS gyroscope combined attitude estimation system model:

[0022] S101: Establish system status model:

[0023] X k =Φ k X k-1 +W k-1 (18)

[0024] Among them, X k represents the state vector of the system at time k, X k-1 represents the state vector of the system at time k-1, Φ k represents the state transfer matrix at time k, W k-1represents the noise covariance matrix of the system at time k-1, where the state vector X k It is expressed as follows:

[0025]

[0026] where θ k , γ k , k They represent the pitch angle, roll angle, and yaw angle at time k respectively; They represent the pitch angular rate, roll angular rate, and yaw angular rate at time k respectively.

[0027] S102: Establishing system measurement model:

[0028] Z k =h(X k )+V k (20)

[0029] Where Z k represents the measurement value at time k, h(Xk) represents the measurement transfer nonlinear function at time k, V k represents the measurement noise covariance matrix at time k, where Z k It is expressed as follows:

[0030]

[0031] S2: Based on the constructed system model, the attitude estimation equation of the rotating projectile correction component based on the square root volume Kalman filter is established:

[0032] S201: Establish a state prediction equation; the formula is:

[0033]

[0034] In the formula, Represents the state estimation vector of the system at time k+1; represents the state estimation vector of the system at time k; S k represents the square root form of the covariance matrix at time k; S k+1|k represents the square root form of the prediction covariance matrix at time k+1; Q k represents the system noise variance matrix at time k; QR(·) represents the QR operation.

[0035] S202: Establishing a measurement estimation equation to obtain a measurement error covariance matrix and a one-step prediction cross-correlation covariance matrix;

[0036] The formula is:

[0037]

[0038] Where, X i,k+1∣k is the state volume point, h(X i,k+1∣k ) is the measurement transfer nonlinear calculation of the state volume point, ξ i is the basic volume point of the third-order volume criterion; i is the volume point sequence number; Z i,k+1|k is the volume measurement point; represents the measured predicted value at time k+1; n represents the dimension of the measurement equation.

[0039] Then the measurement error covariance matrix at time k+1 is and the one-step prediction cross-correlation covariance matrix for:

[0040]

[0041] In the formula, R k represents the measurement noise variance matrix.

[0042] S3: Based on the maximum correlation entropy criterion of the Cauchy kernel function, the sensor measurement noise is corrected to obtain the estimated value of the measurement noise variance matrix

[0043] S301: Solve the weight matrix based on the residual vector;

[0044] The Cauchy kernel function is introduced, which is specifically expressed as follows:

[0045]

[0046] In the formula, c k represents the weight matrix; represents the Cauchy kernel function; σ represents the width parameter of the Cauchy kernel function; diag[·] represents the function of taking the diagonal elements of the matrix; e k Represents the residual vector at time k, and its calculation formula is as follows: In the formula, π k represents the constructed state and measurement prediction residual vector, M k It represents the square root form of the constructed state covariance and measurement variance diagonal matrix, and its calculation formula is as follows:

[0047]

[0048] In the formula, Z k represents the measurement vector at time k; I represents the measured predicted value at time k+1; n represents the n-dimensional identity matrix; H k Represents the equivalent linear measurement transfer matrix at time k, and the calculation formula is as follows:

[0049]

[0050] S302: Correct the measurement noise variance matrix based on the calculation of the weight matrix to obtain an estimated value of the measurement noise variance matrix The calculation formula is as follows:

[0051]

[0052] S4: Based on the principle of strong tracking filtering, the measurement error covariance matrix is ​​realized and the one-step prediction cross-correlation covariance matrix Corrections:

[0053] S401: Calculate the estimated value of the variance of the residual sequence, the formula is:

[0054]

[0055] In the formula, represents the measurement residual vector at time k; ε k-1 represents the measurement residual vector at time k-1; ε1 represents the measurement residual vector at the initial time; ρ represents the forgetting factor, which is generally taken as 0.95; E[·] represents the mathematical expectation operation.

[0056] S402: Calculate the fading factor λ k , the formula is:

[0057]

[0058] In the formula, the current moment measurement information deviation matrix is ​​defined as Historical moment measurement information deviation matrix tr(·) is the matrix trace operation; a k is the intermediate calculated value of the fading factor.

[0059] S403: Recalculate the measurement error covariance matrix at time k+1 and the one-step prediction cross-correlation covariance matrix The formula is:

[0060]

[0061] S5: Calculate the attitude estimation state vector of the rotating projectile correction component:

[0062] S501: Calculate the filter gain matrix K k+1 , the formula is:

[0063]

[0064] S502: Calculate the state vector measurement update result The formula is:

[0065]

[0066] S503: Calculate the covariance matrix update result P k+1 And its square root expression S k+1 , the formula is:

[0067]

[0068] Example

[0069] The attitude estimation method of the rotating projectile correction component based on maximum correlation entropy and strong tracking filtering is further explained in conjunction with the following examples.

[0070] Aiming at the problem of attitude estimation of the rotating projectile correction component, the motion state of the correction component is simulated and the simulation data of attitude angle change is given. The simulation data is as follows: Figure 2 As shown, the motion state of the correction component is as follows:

[0071] (1) 0-4.8s: The correction unit is in the uncontrolled flight stage, rolling at a low speed with the missile body;

[0072] (2) 4.8s to 30s: At 14.8s, the correction component starts to control, the roll angular velocity rapidly decreases to zero, and the roll angle follows the rudder deflection command and stabilizes to -43.19°. At the same time, the yaw angle of the correction component changes accordingly.

[0073] (3) 30-60s: A step rudder deflection command is generated at 30s, and the roll angle changes rapidly to -13.56°.

[0074] The three-axis geomagnetic sensor measurement data and the gyroscope sensor measurement data are inverted based on the above simulation data. The measurement noise of the three-axis geomagnetic sensor is a zero-mean Laplace noise with a standard deviation of [100 100 100]nT, and the measurement noise of the gyroscope sensor is a zero-mean Laplace noise with a standard deviation of [0.1 1 0.1]° / s. The sensor measurement data is as follows: Figure 3 As shown. Based on the data, the traditional attitude estimation method based on square root volume Kalman filter (SCKF), the attitude estimation method based on strong tracking square root volume Kalman filter (STSCKF) and the method proposed in the present invention (MCKSTSCKF) are used for simulation comparison analysis to verify the effectiveness of the present invention. For the control variables, the comparison algorithm uses the same parameters, and the key parameters are set as:

[0075]

[0076] The error curve of attitude estimation is as follows Figure 4 shown.

[0077] from Figure 4 It can be seen that the strong tracking square root volumetric Kalman filter has the largest attitude angle estimation error and divergence trend. Compared with the traditional square root volumetric Kalman filter, it is more susceptible to interference from non-Gaussian noise and measurement wild values. The attitude estimation method of a rotating projectile correction component based on maximum correlation entropy and strong tracking filtering provided by the present invention can well alleviate the above problems. Before the strong tracking filter processes the measurement data, the maximum correlation entropy function based on the Cauchy kernel is introduced to reduce the weight of abnormal measurement data and reduce the interference of non-Gaussian measurement noise and measurement wild values. On the basis of solving the above-mentioned problems existing in the attitude estimation of the strong tracking square root volumetric Kalman filter, the strong tracking filter is fully utilized to solve the attitude estimation problem when the motion state of the carrier changes rapidly. The attitude estimation error has been significantly reduced, especially the roll angle error, which further improves the accuracy of the attitude estimation of the rotating projectile.

[0078] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention is described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for estimating the attitude of a rotating projectile correction component based on maximum correlation entropy and strong tracking filtering, characterized in that: The following steps are involved: S1: Establish a three-axis geomagnetic sensor and MEMS gyroscope combined attitude estimation system model; S2: Establish the attitude estimation equation of the rotating projectile correction component based on square root volume Kalman filter; S3: Based on the maximum correlation entropy criterion of the Cauchy kernel function, the sensor measurement noise is corrected to obtain an estimated value of the measurement noise variance matrix; S4: Based on the principle of strong tracking filtering, the measurement error covariance matrix and the one-step prediction cross-correlation covariance matrix are corrected; S5: Calculate the attitude estimation state vector of the rotating projectile correction component.

2. The method for estimating the attitude of a rotating projectile correction assembly according to claim 1, characterized in that: The S1 includes: S101: Establish system status model: X k =Φ k X k-1 +W k-1 (1) Among them, X k represents the state vector of the system at time k, X k-1 represents the state vector of the system at time k-1, Φ k represents the state transfer matrix at time k, W k-1 Represents the noise covariance matrix of the system at time k-1; S102: Establishing system measurement model: Z k =h(X k )+V k (3) Where Z k represents the measurement value at time k, h(Xk) represents the measurement transfer nonlinear function at time k, V k represents the measurement noise covariance matrix at time k.

3. The method for estimating the attitude of a rotating projectile correction assembly according to claim 2, characterized in that: The measured value Z k It is expressed as follows: In the formula, Represents the gyroscope information of the x-axis at time k, represents the gyroscope information of the y-axis at time k, Indicates the gyroscope information of the z-axis at time k; Represents the geomagnetic sensor information of the x-axis at time k, Represents the geomagnetic sensor information of the y-axis at time k, Shows the geomagnetic sensor information of the z-axis at time k.

4. The method for estimating the attitude of a rotating projectile correction assembly according to claim 1, characterized in that: The S2 includes: S201: Establishing a state prediction equation, specifically: In the formula, Represents the state estimation vector of the system at time k+1; represents the state estimation vector of the system at time k; S k represents the square root form of the covariance matrix at time k; S k+1|k represents the square root form of the prediction covariance matrix at time k+1; Q k represents the system noise variance matrix at time k; QR(·) represents the QR operation. S202: Establish a measurement estimation equation to obtain a measurement error covariance matrix and a one-step prediction cross-correlation covariance matrix.

5. The method for estimating the attitude of a rotating projectile correction assembly according to claim 4, characterized in that: The measurement estimation equation is: In the formula, ξ i is the basic volume point of the third-order volume criterion; X i,k+1∣k is the state volume point; h(X i,k+1∣k ) is the measurement transfer nonlinear calculation of the state volume point; Z i,k+1|k is the volume point for measurement; i is the volume point sequence number; represents the measured predicted value at time k+1; n represents the dimension of the measurement equation; Then the measurement error covariance matrix at time k+1 is and the one-step prediction cross-correlation covariance matrix for: Where, (g) T Represents the transpose of a vector; R k represents the measurement noise variance matrix; m represents the dimension of the state vector.

6. The method for estimating the attitude of a rotating projectile correction assembly according to claim 1, characterized in that: The S3 includes: S301: Solve the weight matrix based on the residual vector; The Cauchy kernel function is introduced, which is specifically expressed as follows: In the formula, c k represents the weight matrix; represents the Cauchy kernel function; e k represents the residual vector at time k; σ represents the width parameter of the Cauchy kernel function; diag[·] represents the function of taking the diagonal elements of the matrix; S302: Based on the calculation of the weight matrix, an estimated value of the measurement noise variance matrix is ​​obtained The calculation formula is as follows: In the formula, R k represents the measurement noise variance matrix.

7. The method for estimating the attitude of a rotating projectile correction assembly according to claim 6, characterized in that: The measurement innovation vector e at time k k The calculation formula is as follows: In the formula, e k is the measurement innovation vector at time k, π k represents the constructed state and measurement prediction residual vector, M k It represents the square root form of the constructed state covariance and measurement variance diagonal matrix, and its calculation formula is as follows: In the formula, Z k I represents the measurement vector at time k; n represents the n-dimensional identity matrix; represents the state estimation vector of the system at time k+1, represents the measured predicted value at time k+1; Z k represents the measured value at time k, S k+1|k represents the square root form of the prediction covariance matrix at time k+1, H k Represents the equivalent linear measurement transfer matrix at time k, and the calculation formula is as follows: In the formula, is the one-step prediction cross-correlation covariance matrix.

8. The method for estimating the attitude of a rotating projectile correction assembly according to claim 1, characterized in that: The S4 includes: S401: Calculate the estimated value of the variance of the residual sequence, In the formula, ε k represents the measurement residual vector at time k; ε k-1 represents the measurement residual vector at time k-1; ε1 represents the measurement residual vector at the initial time; ρ represents the forgetting factor; E[·] represents the mathematical expectation operation; S402: Calculate the fading factor λ k , In the formula, the current moment measurement information deviation matrix is ​​defined as Historical moment measurement information deviation matrix tr(·) is the matrix rank operation; a k is the intermediate calculated value of the fading factor; S403: Recalculate the measurement error covariance matrix at time k+1 and the one-step prediction cross-correlation covariance matrix The formula is:

9. The method for estimating the attitude of a rotating projectile correction assembly according to claim 1, characterized in that: The S5 includes: S501: Calculate the filter gain matrix K k+1 (Explanation), the formula is: In the formula, is the measurement error covariance matrix at time k+1, is the one-step prediction cross-correlation covariance matrix, S502: Calculate the state vector measurement update result The formula is: In the formula, is the state estimation vector of the system at time k+1, K k+1 is the filter gain matrix, Z k represents the measured value at time k, S503: Calculate the covariance matrix update result P k+1 And its square root expression S k+1 , the formula is:

Images