Navigation positioning method and system of a spacecraft

Abstract

The embodiment of the application provides a kind of navigation positioning method and system of spacecraft, the method comprises: for spacecraft autonomous navigation system, construct spacecraft autonomous navigation nonlinear system model, nonlinear system model includes state equation and measurement equation;Construct quasi-uniform sequential extended Kalman filtering algorithm;Using quasi-uniform sequential extended Kalman filtering algorithm, based on the state equation, the state estimation value of the last time of spacecraft is used to predict the state prediction value of current time, based on the measurement equation, the state measurement value of current time of spacecraft is obtained by the state prediction value of current time;Wherein, in the filtering phase in the process of estimating the state of current time, using sequential correction processing mode, the components of the state measurement value of current time are divided into multiple groups, and the state estimation value of current time of spacecraft is obtained by combining the state prediction value of current time. The autonomous positioning navigation precision of spacecraft is improved.

Description

Technical Field

[0001] This invention relates to the field of spacecraft positioning, specifically to a spacecraft navigation and positioning method and system. Background Technology

[0002] In solving the nonlinear system of a spacecraft's autonomous navigation system, the Extended Kalman Filter (EKF) is commonly used. However, the EKF has the following technical problems: 1. Accumulated truncation errors lead to inconsistent filter estimates: The consistency of parameter estimation methods is a fundamental criterion for evaluating their performance. Consistency refers to the consistency of a parameter X... k The estimation method is unbiased: Consistency analysis can determine the limits of filter estimation errors, and quasi-consistency can ensure that filter calculation errors are controlled within an acceptable range, providing a guarantee of error boundaries for the system and maintaining system stability. However, when EKF estimates the system state, the nonlinear function is linearized at the current state estimate. in, For state estimation error, for Infinitesimals of the same order. EKF does not consider [these] during linearization. That is, direct command This can lead to the accumulation of truncation errors, resulting in decreased estimation accuracy. The metric for measuring estimation accuracy is generally the mean square error (MSE). In the formula, the variance of the estimated value is reflected by the error covariance matrix calculated by the filter. When the deviation of the estimate is zero, the error covariance matrix is ​​the MSE of the estimate, and the estimate is called an unbiased estimate, also known as a consistent estimate. When there is a truncation error, the deviation of the estimate is not zero, therefore EKF is a biased filter, and thus an unbiased filter needs to be constructed. 2. Large matrix inversion leads to a significant increase in computational complexity: The core of EKF is based on the calculation of Kalman gain, but this calculation involves large matrix inversion operations. The computational complexity of inversion is O(n^2). 3 Here, n is the dimension of the state variables. High-dimensional state spaces significantly increase the computational burden of the system. While higher-order EKFs are used to reduce the impact of truncation errors caused by EKF linearization on the estimation results, this leads to a significant increase in computational complexity, making the computational load non-negligible. Therefore, lightweight filters are needed to improve their computational efficiency. Summary of the Invention

[0003] This invention provides a navigation and positioning method and system for spacecraft, which can solve one of the above-mentioned technical problems in the prior art.

[0004] To achieve the above objectives, in one aspect, embodiments of the present invention provide a navigation and positioning method for a spacecraft, comprising:

[0005] For spacecraft autonomous navigation systems, a nonlinear system model for spacecraft autonomous navigation is constructed. The nonlinear system model includes state equations and measurement equations. Process noise exists in the state equations, and observation noise exists in the measurement equations.

[0006] Construct a quasi-consistent sequential extended Kalman filter algorithm;

[0007] A quasi-consistent sequential extended Kalman filter algorithm is adopted. Based on the state equation, the predicted state value of the spacecraft at the current moment is predicted using the state estimate value of the spacecraft at the previous moment. Based on the measurement equation, the measured state value of the spacecraft at the current moment is obtained using the predicted state value of the current moment. In the filtering stage of the process of estimating the state at the current moment, a sequential correction processing method is used to divide the components of the measured state value at the current moment into multiple groups, and combined with the predicted state value at the current moment, the estimated state value of the spacecraft at the current moment is obtained.

[0008] On the other hand, embodiments of the present invention provide a navigation and positioning system for a spacecraft, comprising:

[0009] The model building unit is used to build a nonlinear system model for the autonomous navigation system of a spacecraft. The nonlinear system model includes state equations and measurement equations. Process noise exists in the state equations and observation noise exists in the measurement equations.

[0010] Algorithm building unit, used to construct a quasi-consistent sequential extended Kalman filter algorithm;

[0011] The solution unit employs a quasi-consistent sequential extended Kalman filter algorithm to predict the current state value based on the state equation using the state estimate of the spacecraft at the previous moment, and to obtain the current state measurement value of the spacecraft based on the measurement equation using the current state prediction value. Specifically, during the filtering stage of estimating the current state, a sequential correction process is used to divide the components of the current state measurement value into multiple groups, and these groups are combined with the current state prediction value to obtain the current state estimate of the spacecraft.

[0012] The above technical solution has the following beneficial effects: The quasi-consistent sequential extended Kalman filter algorithm can determine the upper bound of the unknown true estimation error matrix by calculating the error covariance matrix, thereby determining the boundary of the estimation error in real time and effectively improving the algorithm accuracy. Because the quasi-consistent sequential extended Kalman filter algorithm has quasi-consistency, it can determine the boundary of the estimation error in real time, thus effectively improving the algorithm accuracy and the autonomous positioning and navigation accuracy of spacecraft. Attached Figure Description

