An angular acceleration estimation method based on accelerometer correction

Abstract

The application discloses an angular acceleration estimation method based on accelerometer correction, which comprises the following steps: S1, establishing a process model of an estimated signal, including a state equation and an observation equation; S2, performing first prediction based on a nonlinear dynamic model, obtaining a first k+1 prior state estimation matrix and a prior estimation error covariance matrix; S3, performing first correction, establishing an observation equation, calculating a Kalman gain K, and updating a posteriori estimation matrix and a posteriori estimation error covariance matrix P; S4, performing second prediction; S5, performing second correction; setting the posteriori estimation matrix after the first Kalman filtering as the initial value of the second correction process, performing second correction, and completing correction; and S6, returning to step S2 and performing iterative estimation. The application has the characteristics of further reducing time delay and more accurately and effectively estimating the angular acceleration under interference.

Description

Technical Field

[0001] This invention relates to an angular velocity estimation method, and more particularly to an angular acceleration estimation method based on accelerometer correction. Background Technology

[0002] Angular acceleration estimation has always been a key engineering problem. For practical systems, angular acceleration is usually obtained through differential operations and low-pass filters or Kalman filters, but these methods have certain limitations. If there is interference noise, differential operations will amplify the noise, while other methods will introduce time delays to varying degrees.

[0003] For example, the invention patent CN110440795A, entitled "An Angular Acceleration Estimation Method Based on Kalman Filtering," discloses a technical solution that uses Kalman filtering for angular acceleration estimation to remove high-frequency noise and ensure minimal delay. While this reduces delay, a certain degree of delay still exists, which can introduce errors into some control law designs and affect accuracy. Therefore, there is an urgent need to develop an angular acceleration estimation method to further reduce time delay and improve the accuracy of angular acceleration estimation. Summary of the Invention

[0004] The purpose of this invention is to provide an angular acceleration estimation method based on accelerometer correction. This invention has the advantages of further reducing time delay and estimating angular acceleration more accurately and effectively under interference conditions.

[0005] The technical solution of this invention: an angular acceleration estimation method based on accelerometer correction, comprising the following steps:

[0006] S1. Establish a process model of the signal to be estimated, including state equations and observation equations;

[0007] S2. Based on the nonlinear dynamic model, the first prediction is made to obtain the first k+1 prior state estimation matrix. and the prior estimation error covariance matrix

[0008] S3. Perform the first correction, establish the observation equation, calculate the Kalman gain K, and update the posterior estimation matrix. And the posterior estimation error covariance matrix P;

[0009] S4. Make a second prediction;

[0010] Prediction can be made using the following formula:

[0011]

[0012] S5. Perform a second calibration;

[0013] Let the posterior estimation matrix after the first Kalman filter be... Assign the initial value to the second correction process, perform the second correction, and complete the correction:

[0014] S6. Return to step S2 and perform iterative estimation.

[0015] In the aforementioned angular acceleration estimation method based on accelerometer correction, the specific process of step S1 is as follows: The state variable is set as... Where p, q, and r are angular velocities. For angular acceleration, establish the state equation and observation equation;

[0016] The state equation is:

[0017]

[0018] The observation equation is:

[0019]

[0020] Where, x k Let z represent the system state matrix at time k. k Let A represent the observed value at time k, and let A represent the action on x. k The 6×6 state transition matrix is ​​given by H, the 6×6 state observation matrix is ​​given by ΔT, and w is the sampling period. k For process noise, v k+1 To observe noise.

[0021] In the aforementioned angular acceleration estimation method based on accelerometer correction, step S2, the process of initial estimation based on the nonlinear dynamic model, is as follows:

[0022] The k+1 prior state estimation matrix for step S1 is calculated.

[0023] Furthermore, the prior estimation error covariance matrix was calculated: in, Let P be a 6×6 prior estimation error covariance matrix. k Let Q be the 6×6 posterior estimation error covariance matrix, and let Q be the 6×6 process noise covariance matrix.

