A novel adaptive robust capacitive Kalman filter method for spacecraft attitude estimation

Abstract

The purpose of this invention is to provide a novel adaptive robust capacitive Kalman filter method for spacecraft attitude estimation, comprising the following steps: applying the output data of star sensors and gyroscopes during spacecraft operation as measurements; selecting quaternions as attitude description parameters and establishing a nonlinear attitude estimation system model based on quaternions for star sensors and gyroscopes; establishing a nonlinear regression model based on MCC and proposing a cost function based on MCC; deriving an adaptive fading factor based on the innovation sequence and residual sequence, and correcting process noise according to the adaptive fading factor to mitigate the impact of insufficient statistical information on process noise on the system; and substituting the obtained robust filter and adaptive factor into the capacitive Kalman filter framework. This invention solves the problem of non-Gaussian noise; the Gaussian kernel function commonly used in deriving the cost function of the robust filter is replaced by the Cauchy kernel function, which can prevent the occurrence of singular matrices during algorithm operation.

Description

Technical Field

[0001] This invention relates to an attitude estimation method, specifically a novel method for spacecraft attitude estimation. Background Technology

[0002] State estimation theory has developed rapidly in recent years and has been successfully applied to fields such as spacecraft attitude estimation. To solve nonlinear filtering problems, many estimation algorithms have been proposed, among which Kalman filtering theory is one of the most widely used methods. The Extended Kalman Filter (EKF) is widely used. However, EKF has theoretical limitations. This algorithm only retains the first-order terms of the Taylor series expansion of the nonlinear function, reducing the estimation accuracy. Furthermore, the calculation of the Jacobian matrix is ​​very time-consuming, affecting the real-time performance of the system. Compared with EKF, the Unscented Kalman Filter (UKF) algorithm can achieve the second-order accuracy of the Taylor series expansion Kalman filter. However, in high-dimensional systems, the performance of UKF may not meet expectations. To overcome these problems, the Cubature Kalman Filter (CKF) is proposed. CKF is suitable for nonlinear filters with high-dimensional state estimation. Moreover, CKF has undergone rigorous mathematical derivation, resulting in more stable filtering performance.

[0003] For describing spacecraft attitude, attitude description parameters play a crucial role, such as Euler angles, Rodriguez parameters, and quaternions. Quaternions have received widespread attention due to their computational simplicity, avoidance of Euler angle singularities, and lack of trigonometric function operations. In recent years, robust filtering and adaptive filtering have been extensively studied. For non-Gaussian noise problems, Student's t-filter and Huber filter have been proposed. Furthermore, the maximum correlation criterion (MCC) has been successfully applied to enhance the robustness of filters in non-Gaussian noise environments. To address the problem of insufficient noise statistics, adaptive filtering algorithms based on real-time calculation of process noise covariance and covariance matching principles have been proposed. In addition, an adaptive filtering algorithm based on innovation sequences and residual sequences has been proposed and successfully applied to nonlinear systems, possessing advantages such as high filtering accuracy and simple implementation. Summary of the Invention

[0004] The purpose of this invention is to provide a novel adaptive robust voluminous Kalman filter method for spacecraft attitude estimation that can improve the accuracy of attitude estimation in cases of insufficient process noise statistics and non-Gaussian noise.

[0005] The object of the present invention is achieved like this:

[0006] This invention discloses a novel adaptive robust capacitive Kalman filter method for spacecraft attitude estimation, characterized by:

[0007] (1) Use the output data of star sensors and gyroscopes during spacecraft operation as a measurement;

[0008] (2) Quaternions are selected as attitude description parameters, and a nonlinear attitude estimation system model based on quaternions for star sensors and gyroscopes is established.

[0009] (3) Based on MCC, a nonlinear regression model is established, and a cost function based on MCC is proposed. Where the kernel function C δ The robust filter is derived by replacing the commonly used Gaussian kernel function with the Cauchy kernel function and based on the cost function.