[0013] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0014] Figure 1 This is a flowchart of a spacecraft navigation and positioning method according to an embodiment of the present invention; Figure 2 This is a structural diagram of a spacecraft navigation and positioning system according to an embodiment of the present invention; Figure 3 This refers to the actual state versus the estimated state in an embodiment of the present invention. Figure 4 The relative position estimation error SEKF vs QCSEKF in this embodiment of the invention; Figure 5 This is an embodiment of the present invention, RMSE SEKF vs QCSEKF; Figure 6 This refers to SEKF vs QCSEKF in the embodiments of the present invention; Figure 7 This is a schematic diagram of the coordinate system and observation in an embodiment of the present invention; Figure 8 This is the relative position estimation error (ΔX(0)=ΔX1(0)) in the embodiments of the present invention; Figure 9 This is the relative position estimation error (ΔX(0)=ΔX2(0)) in the embodiment of the present invention; Figure 10 The root mean square error (ΔX(0)=ΔX1(0)) of SEKF and QCSEKF in the embodiments of the present invention; Figure 11 The root mean square error (ΔX(0)=ΔX2(0)) of SEKF and QCSEKF in the embodiments of the present invention; Figure 12 This is SEKF vs QCSEKF (ΔX(0)=ΔX1(0)) in an embodiment of the present invention; Figure 13 This is SEKF vs QCSEKF (ΔX(0)=ΔX2(0)) in an embodiment of the present invention; Figure 14 This is SEKF vs QCSEKF (ΔX(0)=ΔX1(0)) in an embodiment of the present invention; Figure 15 This is SEKF vs QCSEKF (ΔX(0)=ΔX2(0)) in an embodiment of the present invention. Detailed Implementation

[0015] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0016] like Figure 1 As shown, in conjunction with embodiments of the present invention, a navigation and positioning method for a spacecraft is provided, comprising:

[0017] S101: For spacecraft autonomous navigation systems, construct a nonlinear system model for spacecraft autonomous navigation. The nonlinear system model includes state equations and measurement equations. Process noise exists in the state equations, and observation noise exists in the measurement equations.

[0018] S102: Construct a quasi-consistent sequential extended Kalman filter algorithm;

[0019] S103: A quasi-consistent sequential extended Kalman filter algorithm is adopted. Based on the state equation, the state prediction value of the spacecraft at the current moment is predicted using the state estimate value of the spacecraft at the previous moment. Based on the measurement equation, the state measurement value of the spacecraft at the current moment is obtained using the state prediction value of the current moment. In the filtering stage of the process of estimating the state at the current moment, a sequential correction processing method is adopted to divide the components of the state measurement value at the current moment into multiple groups, and combined with the state prediction value at the current moment, the state estimate value of the spacecraft at the current moment is obtained.

[0020] like Figure 2 As shown, in conjunction with embodiments of the present invention, a navigation and positioning system for a spacecraft is provided, comprising:

[0021] Model building unit 21 is used to build a nonlinear system model for the autonomous navigation system of a spacecraft. The nonlinear system model includes state equations and measurement equations. Process noise exists in the state equations and observation noise exists in the measurement equations.

[0022] Algorithm building unit 22 is used to build a quasi-consistent sequential extended Kalman filter algorithm;

[0023] The solving unit 23 is used to employ a quasi-consistent sequential extended Kalman filter algorithm to predict the state prediction value of the spacecraft at the current moment based on the state equation using the state estimate value of the spacecraft at the previous moment, and to obtain the state measurement value of the spacecraft at the current moment based on the state prediction value of the current moment using the measurement equation. In the filtering stage of the process of estimating the state at the current moment, a sequential correction processing method is used to divide the components of the state measurement value at the current moment into multiple groups, and combined with the state prediction value at the current moment, the state estimate value of the spacecraft at the current moment is obtained.

[0024] The technical solutions of the present invention will be described in detail below with reference to specific application examples. For technical details not described in the implementation process, please refer to the relevant descriptions above.

[0025] I. Constructing a Nonlinear System Model

[0026] For spacecraft autonomous navigation systems, a nonlinear system model for spacecraft autonomous navigation is constructed. The nonlinear system model includes state equations and measurement equations, including:

[0027] A nonlinear system model for spacecraft autonomous navigation is constructed. The nonlinear system model is as follows:

[0028]

[0029] k represents the current time, and k-1 represents the previous time. X k-1 , Let X be the state vector, where X represents the spacecraft's position and velocity. k X represents the true state value at the current moment. k-1 This represents the true value of the state at the previous moment. Let f(·) be the observation vector, representing the spacecraft's state measurement value at the current moment, obtained from measurements of the spacecraft's own state using its onboard optical sensors. The state equation f(·) and measurement equation h(·) are nonlinear functions, and the process noise w is... k-1 It has zero mean and a covariance matrix of Q. k Time-invariant white Gaussian noise, observation noise v k It has zero mean and a covariance matrix of R. k The time-invariant white Gaussian noise is used. Since Q and R are set to be constant, the subscript k can be removed.

[0030] The state equation is the equation of motion for a spacecraft, while the measurement equation is the equation obtained by measuring the spacecraft's state using its onboard equipment. Because both the measured state value and the predicted state value at the current moment are inaccurate, filtering is required to solve them.

[0031] The above-mentioned technical means are also specific implementation methods of the model building unit 21.

[0032] Next, a quasi-consistent sequential extended Kalman filter algorithm is constructed, including: based on the sequential extended Kalman filter algorithm, a quasi-consistent sequential extended Kalman filter algorithm with consistency is constructed based on the sequential extended Kalman filter algorithm.

[0033] The above-mentioned technical means are also specific implementation methods of the algorithm construction unit 22.

[0034] II. Based on the calculation process of the Sequential Extended Kalman Filter (SEKF) algorithm, a consistency analysis of the SEKF algorithm is performed.

