Quaternion filtering method based on angular velocity smoothing

Abstract

The invention relates to a quaternion filtering method based on angular velocity smoothing. The quaternion filtering method comprises the following steps: initializing an extended Kalman filter according to a first frame of effective attitude quaternion; polynomial fitting is carried out on historical N frames of angular velocity, and smoothing processing is carried out on the current frame of angular velocity by using the fitted polynomial; integrating the smoothed angular velocity into an extended Kalman filter, and predicting a current state and a covariance matrix; updating a state and a covariance matrix by using observation data; normalizing the quaternion; and repeating the steps to continuously filter the attitude quaternion of the star sensor. According to the quaternion filtering method based on angular velocity smoothing, polynomial fitting smoothing processing of historical N-frame angular velocities is introduced, the influence of star sensor measurement errors on angular velocity calculation is effectively reduced, the extended Kalman filtering technology is combined, the attitude estimation precision in a dynamic nonlinear system environment is remarkably improved, and the method is suitable for being applied to the field of attitude estimation. And a more stable and reliable attitude measurement solution is provided for the star sensor.

Description

Technical Field

[0001] The invention relates to the technical field of star sensors, and in particular to a quaternion filtering method based on angular velocity smoothing. Background Art

[0002] A star sensor is a high-precision space attitude measurement device based on stars as a reference system. It detects stars at different positions on the celestial sphere and performs calculations to provide accurate spatial orientation and reference for satellites, spacecraft and other aerospace vehicles. Star sensors have autonomous navigation capabilities and are of vital application value for the successful execution of aerospace missions.

[0003] In the attitude estimation process of star sensors, the selection and application of filtering methods are of great significance for improving the accuracy and stability of attitude measurement. Traditional star sensor filtering methods often rely on the measurement values ​​of accelerometers or gyroscopes and cannot work independently. Since there are small deviations in the angular velocity information measured by the gyroscope, these deviations will continue to accumulate after integration, resulting in large deviations in the angle measurement results. Although the angle information provided by the accelerometer is not affected by the deviation, it is greatly affected by noise and has low reliability in a short period of time.

[0004] In summary, finding a filtering method that does not rely on external devices such as gyroscopes and enables star sensors to work autonomously is of great significance for improving system autonomy, reducing power consumption, promoting miniaturization, and improving attitude estimation accuracy. Summary of the invention

[0005] The present invention aims to solve the technical problems in the prior art and provides a quaternion filtering method based on angular velocity smoothing.

[0006] In order to solve the above technical problems, the technical solutions of the present invention are as follows:

[0007] A quaternion filtering method based on angular velocity smoothing comprises the following steps:

[0008] Step 1: Initialize the extended Kalman filter according to the first frame valid attitude quaternion;

[0009] Step 2: Perform polynomial fitting on the angular velocity of the historical N frames, and use the fitted polynomial to smooth the angular velocity of the current frame; integrate the smoothed angular velocity into the extended Kalman filter to predict the current state and covariance matrix;

[0010] Step 3: Update the state and covariance matrix with observed data;

[0011] Step 4: Normalize the quaternion;

[0012] Step 5: Repeat steps 2 to 4 to continue filtering the star sensor attitude quaternion.

[0013] In the above technical solution, step one is specifically as follows:

[0014] x0 = initial quaternion;

[0015] P0=I;

[0016] I is the identity matrix.

[0017] In the above technical solution, step 2 is specifically as follows:

[0018] Step 2.1 smoothes the angular velocity of the current frame;

[0019] Step 2.2 uses the smoothed current frame angular velocity prediction state:

[0020] Step 2.3 Predict the covariance matrix.

[0021] In the above technical solution, step 2 is specifically as follows:

[0022] Step 2.1 Smooth the three-axis angular velocity of the current frame:

[0023]

[0024] Step 2.2 uses the smoothed current frame angular velocity prediction state:

[0025]

[0026] Among them, the state transfer matrix u k-1 is the angular velocity after smoothing;

[0027] Step 2.3 Predict the covariance matrix:

[0028]

[0029] in, is the state estimation covariance matrix; Q k =AQ k-1 A T , is the process noise covariance; A is the state transfer matrix.

[0030] In the above technical solution, step three is specifically as follows:

[0031] Step 3.1 Calculate the Kalman gain:

[0032]

[0033] Among them, H k is the Jacobian matrix of the observation function h to the state, H k=I (identity matrix); R is the observation noise covariance;

[0034] Step 3.2 Update status:

[0035]

[0036] Right now:

[0037]

[0038] Step 3.3 Update the covariance matrix:

[0039]

[0040] Among them, x k is the state vector, containing the quaternion estimate; z k is the observation vector, containing the quaternion estimated measurement value, h() is the observation function; K k is the Kalman gain.

[0041] In the above technical solution, step 4 is specifically as follows:

[0042] Compute the modulus of a quaternion:

[0043]

[0044] Normalized quaternion:

[0045]