[0010] (4) Derive the adaptive fading factor a based on the innovation sequence and residual sequence, and correct the process noise according to the adaptive fading factor. This helps to mitigate the impact of insufficient process noise statistics on the system.

[0011] (5) Substitute the obtained robust filter and adaptive factor into the capacitive Kalman filter framework.

[0012] The present invention may also include:

[0013] 1. In step (2), the rate integral gyroscope model is shown below:

[0014]

[0015] In the formula: ρ(t) is the measurement output of the gyroscope; ω(t) is the gyroscope drift; ω(t) is the true angular velocity; ζ v (t) and ζ u (t) represents independent white noise.

[0016] Quaternions and gyroscope drift are chosen as the state vectors. The discrete-time state equation can be written as:

[0017]

[0018] In the formula: q = [q1, q2, q3, q4] T =[ρ,q4] T The attitude quaternion is Ψ(ω); Δt is the gyroscope sampling interval; Ψ(ω) and Γ(q) are respectively... and Where ω = [ω1 ω2 ω3],

[0019] The star sensor observation equation is:

[0020]

[0021] In the formula: For measurement vectors, To represent the corresponding reference vector; Represents measurement noise; A(q) is the attitude matrix.

[0022] 2. In step (3), consider the following nonlinear system:

[0023] x k =f(x) k-1 )+w k-1

[0024] z k =h(x k )+v k

[0025] In the formula: x k ∈R n×1 and z k ∈R m×1 These are the state vector and measurement vector, respectively; f(·) and h(·) are the process function and measurement function, respectively; w k-1 and v k These are process noise and measurement noise, respectively.

[0026] Establish the following nonlinear regression model:

[0027]

[0028] In the formula: make in

[0029]

[0030] exist Multiply both sides by D k The negative first power yields:

[0031] G k =b(x k )+e k )

[0032] In the formula:

[0033] The following cost function based on MCC is proposed:

[0034]

[0035] Where: g i,k C δ The i-th element; bi (x k ) is b(x k The i-th row of ); L = m + n represents C δ The dimension of C δ Cauchy kernel function x k The optimal estimate is expressed as follows:

[0036]

[0037] formula Solve using the following equation:

[0038]

[0039] Define H i,k =C δ (e i,k ),get

[0040]