[0035] When solving for the current state estimate of the spacecraft, in order to avoid directly inverting the Kalman gain of the Extended Kalman Filter (EKF), a sequential algorithm is used to group the correction segments of the EKF, thereby transforming the inversion of a high-order matrix into the inversion of a low-order matrix, or even into the reciprocal of a real number, effectively reducing the computational load.

[0036] Based on the sequential extended Kalman filter algorithm, a quasi-consistent sequential extended Kalman filter algorithm with consistency is constructed. The steps for constructing the sequential extended Kalman filter algorithm are as follows:

[0037] First, under the sequential extended Kalman filter algorithm, the prior state estimate is set. The initial value X0 and the error covariance matrix P k The initial value is P0, and the process noise is set to w. k The covariance matrix Q k ;

[0038] Subsequently, the time update phase begins, where the state at the previous time step is predicted based on the state equation to obtain the prior estimate for the current time step. and its error covariance matrix P kk-1 The sequential correction method of the sequential extended Kalman filter algorithm is used to correct the state measurement Z. k Update sequentially, Z k For including z i ,i=1,2,…,m are the state measurements of their corresponding columns;

[0039] The state measurement Z is corrected using a sequential correction method based on the sequential extended Kalman filter algorithm. k Update sequentially, including:

[0040] H k For including h i ,i=1,2,…,m is the observation matrix of its corresponding columns, Rk For including r i The measurement noise covariance matrix, i = 1, 2, ..., m, is the element corresponding to the diagonal elements. This is then combined with the error covariance matrix P. kk-1 Find the component g of the filter gain under the sequential extended Kalman filter algorithm. k ;

[0041] The component g of the filter gain under the sequential extended Kalman filter algorithm k and state measurement component z i For prior estimates The components are corrected to obtain a new state estimate. The components are updated simultaneously, and the error covariance matrix P is updated. kk-1 The new error covariance matrix P is obtained. kk ;

[0042] When all state measurements Z k After completing the updates sequentially, the updated state estimates are obtained. As the state estimate at the current moment The latest error covariance matrix P is also obtained. kk As the error covariance matrix P k .

[0043] The above-mentioned technical means are also specific implementation methods of the algorithm construction unit 22.

[0044] (I) Sequential Extended Kalman Filter Algorithm (SEKF)

[0045] Since the variance matrix R k Given that is a diagonal matrix, denoted as:

[0046] R k =diag[r 1 ,…,r m (2)

[0047] Where m represents the dimension of the state vector;

[0048] The computational flow of the Sequential Extended Kalman Filter (SEKF) algorithm is as follows:

[0049] Input: X0, P0, Q k

[0050] Iteration: k = k + 1

[0051] 1.

[0052] 2.

[0053] Sequential correction:

[0054] Input: P 0 =P k∣k-1 , i=0

[0055] Iteration: i = i + 1

[0056] 3.

[0057] 4.

[0058] 5.P i =P i-1 -g i (h i ) T P i-1

[0059] Output: P k =P m ,

[0060] In the Sequential Extended Kalman Filter (SEKF) algorithm, P 0 Represents the error covariance matrix P m initial value, Z is the predicted state value at the current moment. k The current state measurement, including z. i Let i = 1, 2, ..., m be the corresponding column vectors, and let F be the state transition matrix. kk-1 The state equation f at the current moment k (X k )exist H is the first-order linear term in the Taylor expansion. k Let h be the observation matrix. i For its corresponding column vector, P k|k-1 Let g be the error covariance matrix at the current time. i This represents the filter gain of the i-th column in the sequential extended Kalman filter algorithm. To obtain the state estimate at the current time after the update, P k This is to obtain the error covariance matrix at the current time after the update.

[0061] The Sequential Extended Kalman Filter (SEKF) algorithm is based on the Extended Kalman Filter (EKF) algorithm, and modifies the state measurement Z during the filtering and correction phase. k Sequential processing is performed, and the state measurement quantity Z is... k The dimension is i, which is the number of iterations in the sequential processing loop. The state variable obtained in the last iteration is... And error covariance matrix P m That is, based on the state measurement value Z respectivelyk and state prediction value The corrected state update value And update the error covariance value. Since the sequential processing in SEKF only updates the measured quantity Z... k Each component is processed in groups without losing any measurement information; therefore, SEKF does not reduce the estimation accuracy of EKF. However, SEKF still suffers from the problem of filter inconsistency caused by truncation error. Quasi-consistency of a filter means that the upper bound of the unknown true estimation error matrix can be determined by the error covariance matrix calculated by the algorithm. The Sequential Extended Kalman Filter (SEKF) algorithm, through the analysis of the multidimensional state measurement Z... k The successive corrections avoid the high-dimensional operations of matrix inversion, resulting in a smaller computational load and making it suitable for navigation and positioning scenarios with high real-time requirements.

[0062] (II) SEKF Consistency Analysis of Sequential Extended Kalman Filter Algorithm

[0063] For the general nonlinear system corresponding to formula (1), when using the sequential extended Kalman filter algorithm, it is assumed that... It is the linearization error of the state equation at the previous time step, ψ k It measures the linearization error of the equation at the current time:

[0064]

[0065] Among them, H k Let F represent the observation matrix. k-1 Let be the state transition matrix, representing the state equation at the previous time step. The first-order linear term in the Taylor expansion, This represents the state estimate from the previous time step when using the sequential extended Kalman filter algorithm. X represents the predicted state at the current time step when using the Sequential Extended Kalman Filter algorithm. k This represents the actual state value at the current moment;

[0066] Therefore, for the Sequential Extended Kalman Filter (SEKF) algorithm, the estimation error of the state vector will be:

[0067]

[0068] v k K represents the observation noise at the current moment. k Let be the filter gain at the current time step when using the sequential extended Kalman filter algorithm; let as well as

[0069]

[0070] Among them, Kk-1 ψ represents the filter gain at the previous time step when using the sequential extended Kalman filter algorithm. k( · ) For ψ k , for X k-1 ΔP represents the true value of the state at the previous moment. k This represents the covariance matrix bias of the estimation error caused by the mean square error of the state estimation after considering linearization error;

[0071] Therefore, the mean square error (MSE) of the estimation error of the state variables is:

[0072]

[0073] Among them, P k Let ΔP represent the error covariance matrix after considering linearization error. k This represents the covariance matrix bias of the estimation error caused by the mean square error of the state estimation after considering the linearization error.

[0074] The above-mentioned technical means are also specific implementation methods of the algorithm construction unit 22.

[0075] If ΔP k If the mean square error (MSE) of the actual state quantity is greater than 0, then the mean square error (MSE) of the actual state quantity is not equal to the calculated error covariance matrix P. k Therefore, SEKF does not satisfy consistency for general nonlinear systems.

[0076] At this point, the filter's estimation error accumulates over time, leading to increasingly inaccurate system state estimation. This accumulation of error can cause the filter's output to gradually deviate from the true state, especially during multiple iterations or long-term operation, potentially causing the system to lose effective tracking of the true state. Therefore, the error covariance matrix P in EKF is used... k+1 Using this method to calculate the Kalman gain will produce a state estimate with a large error, which will seriously affect the estimation accuracy of the filter and may even lead to the inability to reliably predict the system state, resulting in decision errors, system failures and other consequences.

[0077] III. Quasi-consistent sequential Kalman filter algorithm QCSEKF

[0078] Constructing a quasi-consistent sequential extended Kalman filter algorithm based on the sequential extended Kalman filter algorithm includes: constructing a quasi-consistent sequential extended Kalman filter algorithm based on the sequential extended Kalman filter algorithm. When the filter corresponding to the quasi-consistent sequential extended Kalman filter algorithm has quasi-consistent characteristics, the mean square error of the actual state quantity of the filter is equal to or less than the error covariance matrix of the state quantity calculated by the filter. Here, having quasi-consistent characteristics means that the calculation error of the quasi-consistent sequential extended Kalman filter algorithm is controlled within a set range.

[0079] For general nonlinear systems, the first two moments of equation (3) corresponding to the estimation error of the state variables are difficult to calculate accurately, according to equations (3) and (4).

[0080] Quasi-consistency means that if a filter is quasi-consistent, then the actual mean square error (MSE) of the filter is equal to or less than the calculated error covariance matrix of the filter. That is, the mean square error of the actual state variables of the filter is equal to or less than the error covariance matrix of the calculated state variables of the filter, expressed as:

[0081] in, It is the actual MSE of the estimation error of the state variables. It is the error covariance matrix of the estimation error calculated using the filter corresponding to the Quasi-Consistent Sequential Extended Kalman Filter (QCSEKF) algorithm. The QCSEKF algorithm is as follows:

[0082] Input: X0,

[0083] Iteration: k = k + 1

[0084] 1.

[0085] 2.

[0086] Sequential correction:

[0087] enter: i=0

[0088] Iteration: i = i + 1

[0089] 3.

[0090] 4.

[0091] 5.

[0092] 6.

[0093] Output: P k =P m ,

[0094] The mean square error (MSE) of the actual state variables of the filter is equal to or less than the error covariance matrix of the state variables calculated by the filter, including:

[0095] Two distinct covariance matrices are constructed, each based on the mean square error of the linearization error of the state variables in the Sequential Extended Kalman Filter (SEK). By introducing these two distinct covariance matrices into the SEK, the linearization error of the state variables in the SEK is compensated, ensuring that the mean square error of the actual state variables in the filter is equal to or less than the error covariance matrix of the state variables calculated by the filter.

[0096] Wherein, the two different covariance matrices are and and It is a symmetric matrix that satisfies:

[0097]

[0098] in, The initial value of the error covariance matrix represents the estimation error calculated using the quasi-consistent sequential extended Kalman filter algorithm, and the state transition matrix F represents the state transition matrix F. k For F k|k-1 The state equation f at the current moment k (X k )exist The first-order linear term in the Taylor expansion;

[0099] When using QCSEKF, the covariance matrix diagonal elements, This is the error covariance matrix of the state measurement quantities in the i-th column of the error covariance matrix calculated using the filter corresponding to the QCSEKF method. The i-th diagonal element;

[0100] Will Add to process noise w k covariance matrix Q k ,Will Fully added to the observation noise v k covariance matrix R k .

[0101] The above-mentioned technical means are also specific implementation methods of the algorithm construction unit 22.

[0102] For the QCSEKF algorithm, the following theorem holds. Here, k represents the current time step, and k+1 represents the next time step. The specific time step used has no impact on the proof; it is sufficient to use two adjacent time steps.

[0103] Theorem: For the formula (1) corresponding to a nonlinear system, the QCSEKF algorithm is quasi-consistent, that is:

[0104]

[0105] Proof: Assume Linearizing the process equations, the following equations hold: In the formula, The linearization error is then the prediction error at time step k+1 is:

[0106] Therefore, the mean square error of the predicted value can be expressed in the following form:

[0107]

[0108] Without loss of generality, assume:

[0109] available:

[0110]

[0111] in,

[0112] Linearizing the output function, the following equation holds:

[0113] In the formula, ψ k+1 The linearization error is the filtering error at time step k+1.

[0114]

[0115] Therefore, the mean square error of the filter estimate can be expressed in the following form:

[0116]

[0117] In the formula, set up satisfy:

[0118]

[0119] First, the theorem introduces matrices from equations (6) and (7). and To compensate for the truncation error (i.e., linearization error) in SEKF, it is determined by the MSE of the truncation error. and This allows QCSEKF to provide more robust estimates. In practical applications, the parameters need to be adjusted according to the specific circumstances. If the parameters... and An inappropriate selection may lead to a situation where the actual MSE of the estimation error is bounded. It is divergent.

[0120] Example for considering the system

[0121] It is the state transition function, X k =[x 1,k ,x 2,k ] T It is a two-dimensional state vector, y k It is a two-dimensional measurement vector, and the measurement function is H(X). k )=I2X k Process noise w k and measurement noise v k Uncorrelated Gaussian white noise, with covariance matrices Q and Q', respectively. k =R k =I2, initial value x 2,0 ∈[-0.5,0.5] and X0 and v0 are independent, the initial error covariance is:

[0122]

[0123] The initial state is estimated as follows According to QCSEKF, F0 = I2. Take... For system (8), Satisfying inequality (6), according to step 2 of QCSEKF, we can calculate:

[0124] because It is a two-dimensional measurement vector, therefore According to QCSEKF, .when hour:

[0125]

[0126] Due to g 1 With y k It is independent, take then:

[0127] When i = 2, take have:

[0128]

[0129] Right now: Easy to obtain It is a diagonal matrix, and it has the following properties: visible, and Both with y k independent.

[0130] Pick have

[0131] Obviously, It is divergent. However, the actual MSE of the state estimate is bounded. In fact, according to the theorem, QCSEKF is quasi-consistent. Therefore:

[0132] In this example, due to the parameter An inappropriate selection of [a parameter] leads to the actual MSE of the state estimate being bounded. Divergence. This indicates that in QCSEKF, the parameters... and The selection of [the appropriate element] is very important and crucial.

[0133] Second, select according to the parameter determination rules of QCSEKF in the theorem. and Then, its filtering error can be used To evaluate. This ensures that the actual MSE of the estimation error is bounded. Third, since both the QCSEKF and SEKF algorithms perform sequential processing on the correction segment, the efficiency of QCSEKF computation is guaranteed, which improves the computational efficiency compared to EKF. The computational efficiency of SEKF was analyzed in detail earlier, and Table 1 shows the number of floating-point calculations for the three algorithms.

[0134] Table 1 Computational Complexity

[0135] EKF filter gain <![CDATA[m 3 + 4m 2 n + 4 min 2 -3mn]]> EKF Status Update 2mn+m EKF Error Covariance Update <![CDATA[2mn 2 -n 2 + 2n 3 ]]> SEKF filter gain <![CDATA[4n 2 + 2]]> SEKF Status Update 2n+2 SEKF Error Covariance Update 3n+2 QCSEKF filter gain <![CDATA[4n 2 +2]]> QCSEKF Status Update 2n+2 QCSEKF Error Covariance Update 3n+3

[0136] IV. In the filtering stage of estimating the state at the current moment, a sequential correction processing method is used to divide the components of the state measurement value at the current moment into multiple groups, and combined with the state prediction value at the current moment, the state estimate value of the spacecraft at the current moment is obtained, including: in the filtering stage of estimating the state at the current moment, a sequential correction processing method is used to divide the components of the state measurement value at the current moment into multiple groups; the state prediction value and the measurement value are weighted and fused by the filter gain corresponding to the quasi-consistent sequential extended Kalman filter algorithm, and the state estimate value is solved by formula (5). Obtain the spacecraft state estimate at the current moment.

[0137] The above-mentioned technical means are also specific implementation methods of the solution unit 23.

[0138] V. Algorithm Verification

[0139] The estimation accuracy of SEKF and QCSEKF was compared through numerical examples and application scenario simulations.

[0140] (I) Numerical Simulation Examples

[0141] Select the nonlinear system shown in equation (9)

[0142]

[0143] In the formula, {w k} and {v k} is a Gaussian white noise sequence with zero mean and variance of 1.

[0144] Assuming the initial state x0 = 10, the initial state is estimated. The initial error covariance P0 = 1, the number of steps is 50, and 100 Monte Carlo simulations are performed. The SEKF and QCSEKF algorithms are used to evaluate x. k Estimate the state by means of Figure 3 The comparison between the actual state and the estimated state of the two filters shows that QCSEKF presents a more accurate estimation effect.

[0145] Figure 4 and Figure 5 The charts show a comparison of the relative position estimation error and root mean square error (RMSE) for SEKF and QCSEKF methods, respectively. The RMSE calculation formula is: Where n is the number of Monte Carlo simulations.

[0146] from Figure 4 and Figure 5 It can be seen that QCSEKF has better convergence performance. For example... Figure 6 The box plots shown in (Comparison of Relative Position Estimation Error) and (Comparison of RMSE) depict the maximum, upper quartile, median, lower quartile, and minimum values ​​of the data, with each horizontal line representing the data's maximum value, upper quartile, median, lower quartile, and minimum value. This provides a more intuitive comparison of the errors of the two algorithms. QCSEKF has a slightly higher accuracy than SEKF. The data from QCSEKF is more concentrated and has no obvious outliers, indicating that the estimation results of QCSEKF are more stable.

[0147] Table 2 compares the RMSE data, providing a more intuitive reflection of the accuracy of the two algorithms. Table 2 shows that the QCSEKF algorithm improves estimation accuracy by approximately 66.08%. This is because, in designing the QCSEKF algorithm, the truncation error caused by linearization is considered to expand over time due to iterative updates. Therefore, the truncation error is included in the calculation of the error covariance matrix to control the upper bound of the error. Consequently, the QCSEKF algorithm has relatively high accuracy.

[0148] Table 2 RMSE Comparison

[0149] RMSE 0.3832 0.1300 66.08％

[0150] (II) Application Scenario Simulation