[0046] Normalized quaternion Satisfies the unit quaternion conditions:

[0047]

[0048] Among them, x k The state vector before normalization, is the normalized state vector, which contains the quaternion estimate.

[0049] The present invention has the following beneficial effects:

[0050] The quaternion filtering method based on angular velocity smoothing of the present invention effectively reduces the influence of the star sensor measurement error on the angular velocity calculation by introducing the polynomial fitting smoothing processing of the historical N frames of angular velocity. Combined with the extended Kalman filtering technology, the attitude estimation accuracy in the dynamic nonlinear system environment is significantly improved, and a more stable and reliable attitude measurement solution is provided for the star sensor. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0052] Figure 1 The figure is a schematic flow chart of the steps of the quaternion filtering method based on angular velocity smoothing of the present invention. DETAILED DESCRIPTION

[0053] The inventive concept of the present invention is:

[0054] The quaternion filtering method based on angular velocity smoothing of the present invention adopts the extended Kalman filtering technology to adapt to the dynamic nonlinear system environment in which the star sensor is located. In view of the fact that the measurement error of the star sensor leads to an error in the angular velocity calculated by the inter-frame attitude quaternion thereof, the present invention innovatively introduces a method of polynomial fitting of the angular velocity of the historical N frames, thereby smoothing the angular velocity of the current frame, and taking the smoothed angular velocity as input, and integrating it into the state transfer matrix of the extended Kalman filter, thereby achieving more accurate state estimation.

[0055] The present invention is described in detail below with reference to the accompanying drawings.

[0056] A quaternion Q can be defined as follows:

[0057] Q=(q w ,q v )=q0+q1i+q2j+q3k=q w +q v (1)

[0058] Among them, q w is the real part of the quaternion Q, q v =q1i+q2j+q3k is the imaginary part of the quaternion Q, where i, j, k are imaginary units.

[0059] Kalman filtering is a recursive estimation method that is mainly divided into two stages: prediction step and update step. The specific formula for the extended Kalman filter (EKF) for quaternion state is as follows:

[0060] 1. State transfer equation (predicting system state):

[0061] x k =f(x l-1 ,u k-1 )+w k-1 (2)

[0062] in,

[0063] x k : state vector (here is a quaternion )

[0064] u k-1 : Control input, an antisymmetric matrix of angular velocity

[0065] f(x k-1 ,u k-1 ): Quaternion nonlinear state transfer function.

[0066] w k-1 ~N(0,Q): process noise, with covariance matrix Q.

[0067] The state transfer matrix A is:

[0068]

[0069] in,

[0070] I: Identity matrix.

[0071] u k-1 : The antisymmetric matrix constructed from the angular velocity is in the form of:

[0072]

[0073] T: time step.

[0074] The state prediction formula is:

[0075]

[0076] Process noise covariance matrix Q k The recursive formula is:

[0077] Q k =AQ k-1 A T (5)

[0078] 2. Observation equation:

[0079] z k =h(x k )+v k (6)

[0080] z k : The measured quaternion.

[0081] h(x k ): Observation function:

[0082] h(x k )=H k x k (7)

[0083] H k is the Jacobian matrix of the observation function h to the state. Since the sensor observation is directly a quaternion, H k =I (unit matrix).

[0084] vk ~N(0,R): observation noise, the covariance matrix is ​​R.

[0085] According to formulas (2) and (3), the construction of the state transfer matrix depends on the angular velocity information. The present invention proposes a new method, which is based on the differential equation definition of quaternions and calculates the angular velocity between frames by the change of quaternions between frames and the time step. The differential equation of quaternions can be expressed as:

[0086]

[0087] in, Represents quaternion from Q k To Q k+1 Change, Q represents Q k The quaternion when ω b From Q k To Q k+1 The angular velocity of . Then we have:

[0088]

[0089] The quaternion filtering method based on angular velocity smoothing of the present invention comprises the following specific steps:

[0090] Step 1: Initialize the extended Kalman filter according to the first frame valid attitude quaternion.

[0091] x0 = initial quaternion (10)

[0092] P0=I(covariance matrix, usually the identity matrix or given based on experience) (11)

[0093] Step 2: Perform polynomial fitting on the angular velocity of the historical N frames, and use the fitted polynomial to smooth the angular velocity of the current frame; integrate the smoothed angular velocity into the extended Kalman filter to predict the current state and covariance matrix. The current frame and its previous N frames in the above content are the historical N frames.

[0094] Step 2.1 smoothes the angular velocity of the current frame. Since the star sensor has errors in the measurement process, the angular velocity calculated based on the inter-frame attitude quaternion itself also has inaccuracy. To this end, the present invention adopts a method of polynomial fitting of the angular velocity of the historical N frames, smoothes the angular velocity of the current frame through the fitted polynomial, and transfers the smoothed angular velocity to the state transfer matrix of the Kalman filter for state estimation.

[0095] Taking the second-order polynomial fitting of the historical N-frame angular velocity as an example, the historical N-frame angular velocity can be calculated according to formula (9).

