Spacecraft attitude robust estimation method and system based on covariance correction

Abstract

The invention discloses a spacecraft attitude robust estimation method and system based on covariance correction, and aims to improve the estimation precision and robustness of an attitude estimation system. The attitude quaternion of the spacecraft is obtained through the star sensor, and the triaxial angular velocity is obtained through the gyroscope; establishing a spacecraft attitude estimation system state space model based on attitude quaternion; estimating the state of the next moment and the corresponding covariance by using an attitude estimation method of robust volume Kalman filtering based on covariance correction; outputting an attitude quaternion, gyroscopic drift and a corresponding error covariance when an iteration end condition is reached; and according to the attitude quaternion, the gyroscopic drift and the corresponding error covariance, realizing attitude estimation. The adaptive correction factor is adopted to adjust the prediction error covariance, and the robust filtering method is adopted to correct the measurement noise variance, so that the attitude estimation precision is improved, and the robustness of the algorithm for dynamic model structure abnormity, measurement interference and heavy tail noise is improved.

Description

Technical Field

[0001] This invention belongs to the field of attitude estimation technology, and more specifically, it relates to a robust spacecraft attitude estimation method and system based on covariance correction. Background Technology

[0002] Quaternions are widely used to describe spacecraft attitude due to their nonsingularity and low computational cost. However, spacecraft attitude estimation systems based on quaternions are nonlinear systems. Over the past few decades, the Extended Kalman Filter (EKF) has become the most widely used nonlinear filtering algorithm in attitude estimation. For the quaternion normalization constraint problem, a Kalman filter method based on additive quaternions has been proposed and applied to attitude estimation systems with good estimation results.

[0003] Attitude estimation algorithms based on EKF generally only achieve satisfactory filtering results under conditions of small initial errors, and their estimation accuracy is limited. With the development of nonlinear filtering algorithms, Unscented Kalman Filter (UKF), Cubature Kalman Filter (CKF), Sparse Gauss-Hermite Quadrature Filter (SGHQF), and Particle Filter (PF) algorithms have been proposed. Among them, SGHQF and PF algorithms have high computational costs, thus limiting their practical applications. Combining CKF with quaternions for attitude estimation systems has become a hot research topic in recent years. How to ensure quaternion normalization constraints within the CKF framework deserves in-depth research. To address this issue, quaternion normalization constraints need to be considered under the minimum variance criterion, and the filtering recursive formula, such as the filter gain, needs to be re-derived. Furthermore, the noise in the attitude estimation system has coupling characteristics, so the accuracy of state estimation also has a significant impact on state-coupled noise.

[0004] In practical spacecraft attitude estimation applications, such as on-orbit satellites, the measurement noise of their attitude sensors can be affected by the space environment, sudden changes in state, vibration or jitter, and external pressure, causing serious attitude errors. These errors or various disturbances generally cannot be directly handled by traditional filtering algorithms. Robust filtering techniques have been proven to effectively resist these disturbances, ensuring the robustness of the algorithm and the filtering accuracy. However, traditional correction techniques based on covariance dilation and other methods require repeated adjustments of the correction factor. Therefore, based on the above problems, researching a filtering method is of significant theoretical and practical importance for improving the estimation accuracy and robustness of spacecraft attitude estimation systems. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the prior art and, in order to improve the estimation accuracy and robustness of the attitude estimation system, to propose a spacecraft attitude estimation method and its system, storage medium, and computing device. Specifically, it is a robust cubic Kalman filter for attitude estimation based on covariance correction (RCKFCC).

[0006] In a first aspect, the present invention provides a spacecraft attitude estimation method, comprising:

[0007] Step 1: Obtain the attitude quaternion using a star sensor and the three-axis angular velocity using a gyroscope;

[0008] Step 2: Establish a state-space model of the spacecraft attitude estimation system based on attitude quaternions;

[0009] Step 3: At time k, estimate the state and corresponding covariance at time k+1 using the robust capacitive Kalman filter attitude estimation method based on covariance correction;