[0041] In the formula: H x,k =diag(C δ (e 1,k ),...,C δ (e n,k H z,k =diag(C δ (e n+1,k ,...,C δ (e n+m,k )), diag(·) is a diagonal matrix;

[0042] Equation Further expressed as:

[0043]

[0044] Using H in the CKF framework k To optimize measurement information, based on H k The residual covariance matrix is ​​reweighted, and the measurements are reconstructed. The updated covariance is defined as follows:

[0045]

[0046] Define the prior state covariance and the measurement noise covariance as follows:

[0047]

[0048]

[0049] 3. In step (4)

[0050] For P k|k-1 Correction was performed, and the correction strategy was as follows:

[0051]

[0052] In the formula: Here is the corrected prior covariance matrix; a is the adaptive fading factor, which satisfies:

[0053]

[0054] In the formula: H k P is the system measurement matrix; zz,k|k-1 The new information covariance matrix; To estimate the prior covariance matrix; To estimate the innovation covariance matrix, the innovation covariance matrix is ​​defined as:

[0055]

[0056]

[0057] In the formula: To obtain the corrected innovation covariance matrix, and to ensure that the innovation covariance equals the covariance directly calculated from the innovation sequence, we obtain:

[0058]

[0059] In the formula The adaptive fading factor needs to satisfy Based on the above formula, we obtain:

[0060]

[0061]

[0062] Adaptive processing of process noise covariance, Q k The adaptive processing is as follows:

[0063]

[0064] 4. Step (5) specifically includes the following steps:

[0065] Calculate the volume point:

[0066]

[0067] In the formula, m = 2n; [1] i It is the i-th column of the identity matrix;

[0068] Calculate the volumetric point state prediction, target state prediction, and prediction covariance:

[0069] β i,k|k-1 =f(X) i,k-1 ), i = 1, 2, ..., m

[0070]

[0071]

[0072] In the formula Q k-1 From the equation Calculated;

[0073] Calculate the volume point:

[0074]

[0075] Calculate volumetric measurement prediction, target measurement prediction, innovation covariance, cross-covariance, and gain:

[0076] Z i,k|k-1 =h(X) i,k|k-1 ), i = 1, 2, ..., m

[0077]

[0078]

[0079] In the formula R k From the equation Calculated;

[0080]

[0081]

[0082] Calculate the state estimate and the corresponding covariance:

[0083]

[0084]

[0085] The advantages of this invention are:

[0086] This invention derives an adaptive fading factor to automatically adjust process noise when statistical information is insufficient. The adaptive fading factor has no parameter requirements, which is beneficial for the practical application of the algorithm.

[0087] This invention addresses the problem of non-Gaussian noise by establishing a robust filter based on the maximum correlation entropy criterion. Furthermore, the Gaussian kernel function commonly used in deriving the cost function of the robust filter is replaced with the Cauchy kernel function, which prevents the emergence of singular matrices during algorithm execution and thus avoids system crashes. Attached Figure Description

[0088] Figure 1 This is a comparison chart of the roll angle error estimation results of an independent experiment between the present invention and existing filtering methods;

[0089] Figure 2 This is a comparison chart of the pitch angle error estimation results of an independent experiment between the present invention and existing filtering methods;

[0090] Figure 3 This is a comparison chart of the yaw angle error estimation results of an independent experiment between the present invention and existing filtering methods;

[0091] Figure 4 This is a flowchart of the method of the present invention. Detailed Implementation

[0092] The invention will now be described in more detail with reference to the accompanying drawings:

[0093] Combination Figure 1-4 The objective of this invention is achieved through the following components:

[0094] (1) Use the output data of star sensors and gyroscopes during spacecraft operation as a measurement;

[0095] (2) Quaternions are selected as attitude description parameters, and a nonlinear attitude estimation system model based on quaternions for star sensors and gyroscopes is established.

[0096] (3) Based on MCC, a nonlinear regression model is established, and a cost function based on MCC is proposed. Where the kernel function C δ The Cauchy kernel function is used instead of the commonly used Gaussian kernel function to prevent singular matrices from appearing during the algorithm's operation, and a robust filter is derived based on the cost function.

[0097] (4) Derive the adaptive fading factor a based on the innovation sequence and residual sequence, and correct the process noise according to the adaptive fading factor. This helps to mitigate the impact of insufficient process noise statistics on the system.

[0098] (5) Substitute the obtained robust filter and adaptive factor into the capacitive Kalman filter framework.

[0099] The novel adaptive robust capacitive Kalman filter (ACKF) method for spacecraft attitude estimation proposed in this invention was simulated using MATLAB software. Its estimation performance was compared with that of CKF, Maximum Correlation Entropy Cubic Kalman Filter (MCCKF), and Adaptive Cubic Kalman Filter (ACKF). The simulation hardware environment consisted of an Intel(R) Core(TM) i7-10750H CPU (2.60GHz, 2.59GHz) and Windows 10 operating system. Figures 1 to 3 It can be seen that the filtering accuracy of CKF and ACKF decreases significantly under conditions of insufficient statistical information on process noise and non-Gaussian noise, while the filtering accuracy of MCCKF is superior to that of CKF and ACKF. Due to the insensitivity of maximum correlation entropy to outliers, ARCKF can suppress outliers through a robust filter derived based on the maximum correlation entropy criterion, thereby achieving stronger filtering capability and stability. Specifically, the use of the Cauchy kernel function instead of the commonly used Gaussian kernel function in the cost function solves the problem of singular matrices easily occurring during algorithm operation, further enhancing the robustness of ARCKF. Furthermore, this invention uses an adaptive fading factor to correct for process noise, further improving the estimation accuracy of the algorithm.

[0100] This invention is a novel adaptive robust capacitive Kalman filter method for spacecraft attitude estimation. The process is as follows: Figure 4 The document is provided, and includes the following parts:

[0101] (i) Collect the output data of star sensors and gyroscopes during spacecraft operation and use them as measurement data;

[0102] (ii) Establish a nonlinear attitude estimation system model based on quaternions for star sensors and gyroscopes;

[0103] A commonly used rate integral gyroscope model is shown below:

[0104]

[0105] In the formula: ρ(t) is the measurement output of the gyroscope; ω(t) is the gyroscope drift; ω(t) is the true angular velocity; ζ v (t) and ζ u (t) represents independent white noise.

[0106] Quaternions and gyroscope drift are chosen as the state vectors. The discrete-time state equation can be written as:

[0107]

[0108] In the formula: q = [q1, q2, q3, q4] T =[ρ,q4] T The attitude quaternion is Ψ(ω); Δt is the gyroscope sampling interval; Ψ(ω) and Γ(q) are respectively... and Where ω = [ω1 ω2 ω3],

[0109] The star sensor observation equation is:

[0110]

[0111] In the formula: For measurement vectors, To represent the corresponding reference vector; Represents measurement noise; A(q) is the attitude matrix.

[0112] (III) To improve the robustness of the system, a robust nonlinear filter is constructed based on MCC. Consider the following nonlinear system:

[0113] x k =f(x) k-1 )+w k-1 (4)