[0151] Numerical simulations were performed using SEKF and QCSEKF methods to estimate the relative orbital parameters of space targets. The experimental scenario was selected from the PRISMA mission. Mango was the observation spacecraft, and Tango was the target spacecraft, designed to demonstrate relative navigation.

[0152] Figure 7 This visually illustrates the transformations between coordinate systems and a schematic diagram considering camera offset. Coordinate system: Spacecraft home system: O B -x B y B z B Spacecraft orbital system: O O -x O y O z O Spacecraft Camera System: O C -x C y C z C Target system: O T -x T y T z T and inertial frames of reference: O I -x I y I z ISpacecraft's own coordinate system: Origin at the spacecraft's center of mass; x-axis: roll; y-axis: pitch; z-axis: yaw. Spacecraft's orbital coordinate system: Origin at the spacecraft's center of mass; z-axis: points towards the Earth's center; y-axis: perpendicular to the orbital plane; x-axis: determined by y and z according to the right-hand rule. Spacecraft's camera coordinate system: Origin at the camera's optical center; z-axis: points along the camera's optical axis; z and y axes: lie in a focal plane perpendicular to the optical axis; x-axis: usually points towards the spacecraft's orbital direction; y-axis: perpendicular to the x-axis. Because the coordinate systems are defined differently, it's necessary to specify which coordinate system the state vector (including position and velocity vectors) is relative to. The spacecraft's position and velocity in the orbital coordinate system define all position and velocity vectors in the simulation as being in this coordinate system. The spacecraft's average angular velocity: the average angle swept by the spacecraft per unit time as it orbits. The spacecraft's orbital radius: usually refers to the instantaneous distance from the center of mass of the central celestial body to the spacecraft's center of mass. Coordinate system transformations: This represents the rotation matrix from coordinate system ** to coordinate system *. The camera bias d is a known constant vector.

[0153] The state vector is Where r = [x, y, z] T and Let represent the spacecraft's position and velocity in the orbital frame, respectively. The system state model is chosen as the CW equations:

[0154]

[0155] in, r is the average angular velocity of the spacecraft, μ is the Earth's gravitational constant, and r is the mean angular velocity of the spacecraft. c This is the orbital radius of the spacecraft. Without loss of generality, consider the process noise ξ = [ξ x ,ξ y ,ξ z ] T Then the discrete form of the CW equation is:

[0156] w = [0 1×3 ξ T ] T .

[0157] The process noise w is Gaussian white noise with zero mean and covariance matrix Q.

[0158] The spacecraft observes the target using an installed optical camera. Since the optical camera can only acquire target orientation information and not distance information, a measurement equation with camera bias is considered. The measurement equation is: z = r C / ||r C ||2+v. where, The measurement noise v is a zero-mean value uncorrelated with w, and its covariance matrix is... Gaussian white noise. The symbol ||·||2 denotes the L2 norm of the vector.

[0159] 1. Simulation parameter settings

[0160] The QCSEKF-based model was validated using orbital parameters from Tango and Mango. The number of orbital elements is shown in Table 3.

[0161] Table 3. Number of orbital elements

[0162] Semi-long wheelbase, km 7128.52 7083.96 Eccentricity 0.00042 0.00057 Inclination angle, ° 98.5430 98.2565 Angle of perigee, ° 248.3887 80.8501 Ascending node, ° 313.3801 271.8002 Angle of near point, ° 111.7325 279.2711

[0163] Since only the measurement equation is nonlinear in this simulation, therefore... and Selected as:

[0164]

[0165] in:

[0166] The initial errors are ΔX1(0)=[8,5,5,0.2,0.2,0.5] and ΔX2(0)=[10,10,10,1,1,1]. The standard deviation of the measurement noise is σ. r =20,σ b =3×10 -3 ,σ e =3×10 -3 .

[0167] 2. Simulation Results

[0168] When the initial error ΔX(0) = ΔX1(0), the relative position estimation errors of SEKF and QCSEKF are obtained, such as Figure 8 As shown.

[0169] Because of initial errors, the results of the initial filtering are not stable. For example... Figure 9 As shown, the filtering error on the x-axis reaches an inflection point around the 20th sampling iteration. As time increases, the filtered value after the inflection point tends to smooth out, indicating that the filter converges. Similar results can be obtained when the initial error is set to ΔX(0) = ΔX2(0). The RMSE of SEKF and QCSEKF were calculated and compared as follows: Figure 10 and Figure 11 As shown. Figure 12 and Figure 13 (Comparison of estimation errors in different directions) and Figure 14 and Figure 15 The box plot shown in the (Distance Estimation Error & RMSE Comparison) indicates that QCSEKF has higher accuracy than SEKF.

[0170] Table 4 shows the RMSE in the x, y, and z directions and the RMSE for relative positions after filter convergence. With different initial errors, QCSEFK improves accuracy by approximately 57.91% (ΔX1(0)) and 72.54% (ΔX2(0)) compared to SEKF. The difference in accuracy is due to the different initial errors, which gradually accumulate in subsequent time update iterations, resulting in different estimation errors under the two initial errors. As time updates, the estimation error of QCSEKF gradually converges and stabilizes around 0. Therefore, the estimation accuracy of the algorithm with a larger initial error (ΔX2(0)) is relatively higher than that with a smaller initial error (ΔX1(0)). The average accuracy improvement of the new algorithm is approximately 65.23%.

[0171] Table 4 Comparison of Root Mean Square Error under Different Initial Errors: SEKF vs QCSEKF

[0172] SEKF 1.1728 0.1393 0.2587 1.2090 QCSEKF 0.4890 0.0587 0.1003 0.5027 57.91％ <![CDATA[ΔX(0)=ΔX2(0)]]> SEKF 1.6645 0.1996 0.8416 1.7225 QCSEKF 0.4592 0.0559 0.0985 0.4730 72.54％