[0010] Step 4: Set the filtering time to N filter If k <N filter Then repeat step 3, if k = N filter If the filtering ends, the output will be the estimated attitude quaternion, gyroscope drift, and corresponding error covariance.

[0011] Step 5: Achieve attitude estimation based on attitude quaternions, gyroscope drift, and the corresponding error covariance.

[0012] Preferably, step 3 includes:

[0013] Step 3.1, Time Update;

[0014] Step 3.2, Robust Correction:

[0015] Define T k =chol(R) k ), chole(·) denotes the Choleski decomposition, defining a vector T represents k The inverse of this is introduced by the cost function based on IGG (Institute of Geodesy and Geophysics):

[0016]

[0017] In the formula, k0, k1, and k2 are all adjustment parameters that are greater than zero, and e j Let j be the j-th element of vector e, where j = 1, ..., l, and l is the measurement vector z. k The dimension of the correction matrix C is defined. k =diag[ψ IGG (e1),…,ψ IGG (e l If diag[·] represents a diagonal matrix with diagonal elements of ·, then the measurement covariance R0 is given by the formula. k Revised to:

[0018]

[0019] Step 3.3, Measurement Update, including:

[0020] Step 3.3.1: One-step measurement of the predicted variance P zz,k+1|k and cross-covariance P xz,k+1|k The calculation is as follows:

[0021]

[0022]

[0023] Step 3.3.2, the filtering gain based on attitude quaternion constraints is:

[0024]

[0025] In the formula, Represents the cross-covariance P xz,k+1|k It only contains the quaternion part. Represents the cross-covariance P xz,k+1|k It only contains the part about gyroscope drift. It is a Lagrange multiplicative factor. This represents the attitude quaternion part of the one-step state prediction;

[0026] Step 3.3.3: Calculate the state estimate and corresponding covariance at time k+1:

[0027]

[0028]

[0029] Step 3.3.4: Calculate the prediction error covariance correction factor:

[0030]

[0031] The prediction error covariance is corrected using the aforementioned correction factor.

[0032] In a second aspect, the present invention provides a spacecraft attitude estimation system for implementing the method, comprising:

[0033] The data acquisition module obtains the spacecraft's attitude quaternion through a star sensor and the three-axis angular velocity through a gyroscope;

[0034] The modeling module establishes a state-space model of the spacecraft attitude estimation system based on attitude quaternions;

[0035] The calculation module estimates the state and corresponding covariance of the next moment based on the state space model of the spacecraft attitude estimation system based on attitude quaternions; until the maximum iteration time is reached, it outputs the attitude quaternion, gyroscope drift and corresponding error covariance.

[0036] The attitude estimation module performs attitude estimation based on attitude quaternions, gyroscope drift, and the corresponding error covariance.

[0037] Thirdly, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed in a computer, causes the computer to perform the method described thereon.

[0038] Fourthly, the present invention provides a computing device, including a memory and a processor, wherein the memory stores executable code, and the processor executes the executable code to implement the method described above.

[0039] The beneficial effects of this invention are:

[0040] (1) The present invention uses an adaptive correction factor to adjust the prediction error covariance. When the state estimation accuracy decreases due to external interference, such as measurement anomalies, dynamic model structure anomalies and state abrupt changes, the influence of interference can be appropriately suppressed, thereby improving the stability and estimation accuracy of the algorithm.

[0041] (2) The present invention uses a robust filtering method to correct the measurement noise variance. The filter gain is adjusted based on the measurement environment from the perspective of the filter structure, which not only improves the accuracy of attitude estimation but also improves the robustness of the algorithm against measurement interference and heavy tail noise. Attached Figure Description

[0042] Figure 1 This is a comparison chart of the root mean square error results of all filtered attitude angle estimates under the initial condition error case 1.

[0043] Figure 2This is a comparison chart of the attitude angle error results in three directions: AQCKF, RCKF-Huber (Robust Cubature KalmanFilter based on Huber filter), and RCKFCC, under the initial condition error scenario 1.

[0044] Figure 3 This is a comparison chart of the root mean square error results of all filtered attitude angle estimates under the initial condition error case 2;

[0045] Figure 4 This is a comparison chart of the attitude angle error results in the three directions of AQCKF, RCKF-Huber, and RCKFCC under the initial condition error in scenario 2;

[0046] Figure 5 This is a flowchart of the method of the present invention. Detailed Implementation

[0047] The present invention will be further described below with reference to embodiments. The description of the embodiments below is only for the purpose of helping to understand the present invention. It should be noted that those skilled in the art can make several modifications to the present invention without departing from the principle of the present invention, and these improvements and modifications also fall within the protection scope of the claims of the present invention.

[0048] As one embodiment, the present invention provides a robust capacitive Kalman filter spacecraft attitude estimation method based on covariance correction, such as... Figure 5 As shown, it includes:

[0049] Step 1: Obtain the attitude quaternion using a star sensor and the three-axis angular velocity using a gyroscope;

[0050] Step 2: Establish a state-space model of the spacecraft attitude estimation system based on attitude quaternions;

[0051] Step 3: At time k, estimate the state and corresponding covariance at time k+1 using the robust capacitive Kalman filter attitude estimation method based on covariance correction;

[0052] Step 4: Set the filtering time to N filter If k <N filter Then repeat step three, if k = N filter If the filtering ends, the output will be the estimated attitude quaternion, gyroscope drift, and corresponding error covariance.

[0053] Step 5: Achieve attitude estimation based on attitude quaternions, gyroscope drift, and the corresponding error covariance.

[0054] Step 2 includes:

[0055] Step 2.1: Establish the spacecraft discrete-time state equation based on attitude quaternions:

[0056]

[0057] In the formula, the state quantity q k+1 ,β k+1 These are the attitude quaternion and the gyroscope drift, respectively, with the quaternion q = [q1 q2 q3 q4]. T q1, q2, and q3 are the vector elements of the attitude quaternion, and q4 is the scalar part of the attitude quaternion;

[0058] For state-coupled noise; η v η u These are gyroscope measurement noise and gyroscope random drift noise, respectively, both of which are zero-mean Gaussian white noise, with variances of respectively. This represents the measurement output of the gyroscope, where Δt is the sampling time of the gyroscope. ω=[ω1 ω2 ω3] T This represents the angular velocity output vector of the gyroscope's three axes. ρ = [q1 q2 q3] T The vector part of the attitude quaternion, [ω×] and [ρ×] are the antisymmetric matrices of vectors ω and ρ, respectively, and are represented as follows: I 3×3 It is a 3×3 identity matrix, where k represents time and ω T Let ||·|| denote the transpose of ω, ||·|| denotes the norm of the vector, and 0 represents the norm of the vector. 4×3 This represents a zero matrix with dimensions 4×3;

[0059] Step 2.2: Establish an observation model based on a star sensor:

[0060]

[0061] In the formula, z k Let z be the observation vector at time k. i This represents the i-th measurement vector, where i = 1, ..., m, and m is the number of stars observed by the star sensor; v k The noise is zero-mean Gaussian white noise, and its measurement covariance is R. k , Where r i Let A(q) be the corresponding reference vector, and let A(q) denote the attitude matrix, defined as follows:

[0062]

[0063] Step 3 includes:

[0064] Step 3.1, Time Update, including:

[0065] Step 3.1.1: Initialization, k = 1, correction factor τ = 1;

[0066] Corrected prediction error covariance P k+1|k :

[0067]

[0068] Step 3.1.2: Calculate the volume point at time k:

[0069]

[0070] In the formula, n is the dimension of the state variables, and matrix S k Covariance at time k Obtained through decomposition by Cholesky, namely For the i-th volume point, [1] i Let i be the i-th element in the point set consisting of n-dimensional unit vectors. Represents the state estimate at time k;

[0071] Step 3.1.3: Transfer the volume point through a state nonlinear function:

[0072] γ i,k+1|k =f(χ) i ), i = 1, 2, ..., 2n

[0073] Step 3.1.4: Calculate the one-step state prediction and prediction covariance:

[0074]

[0075]

[0076] In the formula, For system noise w k The variance of , where trace represents finding the trace of the matrix. Let P be the variance matrix at time k. k The corresponding part of the attitude quaternion. This represents the attitude quaternion estimate at time k;

[0077] Step 3.1.5: Calculate the new volume point based on the one-step state prediction and prediction covariance:

[0078]

[0079] In the formula, ξ i Indicates the volume point;

[0080] Step 3.1.6: Transfer the above volume points through a measurement nonlinear function:

[0081] γ i,k+1 =h(χ i,k+1|k ), i = 1, 2, ..., 2n

[0082] Step 3.1.7: Calculate the one-step measurement prediction

[0083]

[0084] Step 3.2, Robust Correction:

[0085] Define T k =chol(R) k ), chole(·) denotes the Choleski decomposition, defining a vector T represents k The reverse, To represent measurement prediction, the cost function based on IGG is introduced as follows:

[0086]

[0087] In the formula, k0, k1, and k2 are all adjustment parameters that are greater than zero, and e j Let j be the j-th element of vector e, where j = 1, ..., l, and l is the measurement vector z. k The dimension of the correction matrix C is defined. k =diag[ψ IGG (e1),…,ψ IGG (e l If diag[·] represents a diagonal matrix with diagonal elements of ·, then the measurement covariance R0 is given by the formula. k Revised to:

[0088]

[0089] Step 3.3, Measurement Update, including:

[0090] Step 3.3.1: One-step measurement of the predicted variance P zz,k+1|k and cross-covariance P xz,k+1|k The calculation is as follows:

[0091]

[0092]

[0093] Step 3.3.2, the filtering gain based on attitude quaternion constraints is:

[0094]

[0095] In the formula, Represents the cross-covariance P xz,k+1|k It only contains the quaternion part. Represents the cross-covariance P xz,k+1|k It only contains the part about gyroscope drift. The Lagrange multiplicative factor is used to measure the residuals. This represents the attitude quaternion part of the one-step state prediction;

[0096] Step 3.3.3: Calculate the state estimate and corresponding covariance at time k+1:

[0097]

[0098]

[0099] Step 3.3.4: Calculate the prediction error covariance correction factor:

[0100]

[0101] The method provided by this invention is simulated using Matlab software under the following simulation conditions:

[0102] Angular velocity in step 2 The star sensor has a field of view of 6° × 6°, a sampling frequency of 1 Hz, and a measurement noise standard deviation of σ. s =0.005°, the sampling time of the gyroscope is the sampling frequency Δt = 1s, and the standard deviation of the gyroscope measurement noise is... The standard deviation of gyroscope drift noise is The initial conditions can be divided into the following two cases:

[0103] Scenario 1: Initial attitude error is [-50° 50° 120°] T The initial attitude covariance is (50°I) 3×3 ) 2 The initial gyroscope drift is [0°20°0°]. T The initial gyroscope drift covariance is

[0104] Case 2: Based on Case 1, when time 1790s ≤ k ≤ 1810s, R k Replace with 50R k When time k = 16s, 1360s, and 2750s, the disturbances in the gyroscope measurement model are:

[0105] Δω(t)=0.05(sin(2πt / 150)+0.05cos(2πt / 150))×[1,-1,1] T rad / s.

[0106] In step 3, the adjustment parameters are k0 = 1.5, k1 = 2.5, and k2 = 0.001.

[0107] In step 4, the filtering time N filter =3600s.

[0108] The performance of the method (RCKFCC) provided in this invention is compared with that of existing nonlinear filtering algorithms (including EKF, USQUE, AQCKF, and RCKF-Huber algorithm). The simulation hardware environment is an Intel(R) Core(TM) i9-12900K 3.20GHz, 64.00GB RAM. Figures 1 to 4 In the diagram, the dotted line (-) represents the root mean square error of the attitude angle estimated by EKF, the dashed line (-.) represents the root mean square error of the attitude angle estimated by USQUE, the dashed line (--) represents the root mean square error of the attitude angle estimated by Additive Quaternion Cubature Kalman Filter (AQCKF), the dashed line (:) represents the root mean square error of the attitude angle estimated by RCKF-Huber, and the solid line (-o) represents RCKFCC, i.e., the root mean square error of the attitude angle estimated by the algorithm proposed in this invention; from Figure 1 and Figure 3 It can be observed that EKF exhibits divergence and fluctuation under conditions of large initial condition errors and external disturbances, while USQUE, although converging, has low estimation accuracy. In contrast, AQCKF, RCK-Huber, and RCKFCC provide more ideal estimation results, but RCKFCC has faster convergence speed and higher estimation accuracy. Figure 2 and Figure 4 It becomes more apparent that RCKFCC exhibits better robustness and higher estimation accuracy than AQCKF and RCKF-Huber in the face of large initial angle errors and external disturbances. This demonstrates the effectiveness of the method proposed in this invention.

Claims

1. A spacecraft attitude estimation method, comprising: Step 1: Obtain the spacecraft's attitude quaternion using a star sensor and obtain the three-axis angular velocity using a gyroscope; Step 2: Establish a state-space model of the spacecraft attitude estimation system based on attitude quaternions; Step 3: Estimate the state and corresponding covariance at time k+1; Step 4, if k <N filter Then repeat step 3, if k = N filter The filtering process ends, and the output includes the estimated attitude quaternion, gyroscope drift, and corresponding error covariance; N filter Indicates the maximum iteration time; Step 5: Based on the attitude quaternion, gyroscope drift, and corresponding error covariance, achieve attitude estimation; The key feature is that step 3 uses a robust capacitive Kalman filter attitude estimation method based on covariance correction to estimate the state and corresponding covariance at time k+1 at time k.

2. The method according to claim 1, characterized in that, Step 2 includes: Step 2.1: Establish the spacecraft discrete-time state equation based on attitude quaternions: In the formula, the state quantity q k+1 ,β k+1 Let q be the attitude quaternion and gyroscope drift at time k+1, respectively. The attitude quaternion q = [q1 q2 q3 q4]. T The superscript T indicates transpose, q1, q2, and q3 are the vector elements of the attitude quaternion, and q4 is the scalar element of the attitude quaternion. For state-coupled noise, η v η u These are gyroscope measurement noise and gyroscope random drift noise, respectively, both of which are zero-mean Gaussian white noise, with variances of respectively. in This represents the measurement output of the gyroscope, where Δt is the sampling time of the gyroscope. ω=[ω1ω2 ω3] T This represents the angular velocity output vector of the gyroscope's three axes. ρ = [q1 q2 q3] T The vector part of the attitude quaternion, [ω×] and [θ×] are the antisymmetric matrices of vectors ω and θ, respectively, represented as... I 3×3 It is a 3×3 identity matrix, ω T Let ||·|| denote the transpose of ω, and ||·|| denote the norm of the vector; 0 4×3 This represents a zero matrix with dimensions 4×3; Step 2.2: Establish an observation model based on a star sensor: In the formula, z k Let z be the observation vector at time k. i This represents the i-th measurement vector, where i = 1, ..., m, and m is the number of stars observed by the star sensor; v k The noise is zero-mean Gaussian white noise, and the measurement covariance is R. k , Where r i Let A(q) be the corresponding reference vector, and let A(q) denote the attitude matrix, defined as follows:

3. The method according to claim 2, characterized in that, Step 3 includes: Step 3.1, Time Update; Step 3.2, Robust Correction: Define T k =chol(R) k ), chole(·) denotes the Choleski decomposition, defining a vector T represents k The reverse, To represent measurement prediction, the cost function based on IGG is introduced as follows: In the formula, k0, k1, and k2 are all adjustment parameters that are greater than zero, and e j Let z be the j-th element of vector e, where j = 1, ..., l, and l is the measurement vector z. k dimensionality; Define the correction matrix C k =diag[ψ IGG (e1),…,ψ IGG (e l If diag[·] represents a diagonal matrix with diagonal elements of ·, then the measurement covariance R0 is given by the formula. k Revised to: In the formula, This represents the corrected measurement covariance; Step 3.3, Measurement Update, including: Step 3.3.1: One-step measurement of the predicted variance P zz,k+1|k and cross-covariance P xz,k+1|k The calculation is as follows: Where n represents the dimension of the state variables, γ i,k+1|k χ represents the nonlinear state transfer function. i,k+1|k Indicates the new volume point. Indicates the state prediction quantity; Step 3.3.2, the filtering gain based on attitude quaternion constraints is: In the formula, Represents the cross-covariance P xz,k+1|k It only contains the attitude quaternion part. Represents the cross-covariance P xz,k+1|k It only contains the part about gyroscope drift. r is the Lagrange multiplicative factor. k+1|k Indicates measurement residuals, This represents the attitude quaternion part of the one-step state prediction; Step 3.3.3: Calculate the state estimate at time k+1. and the corresponding covariance P k+1 : Where K k+1 P represents the filter gain. k+1|k Indicates the predicted covariance; Step 3.3.4: Update the prediction error covariance correction factor τ:

4. The method according to claim 3, characterized in that, Step 3.1 includes: Step 3.1.1: Initialization, k = 1, correction factor τ = 1; Corrected prediction error covariance: Step 3.1.2: Calculate the volume point at time k: In the formula, n is the dimension of the state variables, and matrix S k Covariance at time k Obtained through decomposition by Cholesky, namely For the i-th volume point, [1] i Let i be the i-th element in the point set consisting of n-dimensional unit vectors. Represents the state estimate at time k; Step 3.1.3: Transfer the volume point through the state nonlinear function to obtain the nonlinear transfer function γ. i,k+1|k : γ i,k+1|k = f(χ i ), i = 1, 2, …, 2n Equation (15) Step 3.1.4: Calculate the one-step state prediction. And the predicted covariance P k+1|k : In the formula, State-coupled noise w k The variance of , where trace represents finding the trace of the matrix. Let P be the variance matrix at time k. k The corresponding part of the attitude quaternion. This represents the attitude quaternion estimate at time k; Step 3.1.5: Calculate the new volume point χ based on the one-step state prediction and prediction covariance. i,k+1|k : In the formula, ξ i Indicates the volume point; Step 3.1.6: Calculate the new volume point x. i,k+1|k After measuring the nonlinear function transfer: γ i,k+1 =h(x i,k+1|k ), i = 1, 2, ..., 2n Equation (19) In the formula, γ i,k+1 This represents the measurement nonlinear transfer function; Step 3.1.7: Calculate the one-step measurement prediction 5. A spacecraft attitude estimation system implementing the method of any one of claims 1-4, characterized in that... include: The data acquisition module obtains the spacecraft's attitude quaternion through a star sensor and the three-axis angular velocity through a gyroscope; The modeling module establishes a state-space model of the spacecraft attitude estimation system based on attitude quaternions; The calculation module estimates the state and corresponding covariance of the next moment based on the state-space model of the spacecraft attitude estimation system based on attitude quaternions. Until the maximum iteration time is reached, the attitude quaternion, gyroscope drift, and corresponding error covariance are output. The attitude estimation module performs attitude estimation based on attitude quaternions, gyroscope drift, and the corresponding error covariance.

6. A computer-readable storage medium having a computer program stored thereon, which, when executed in a computer, causes the computer to perform the method of any one of claims 1-4.

7. A computing device comprising a memory and a processor, wherein the memory stores executable code, and the processor, when executing the executable code, implements the method of any one of claims 1-4.

Images