[0114] z k =h(x k )+v k (5)

[0115] In the formula: x k ∈R n×1 and z k ∈R m×1 These are the state vector and the measurement vector, respectively. f(·) and h(·) are the process function and the measurement function, respectively. k-1 and v k These are process noise and measurement noise, respectively.

[0116] Establish the following nonlinear regression model:

[0117]

[0118] In the formula: make in

[0119]

[0120] Multiply both sides of equation (6) by D k The negative first power can be reached

[0121] G k =b(xk )+e k (8)

[0122] In the formula:

[0123] We propose the following cost function based on MCC:

[0124]

[0125] Where: g i,k C δ The i-th element; b i (x k ) is b(x k The i-th row of ); L = m + n represents C δ The dimension of C δ Cauchy kernel function x k The optimal estimate is expressed as follows:

[0126]

[0127] Formula (9) can be solved using the following equation:

[0128]

[0129] Define H i,k =C δ (e i,k ), can be obtained

[0130]

[0131] In the formula: H x,k =diag(C δ (e 1,k ),...,C δ (e n,k H z,k =diag(C δ (e n+1,k ,...,C δ (e n+m,k )), diag(·) is a diagonal matrix.

[0132] Equation (11) can be further expressed as:

[0133]

[0134] We use H in the CKF framework k To optimize measurement information. Based on H k The residual covariance matrix is ​​reweighted, and the measurements are reconstructed. The updated covariance is defined as:

[0135]

[0136] In practice, the real state of x k They are usually unknown. Based on the above inference, the prior state covariance and measurement noise covariance are defined as follows:

[0137]

[0138]

[0139] In previous applications of MCC, the Gaussian kernel function was typically chosen. Through the above derivation, it can be seen that the core difference between MCC filtering based on the Cauchy kernel function and MCC filtering based on the Gaussian kernel function lies in H... k H k It can be viewed as a weighted matrix of the adjusted state prediction and the current observation. Because the Gaussian kernel function converges to 0 exponentially, when e is large, H... k It is prone to becoming a singular matrix. In contrast, the Cauchy kernel function converges much slower than the Gaussian function, which significantly reduces H0. k The probability of becoming a singular number.

[0140] (iv) The statistics of system process noise may be unknown in practical applications, which will affect the innovation covariance matrix. The innovation covariance should be equal to the covariance calculated directly from the innovation sequence to obtain optimal performance; the deviation between them can be attributed to P. k|k-1 and R k The incorrect definition. In section three, a robust filter based on MCC is used for R. k Optimization was performed. Because P k|k-1 Includes information about Q k-1 The information, therefore, can be corrected by Q. k-1 To balance the bias between the theory and the estimated covariance of the new information.

[0141] To satisfy the above requirements for the innovation matrix, for P k|k-1 Correction was performed, and the correction strategy was as follows:

[0142]

[0143] In the formula: Here is the corrected prior covariance matrix; a is the adaptive fading factor, which satisfies:

[0144]

[0145] In the formula: H k P is the system measurement matrix; zz,k|k-1The new information covariance matrix; To estimate the prior covariance matrix; To estimate the innovation covariance matrix. The innovation covariance matrix is ​​defined as:

[0146]

[0147]

[0148] In the formula: Let be the corrected innovation covariance matrix. To ensure that the innovation covariance equals the covariance directly calculated from the innovation sequence, we can obtain:

[0149]

[0150] In the formula The adaptive fading factor needs to satisfy According to formulas (17)-(21), we can obtain:

[0151]

[0152]

[0153] Based on the above analysis, the discrepancy between the information covariances can be attributed to the process noise covariance Q. k The definition of Q is incorrect. Therefore, the process noise covariance needs to be adaptively processed. k The adaptive processing is as follows:

[0154]

[0155] (v) Substitute the obtained robust filter and adaptive factor into the capacitive Kalman filter framework.

[0156] 5.1 Prediction

[0157] Calculate the volume point:

[0158]

[0159] In the formula, m = 2n; [1] i It is the i-th column of the identity matrix.

[0160] Calculate the volumetric point state prediction, target state prediction, and prediction covariance:

[0161] β i,k|k-1 =f(X) i,k-1 ), i = 1, 2, ..., m (26)

[0162]

[0163]

[0164] In the formula Q k-1 It is calculated from equation (24).

[0165] 5.2 Update

[0166] Calculate the volume point:

[0167]

[0168] Calculate volumetric measurement prediction, target measurement prediction, innovation covariance, cross-covariance, and gain:

[0169] Z i,k|k-1 =h(X) i,k|k-1 ), i = 1, 2, ..., m (30)

[0170]

[0171]

[0172] In the formula R k It is calculated from equation (16).

[0173]

[0174]

[0175] 5.3 Estimation

[0176] Calculate the state estimate and the corresponding covariance:

[0177]

[0178]

[0179] The beneficial effects of this invention are described below:

[0180] The present invention was simulated using MATLAB under the following simulation conditions:

[0181] Simulation time: 1000s; gyroscope measurement noise and gyroscope drift noise are set to 10 times the actual values, respectively. and The gyroscope sampling period is Δt = 1s; the star sensor measurement noise is set to non-Gaussian noise v. k ~0.99N(0,(0.005°) 2 )+0.01N(0,(0.5°) 2The spacecraft's angular velocity is ω = [-1 / (90 / (2π)×60),0,0] rad / s; the initial gyroscope drift is set to... The initial attitude variance matrix is ​​set to (0.1°). 2 I 3×3 The initial gyroscope drift variance matrix is ​​set to (0.2° / h). 2 I 3×3 The kernel bandwidth of the Cauchy kernel function is δ = 5.

Claims

1. A novel adaptive robust capacitive Kalman filter method for spacecraft attitude estimation, characterized by: (1) Use the output data of star sensors and gyroscopes during spacecraft operation as a measurement; (2) Quaternions are selected as attitude description parameters, and a nonlinear attitude estimation system model based on quaternions for star sensors and gyroscopes is established. (3) Based on MCC, a nonlinear regression model is established, and a cost function based on MCC is proposed. Where the kernel function C δ The robust filter is derived by replacing the commonly used Gaussian kernel function with the Cauchy kernel function and based on the cost function. Consider the following nonlinear system: x k =f(x k-1 )+w k-1 z k =h(x k )+v k In the formula: x k ∈R n×1 and z k ∈R m×1 These are the state vector and measurement vector, respectively; f(·) and h(·) are the process function and measurement function, respectively; w k-1 and v k These are process noise and measurement noise, respectively. Establish a nonlinear regression model: In the formula: make in exist Multiply both sides by D k The negative first power yields: G k =b(x k )+e k In the formula: The following cost function based on MCC is proposed: Where: g i,k C δ The i-th element; b i (x k ) is b(x k The i-th row of ); L = m + n represents C δ The dimension of C δ Cauchy kernel function x k The optimal estimate is expressed as follows: formula Solve using the following equation: Define H i,k =C δ (e i,k ),get In the formula: H x,k =diag(C δ (e 1,k ),…,C δ (e n,k H z,k =diag(C δ (e n+1,k ,…,C δ (e n+m,k )), diag(·) is a diagonal matrix; Equation Further expressed as: Using H in the CKF framework k To optimize measurement information, based on H k The residual covariance matrix is ​​reweighted, and the measurements are reconstructed. The updated covariance is defined as follows: Define the prior state covariance and the measurement noise covariance as follows: (4) Derive the adaptive fading factor a based on the innovation sequence and residual sequence. k Process noise is corrected based on adaptive fading factor. This helps to mitigate the impact of insufficient process noise statistics on the system. (5) Substitute the obtained robust filter and adaptive factor into the capacitive Kalman filter framework.

2. The novel adaptive robust capacitive Kalman filter method for spacecraft attitude estimation according to claim 1, characterized in that: In step (2), the rate integral gyroscope model is shown below: In the formula: This is the measurement output of the gyroscope; For gyroscope drift; ω(t) is the true angular velocity; ζ v (t) and ζ u (t) represents independent white noise; Quaternions and gyroscope drift are chosen as the state vectors. The discrete-time state equation can be written as: In the formula: q = [q1, q2, q3, q4] T =[ρ,q4] T The attitude quaternion is Ψ(ω); Δt is the gyroscope sampling interval; Ψ(ω) and Γ(q) are respectively... and Where ω=[ω1ω2ω3], The star sensor observation equation is: In the formula: For measurement vectors, To represent the corresponding reference vector; Represents measurement noise; A(q) is the attitude matrix.

3. The novel adaptive robust capacitive Kalman filter method for spacecraft attitude estimation according to claim 1, characterized in that: In step (4) For P k|k-1 Correction was performed, and the correction strategy was as follows: In the formula: The corrected prior covariance matrix; a k Let be the adaptive fading factor, which satisfies: In the formula: H k P is the system measurement matrix; zz,k|k-1 The new information covariance matrix; To estimate the prior covariance matrix; To estimate the innovation covariance matrix, the innovation covariance matrix is ​​defined as: In the formula: To obtain the corrected innovation covariance matrix, and to ensure that the innovation covariance equals the covariance directly calculated from the innovation sequence, we obtain: In the formula The adaptive fading factor needs to satisfy Based on the above formula, we obtain: Adaptive processing of process noise covariance, Q k The adaptive processing is as follows:

4. A novel adaptive robust capacitive Kalman filter method for spacecraft attitude estimation according to claim 1, characterized in that: Step (5) specifically includes the following steps: Calculate the volume point: In the formula, m = 2n; [1] i It is the i-th column of the identity matrix; Calculate the volumetric point state prediction, target state prediction, and prediction covariance: β i,k|k-1 =f(X i,k-1 ),i=1,2,…,m In the formula Q k-1 From the equation Calculated; Calculate the volume point: Calculate volumetric measurement prediction, target measurement prediction, innovation covariance, cross-covariance, and gain: Z i,k|k-1 =h(X i,k|k-1 ),i=1,2,…,m In the formula R k From the equation Calculated; Calculate the state estimate and the corresponding covariance:

Images