[0173] The beneficial technical effects achieved by the embodiments of the present invention are as follows:

[0174] First, for the nonlinear system model of spacecraft autonomous navigation, a sequential algorithm is used to decompose the measurement correction stage of the extended Kalman filter into multiple single-objective optimization problems and process them sequentially. This avoids high-order matrix inversion operations, significantly reducing computational complexity and achieving lightweight design. Second, based on the analysis of the consistency of the sequential extended Kalman filter, a quasi-consistent sequential extended Kalman filter algorithm, QCSEKF, is constructed. The quasi-consistency of the filter can, to a certain extent, ensure that the error remains within an acceptable range, making the estimation error tend to stabilize. Specifically, appropriate parameters are introduced to consider the truncation error generated by linearization in the calculation of the mean square error; that is, within the sequential extended Kalman framework, an error compensation matrix is ​​added to conservatively adjust the covariance matrix and enhance the consistency of the results. When the system meets the observability condition, the quasi-consistent sequential extended Kalman filter algorithm can determine the upper bound of the unknown true estimation error matrix through its calculated error covariance matrix, determining the boundary of the estimation error in real time, thereby effectively improving the algorithm accuracy. Because QCSEKF has quasi-consistency, it can determine the boundary of the estimation error in real time, thus effectively improving the algorithm accuracy and the autonomous positioning and navigation accuracy of the spacecraft. Meanwhile, the sequential processing of the filtering correction segment ensures the high efficiency of QCSEKF calculation. Combining sequential updates with the error compensation matrix and adjusting the covariance prediction value based on the statistical characteristics of the truncation error avoids inconsistencies in the estimates, improving the estimation accuracy by approximately 66.08%. Simulations show that the new algorithm improves accuracy by an average of approximately 65.23%. It has good applicability to unmanned control systems. It is suitable for resource-constrained scenarios such as embedded platforms and spacecraft, and its high computational speed can meet the real-time requirements of navigation and tracking systems.

[0175] It should be understood that the specific embodiments described above further illustrate the purpose, technical solution and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A navigation and positioning method for a spacecraft, characterized in that, include: For spacecraft autonomous navigation systems, a nonlinear system model for spacecraft autonomous navigation is constructed. The nonlinear system model includes state equations and measurement equations. Process noise exists in the state equations, and observation noise exists in the measurement equations. Construct a quasi-consistent sequential extended Kalman filter algorithm; A quasi-consistent sequential extended Kalman filter algorithm is adopted. Based on the state equation, the predicted state value of the spacecraft at the current moment is predicted using the state estimate value of the spacecraft at the previous moment. Based on the measurement equation, the measured state value of the spacecraft at the current moment is obtained using the predicted state value of the current moment. In the filtering stage of the process of estimating the state at the current moment, a sequential correction processing method is used to divide the components of the measured state value at the current moment into multiple groups, and combined with the predicted state value at the current moment, the estimated state value of the spacecraft at the current moment is obtained.

2. The spacecraft navigation and positioning method according to claim 1, characterized in that, For spacecraft autonomous navigation systems, a nonlinear system model for spacecraft autonomous navigation is constructed. The nonlinear system model includes state equations and measurement equations. Process noise exists in the state equations, and observation noise exists in the measurement equations, including: For spacecraft autonomous navigation systems, a nonlinear system model for spacecraft autonomous navigation is constructed: Where k represents the current time, k-1 represents the previous time, and X k-1 , Let X be a state variable, where the state includes the spacecraft's position and velocity. k X represents the true state value at the current moment. k-1 This represents the true value of the state at the previous moment. For the observables, let f(·) represent the current state measurement of the spacecraft. The state equation f(·) and the measurement equation h(·) are nonlinear functions, and the process noise w k-1 It has zero mean and a covariance matrix of Q. k Time-invariant white Gaussian noise, observation noise v k It has zero mean and a covariance matrix of R. k Time-invariant Gaussian white noise.

3. The spacecraft navigation and positioning method according to claim 2, characterized in that, Constructing a quasi-consistent sequential extended Kalman filter algorithm, including: Based on the sequential extended Kalman filter algorithm, a quasi-consistent sequential extended Kalman filter algorithm with consistency is constructed. The steps for constructing the sequential extended Kalman filter algorithm are as follows: First, under the sequential extended Kalman filter algorithm, the prior state estimate is set. The initial value X0 and the error covariance matrix P k The initial value is P0, and the process noise is set to w. k The covariance matrix Q k ; Subsequently, the time update phase begins, where the state at the previous time step is predicted based on the state equation to obtain the prior estimate for the current time step. and its error covariance matrix P kk-1 The sequential correction method of the sequential extended Kalman filter algorithm is used to correct the state measurement Z. k Update sequentially, Z k For including z i ,i=1,2,…,m are the state measurements of their corresponding columns; Among them, the sequential correction method using the sequential extended Kalman filter algorithm is used to correct the state measurement Z. k The updates will be performed sequentially, specifically including: H k For including h i ,i=1,2,…,m is the observation matrix of its corresponding columns, R k For including r i The measurement noise covariance matrix, i = 1, 2, ..., m, is the element corresponding to the diagonal elements. This is then combined with the error covariance matrix P. kk-1 Find the component g of the filter gain under the sequential extended Kalman filter algorithm. k ; The component g of the filter gain under the sequential extended Kalman filter algorithm k and state measurement component z i For prior estimates The components are corrected to obtain a new state estimate. The components are updated simultaneously, and the error covariance matrix P is updated. kk-1 The new error covariance matrix P is obtained. kk ; When all state measurements Z k After completing the updates sequentially, the updated state estimates are obtained. As the state estimate at the current moment And obtain the latest error covariance matrix P kk As the error covariance matrix P k .