[0096] ω b(1),ω b (3),ω b (3),…,ω b (N) (12)

[0097] Fit to a second-order polynomial:

[0098] f(x)=ax 2 +bx+c (13)

[0099] Using the least squares method for fitting, the sum of squared errors is expressed as:

[0100]

[0101] Let S error Minimum, then:

[0102]

[0103] Among them, x i ∈[1,N] represents the second-order polynomial independent variable, ω b (i) represents the historical N frames of angular velocity measurement values.

[0104] According to formula (15), the coefficient matrix of the second-order polynomial is:

[0105]

[0106] Use formula (16) to calculate the angular velocity of the three axes Perform the fitting.

[0107] The obtained polynomial expression is used to smooth the three-axis angular velocity of the current frame:

[0108]

[0109] Step 2.2 uses the smoothed current frame angular velocity prediction state:

[0110]

[0111] in,

[0112] State transition matrix

[0113] u k-1 : Input the smoothed angular velocity.

[0114] Step 2.3 Predict the covariance matrix:

[0115]

[0116] Among them, Q k =AQk-1 A T .

[0117] Step 3: Update the state and covariance matrix with observed data.

[0118] Step 3.1 Calculate the Kalman gain:

[0119]

[0120] Among them, H k =I.

[0121] Step 3.2 Update status:

[0122]

[0123] Right now:

[0124]

[0125] Step 3.3 Update the covariance matrix:

[0126]

[0127] Step 4: Normalize the quaternion.

[0128] Compute the modulus of a quaternion:

[0129]

[0130] Normalized quaternion:

[0131]

[0132] Normalized quaternion Satisfies the unit quaternion conditions:

[0133]

[0134] Among them, x k is the state vector before normalization, is the normalized state vector, which contains the quaternion estimate.

[0135] Step 5: Repeat steps 2 to 4 to continue filtering the star sensor attitude quaternion.

[0136] The quaternion filtering method based on angular velocity smoothing of the present invention effectively reduces the influence of the star sensor measurement error on the angular velocity calculation by introducing the polynomial fitting smoothing processing of the historical N frames of angular velocity. Combined with the extended Kalman filtering technology, the attitude estimation accuracy in the dynamic nonlinear system environment is significantly improved, and a more stable and reliable attitude measurement solution is provided for the star sensor.

[0137] Obviously, the above embodiments are merely examples for the purpose of clear explanation, and are not intended to limit the implementation methods. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the implementation methods here. The obvious changes or modifications derived therefrom are still within the scope of protection of the invention.

Claims

1. A quaternion filtering method based on angular velocity smoothing, characterized in that: The following steps are involved: Step 1: Initialize the extended Kalman filter according to the first frame valid attitude quaternion; Step 2: Perform polynomial fitting on the angular velocity of the historical N frames, and use the fitted polynomial to smooth the angular velocity of the current frame; integrate the smoothed angular velocity into the extended Kalman filter to predict the current state and covariance matrix; Step 3: Update the state and covariance matrix with observed data; Step 4: Normalize the quaternion; Step 5: Repeat steps 2 to 4 to continue filtering the star sensor attitude quaternion.

2. The quaternion filtering method based on angular velocity smoothing according to claim 1, characterized in that: Step 1 is as follows: x0 = initial quaternion; P0=I; I is the identity matrix.

3. The quaternion filtering method based on angular velocity smoothing according to claim 1, characterized in that: Step 2 is as follows: Step 2.1 smoothes the angular velocity of the current frame; Step 2.2 uses the smoothed current frame angular velocity prediction state: Step 2.3 Predict the covariance matrix.

4. The quaternion filtering method based on angular velocity smoothing according to claim 3, characterized in that: Step 2 is as follows: Step 2.1 Smooth the three-axis angular velocity of the current frame: Step 2.2 uses the smoothed current frame angular velocity prediction state: Among them, the state transfer matrix u k-1 is the angular velocity after smoothing; Step 2.3 Predict the covariance matrix: in, is the state estimation covariance matrix; Q k =AQ k-1 A T , is the process noise covariance; A is the state transfer matrix.

5. The quaternion filtering method based on angular velocity smoothing according to claim 1, characterized in that: Step three is as follows: Step 3.1 Calculate the Kalman gain: Among them, H k is the Jacobian matrix of the observation function h to the state, H k =I; R is the observation noise covariance; Step 3.2 Update status: Right now: Step 3.3 Update the covariance matrix: Among them, x k is the state vector, containing the quaternion estimate; z k is the observation vector, containing the quaternion estimated measurement value, h() is the observation function; K k is the Kalman gain.

6. The quaternion filtering method based on angular velocity smoothing according to claim 1, characterized in that: Step 4 is as follows: Compute the modulus of a quaternion: Normalized quaternion: Normalized quaternion Satisfies the unit quaternion conditions: Among them, x k is the state vector before normalization, is the normalized state vector, which contains the quaternion estimate.

Images