[0024] In the aforementioned angular acceleration estimation method based on accelerometer correction, the formula for calculating the Kalman gain K in step S3 is as follows:

[0025] Where R k Let be the measurement noise covariance matrix at time k;

[0026] Update the posterior estimation matrix

[0027] Update the error covariance matrix P:

[0028] In the aforementioned angular acceleration estimation method based on accelerometer correction, the specific process of the second correction in step S5 is as follows:

[0029] Let the observation equation be Where h(·) represents the mapping function from state variables to observation variables;

[0030] Let the state observation matrix H1 be:

[0031]

[0032] Kalman gain K1 is Among them, R 1k Let be the measurement noise covariance matrix at time k;

[0033] Then, update the posterior estimation matrix.

[0034] Update the error covariance matrix P1, P 1K =(I 6×6 -K1H1)P; Correction complete.

[0035] Compared with existing technologies, this invention employs a two-stage accelerometer correction method to estimate angular acceleration (using two extended Kalman filters for estimation and correction of angular acceleration to address unknown environmental noise). Simulation experiments were conducted, comparing the estimated angular acceleration results with actual angular acceleration results. The experiments demonstrate that this method can effectively estimate angular acceleration, providing a solution for engineering applications that replace angular acceleration measurement and reduce costs. Testing showed that this application can estimate angular acceleration more accurately and effectively under interference conditions, and compensates for approximately 0.1 seconds of time delay error in angular acceleration, which is significant for engineering implementation. Attached Figure Description

[0036] Figure 1 This is a schematic diagram showing two angular velocity meters symmetrical about the aircraft's center of mass;

[0037] Figure 2 This is a flowchart of the present invention;

[0038] Figure 3 This is a comparison view of the estimation results of the present invention;

[0039] Figure 4This is a magnified view of the roll acceleration. Detailed Implementation

[0040] The present invention will be further described below with reference to the accompanying drawings and embodiments, but this should not be construed as limiting the present invention.

[0041] Example. An angular acceleration estimation method based on accelerometer correction, the process is as follows: Figure 2 As shown, it mainly includes the following steps:

[0042] S1. Initialization process: Establish the process model of the signal to be estimated, with the state variables as follows: p, q, and r are angular velocities. For angular acceleration, the state equation and observation equation are:

[0043]

[0044]

[0045] Where, x k Let z represent the system state matrix at time k. k Let A represent the observed value at time k, and let A represent the action on x. k The 6×6 state transition matrix is ​​given by H, the 6×6 state observation matrix is ​​given by ΔT, and w is the sampling period. k For process noise, v k+1 To observe noise.

[0046] S2. Prediction process: Initial estimation is performed based on a nonlinear dynamic model to deduce the k+1 prior state estimate from the first step.

[0047]

[0048] Furthermore, the error covariance matrix was calculated:

[0049]

[0050] in, Let P be a 6×6 prior estimation error covariance matrix. k Let Q be the 6×6 posterior estimation error covariance matrix, and let Q be the 6×6 process noise covariance matrix.

[0051] S3. First step of the correction process: Establish the observation equation and calculate the Kalman gain K.

[0052]

[0053] Among them, R k Let be the measurement noise covariance matrix at time k.

[0054] Update the posterior estimation matrix

[0055]

[0056] Update the error covariance matrix P:

[0057]

[0058] S4, Second step of prediction process:

[0059] Let the position of the first angular velocity meter about the aircraft's center of mass be [x]. p y p ζ p That is, the position vector of the first angular velocity meter is The second angular velocity gauge indicates the aircraft's center of gravity position [-x] p -y p -z p The position vector of the second angular velocity meter is The two angular velocity meters are symmetrical about the aircraft's center of mass, as shown in the figure. Figure 1 As shown, it satisfies the following equation:

[0060]

[0061] Differentiating equation (8) with respect to time yields:

[0062]

[0063]

[0064] Equations (9) and (10) are converted into measurable overload expressions respectively:

[0065]

[0066]

[0067] Among them, C b-i Represented as the transformation matrix from the inertial coordinate system to the body coordinate system, and the gravitational acceleration vector.

[0068] Subtracting equation (12) from equation (11) yields:

[0069]

[0070] S5. Second step of the correction process:

[0071] Let the state variables after the first Kalman filter be... Assign the initial value to the second correction process, and perform the second correction step as follows:

[0072] Observation equation z k for:

[0073]

[0074] Where h(·) represents the mapping function from state variables to observation variables.

[0075] The state observation matrix H1 is:

[0076]

[0077] The Kalman gain K1 is:

[0078]

[0079] Among them, R 1k Let be the measurement noise covariance matrix at time k.

[0080] Update the posterior estimation matrix

[0081]

[0082] Update the error covariance matrix P1:

[0083] P 1k =(I 6×6 -K1H1)P (18)

[0084] The correction is now complete. Return to step S2 for iterative estimation. The flowchart is as follows: Figure 2 As shown, Figure 3 and Figure 4 The results are the estimations obtained using the method presented in this paper.

[0085] Depend on Figure 3 As can be seen from top to bottom, the first graph is the actual angular acceleration curve affected by noise, the second graph is the curve processed by the unimproved filtering algorithm, and the third graph is the curve processed by the method in this paper, where dp represents roll angular acceleration. It can be seen that using the extended Kalman filter correction can effectively estimate the angular acceleration even in the presence of external noise interference.

[0086] Figure 4 The image shows a magnified view of the roll angular acceleration (the solid line represents the actual angular acceleration curve affected by noise, the thin dashed line represents the curve processed by the method in this paper, and the thick dashed line represents the curve processed by the unimproved filtering algorithm). It can be seen that the filtering algorithm in this paper can estimate the angular acceleration more accurately and effectively under interference conditions, and it compensates for the time delay error of about 0.1 seconds for the time delay of angular acceleration, which is of certain significance for engineering implementation.

Claims

1. An angular acceleration estimation method based on accelerometer correction, characterized in that, Includes the following steps: S1. Establish a process model of the signal to be estimated, including state equations and observation equations; S2. The first prediction is made based on the nonlinear dynamic model, resulting in the first... Prior state estimation matrix and the prior estimation error covariance matrix ; S3. Perform the first correction, establish the observation equation, and calculate the Kalman gain. Update the posterior estimation matrix and the posterior estimation error covariance matrix ; S4. Make a second prediction; Prediction can be made using the following formula: ; S5. Perform a second calibration; Let the posterior estimation matrix after the first Kalman filter be... Assign the initial value to the second correction process, perform the second correction, and complete the correction: S6. Return to step S2 and perform iterative estimation; The specific process of step S1 is as follows: Set the state variable as ,in Angular velocity, For angular acceleration, establish the state equation and observation equation; The state equation is: ， The observation equation is: ， in, express The system state matrix at time t, express The observed value at time, Indicates the effect on On State transition matrix, for State observation matrix, The sampling period is For process noise, To observe noise; in step S3, Kalman gain The calculation formula is: ;in for The measurement noise covariance matrix at time point; Update the posterior estimation matrix : ; Update error covariance matrix : The specific process of the second correction in step S5 is as follows: Let the observation equation be ,in, This represents the mapping function from state variables to observation variables; Let the state observation matrix be... for: ; Kalman gain for ,in, for The measurement noise covariance matrix at time point; Then, update the posterior estimation matrix. , Update error covariance matrix , Corrections completed.

2. The angular acceleration estimation method based on accelerometer correction according to claim 1, characterized in that, In step S2, the initial estimation process based on the nonlinear dynamic model is as follows: The calculation of step S1 is obtained Prior state estimation matrix, ; Furthermore, the prior estimation error covariance matrix was calculated: ;in, for Prior estimation error covariance matrix, for Posterior estimation error covariance matrix, for Process noise covariance matrix.

Images