4. The spacecraft navigation and positioning method according to claim 3, characterized in that, Also includes: For equation (1), when using the sequential extended Kalman filter algorithm, it is assumed that... It is the linearization error of the state equation at the previous time step, ψ k It measures the linearization error of the equation at the current time: Among them, H k Let F represent the observation matrix. k-1 Let be the state transition matrix, representing the state equation at the previous time step. The first-order linear term in the Taylor expansion, This represents the state estimate from the previous time step when using the sequential extended Kalman filter algorithm. X represents the predicted state at the current time step when using the Sequential Extended Kalman Filter algorithm. k This represents the actual state value at the current moment; Therefore, for the sequential extended Kalman filter algorithm, the estimation error of the state variables will be: Among them, v k K represents the observation noise at the current moment. k This represents the filter gain at the current time when the sequential extended Kalman filter algorithm is used; make as well as Among them, K k-1 ψ represents the filter gain at the previous time step when using the sequential extended Kalman filter algorithm. k( · ) For ψ k , for X k-1 ΔP represents the true value of the state at the previous moment. k This represents the covariance matrix bias of the estimation error caused by the mean square error of the state estimation after considering linearization error; Therefore, the mean square error (MSE) of the estimation error of the state variables is: Among them, P k Let ΔP represent the error covariance matrix after considering linearization error. k This represents the covariance matrix bias of the estimation error caused by the mean square error of the state estimation after considering the linearization error.

5. The spacecraft navigation and positioning method according to claim 4, characterized in that, Based on the aforementioned sequential extended Kalman filter algorithm, a quasi-consistent sequential extended Kalman filter algorithm is constructed, including: Based on the aforementioned sequential extended Kalman filter algorithm, a quasi-consistent sequential extended Kalman filter algorithm is constructed. When the filter corresponding to the quasi-consistent sequential extended Kalman filter algorithm has quasi-consistent characteristics, the mean square error of the actual state variables of the filter is equal to or less than the error covariance matrix of the state variables calculated by the filter. Here, having quasi-consistent characteristics means that the calculation error of the quasi-consistent sequential extended Kalman filter algorithm is controlled within a set range.

6. The spacecraft navigation and positioning method according to claim 5, characterized in that, Based on the aforementioned sequential extended Kalman filter algorithm, a quasi-consistent sequential extended Kalman filter algorithm is constructed, including: The mean square error of the actual state variables of the filter is equal to or less than the error covariance matrix of the calculated state variables, expressed as: in, It is the actual MSE of the estimation error of the state variables. It is the error covariance matrix of the estimation error calculated using the filter corresponding to the quasi-consistent sequential extended Kalman filter algorithm.

7. The spacecraft navigation and positioning method according to claim 6, characterized in that, The mean square error (MSE) of the actual state variables of the filter is equal to or less than the error covariance matrix of the state variables calculated by the filter, including: Two different covariance matrices are constructed, which are obtained based on the mean square error of the linearization error of the state variables in the sequential extended Kalman filter algorithm. By introducing two different covariance matrices into the Sequential Extended Kalman Filter (SEK) algorithm, the linearization error of the state variables in the SEK algorithm is compensated, ensuring that the mean square error of the actual state variables of the filter is equal to or less than the error covariance matrix of the state variables calculated by the filter.

8. The spacecraft navigation and positioning method according to claim 7, characterized in that, Two different covariance matrices are and and satisfy: in, The initial value of the error covariance matrix represents the estimation error calculated using the quasi-consistent sequential extended Kalman filter algorithm, and the state transition matrix F represents the state transition matrix F. k For F k|k-1 The state equation f at the current moment k (X k )exist The first-order linear term in the Taylor expansion; and To achieve the desired covariance matrix when using the quasi-consistent sequential extended Kalman filter algorithm diagonal elements, It is the error covariance matrix of the state measurement quantities in the i-th column of the error covariance matrix calculated using the filter corresponding to the quasi-consistent sequential extended Kalman filter algorithm. The i-th diagonal element; Will Add to process noise w k covariance matrix Q k In the middle, Fully added to the observation noise v k covariance matrix R k middle.

9. The navigation and positioning method for a spacecraft according to claim 8, characterized in that, In the filtering stage of estimating the current state, a sequential correction method is used to divide the components of the current state measurement value into multiple groups, and combined with the current state prediction value, to obtain the spacecraft's current state estimate, including: In the filtering stage of the process of estimating the state at the current moment, a sequential correction method is used to divide the components of the state measurement value at the current moment into multiple groups; The state prediction and measurement values ​​are weighted and fused by the filter gain corresponding to the quasi-consistent sequential extended Kalman filter algorithm, and the state estimate is solved by formula (5). Obtain the spacecraft state estimate at the current moment.

10. A navigation and positioning system for a spacecraft, characterized in that, include: The model building unit is used to build a nonlinear system model for the autonomous navigation system of a spacecraft. The nonlinear system model includes state equations and measurement equations. Process noise exists in the state equations and observation noise exists in the measurement equations. Algorithm building unit, used to construct a quasi-consistent sequential extended Kalman filter algorithm; The solution unit employs a quasi-consistent sequential extended Kalman filter algorithm to predict the current state value based on the state equation using the state estimate of the spacecraft at the previous moment, and to obtain the current state measurement value of the spacecraft based on the measurement equation using the current state prediction value. Specifically, during the filtering stage of estimating the current state, a sequential correction process is used to divide the components of the current state measurement value into multiple groups, and these groups are combined with the current state prediction value to obtain the current state estimate of the spacecraft.

Images