An Adaptive Unscented Kalman Filtering Method Based on Sequential State Difference

Abstract

The present invention provides an adaptive unscented Kalman filtering method based on sequential state difference, including: establishing a state model of an adaptive navigation system based on sequential state difference; establishing a measurement model of an adaptive navigation system based on sequential state difference; constructing a mutation noise detection based on state chi-square test to determine whether to adaptively estimate the process noise covariance; if a mutated process noise is detected, adaptively estimating the process noise covariance; if no mutated process noise is detected, keeping the process noise covariance unchanged. The state chi-square test is used to detect the mutation of the process noise in real time. When a state noise mutation is detected, based on the dynamic model, this method is applied to the state estimation of a Mars spacecraft. This method can effectively suppress the estimation error and solve the divergence problem.

Description

Technical Field

[0001] The present invention belongs to the technical field of aerospace navigation, and particularly relates to an adaptive unscented Kalman filtering method based on sequential state difference. Background Art

[0002] In recent years, deep space exploration has attracted increasing attention. With the rapid development of deep space exploration, the navigation system for determining the position, velocity, and attitude of a detector plays a crucial role in deep space exploration missions, directly affecting the operating state of deep space detectors and the success or failure of deep space exploration missions. The autonomous navigation system of a deep space detector generally uses an orbital dynamics model, a measurement model, and an optimal state estimation method to estimate the current position, velocity, and attitude of the detector.

[0003] The numerical integration method is a widely used method in the calculation of spacecraft orbital dynamics models. However, due to factors such as the integration method and step size selection, its use inevitably introduces integration errors. The sources of integration errors are diverse, including rounding errors, truncation errors, and numerical uncertainties, and the resulting cumulative errors are difficult to predict. Researchers have proposed various numerical integration methods to reduce integration errors, but despite these efforts, integration errors still exist, especially in the uncertain environment of deep space exploration. When calculating the spacecraft orbit, it is necessary to synchronously estimate the errors, mainly truncation errors. Therefore, some researchers have tried to approximate the expression of local truncation errors, but this is only effective for integration algorithms with simple truncation error structures and low computational complexity. Another method for minimizing the spacecraft orbit integration error is to incorporate process noise into the uncertainties in the system dynamics model, usually achieved through Kalman filtering (KF). This method aims to unify the uncertainties caused by numerical integration and model errors by introducing process noise.

[0004] As mentioned above, when calculating the orbit of a spacecraft, it is necessary to synchronously estimate the truncation error of numerical integration. According to the orbit dynamics model of deep space spacecraft, it is not difficult to understand that the truncation error is not only related to the integration method and integration step size, but also related to the gravitational force of celestial bodies. Especially when a deep space spacecraft enters the proximity phase, it enters the gravitational influence range of the target celestial body. During the proximity phase, the orbit switching of the deep space spacecraft is not instantaneous, but a slow process. However, during the proximity phase, the gravitational acceleration of Mars increases rapidly, resulting in an increase in the local truncation error of numerical integration. Coupled with the interference of other disturbances, the orbit integration error and the spacecraft state estimation error increase, ultimately leading to the divergence of the navigation system. This is because the setting of the error variance of the navigation filter model requires knowledge of the statistical characteristics of the error. However, during the proximity phase, the deep space spacecraft encounters rapidly time-varying errors without precise prior knowledge, and the original filter error variance cannot track the rapidly time-varying errors in a timely and accurate manner. Therefore, this problem is transformed into the need for an adaptive filtering method to timely reflect the changes in the rapidly time-varying process noise in the presence of unknown rapidly time-varying process noise, so as to accurately estimate the position and velocity of the spacecraft.

[0005] To solve this problem, adaptive Kalman filtering has been studied in recent years and can be divided into four categories: multi-model estimation methods, numerical optimization methods, Bayesian methods, and covariance matching methods.

[0006] The multi-model estimation method uses multiple Kalman filters to simultaneously estimate multiple sets of states of the spacecraft, and the optimal state estimation is based on a certain weighted combination. Many improved methods have been derived from this method, such as the interacting multiple model (IMM) and the interacting multiple model unscented Kalman filter (IMMUKF).

[0007] The numerical optimization method is a method that establishes a specific objective function according to the maximum likelihood method (MLE), the maximum a posteriori method (MAP), or the expectation maximization method (EM), and then uses an optimization algorithm to obtain the analytical solution of the state or measurement noise covariance. The principle of this method is simple, but the derivation process is complex and the computational efficiency is low.

[0008] The Bayesian method is a method that combines the prior information of unknown parameters with the sample information to obtain the posterior probability distribution of the state and unknown parameters through recursive calculation. The variational Bayesian method is the most representative method, but its theory is complex and the computational amount is large.

[0009] The covariance matching method uses innovation information and residual information to estimate and correct the state noise covariance matrix in real time. The most typical and widely used method is Sage-Huge. The improved Sage-Huge algorithm is applied to target tracking with unknown process noise.

[0010] In summary, although the above methods can adaptively estimate the process noise covariance, there are still some issues that need attention. The first three methods face problems related to long-term operation or complex principles. The fourth method is covariance matching, which is based on the measurement innovation and requires the error trends of the measurement model and the state model to be consistent, restricting the choice of the measurement model. In addition, the covariance matching method based on the measurement innovation is usually applicable to the case where the process noise covariance changes slowly over time. When the noise in the state model changes rapidly, there may be limitations. Summary of the Invention

[0011] To solve the above restrictive problems, the present invention proposes an adaptive unscented Kalman filtering method based on sequential state difference. When the state noise suddenly increases due to the increase in gravitational acceleration, the change in adjacent states also suddenly increases due to the increase in gravitational acceleration. The consistency of these change trends is caused by a common cause, i.e., the sudden acceleration enhancement. Compared with the measurement innovation vector, the change in adjacent states can more accurately reflect the change in state noise. Therefore, when the state noise suddenly increases due to the increase in gravitational acceleration, it is reasonable to use the difference between adjacent state values to approximate the change in the process noise covariance. At the same time, a state chi-square test is used to detect the real-time change in state noise. When a sudden change in state noise is detected, AUKF-SSD is used instead of the traditional AUKF-IVEC based on the measurement innovation vector. In addition, to highlight the role of new data, the present invention adopts a real-time fading factor and obtains a recursive AUKF-SSD using this fading factor. Applying the AUKF-SSD method to the state estimation of a Mars spacecraft, compared with the traditional UKF and AUKF-IVEC methods, this method can effectively suppress the estimation error and solve the divergence problem.

[0012] To achieve the above object, the present invention adopts the following technical solutions:

[0013] An adaptive unscented Kalman filtering method based on sequential state difference uses a state chi-square test to detect the sudden change in process noise in real time. When a change in state noise is detected, based on the dynamic model, the sequential state difference AUKF-SSD method is applied to the state estimation of a Mars spacecraft. This method can effectively suppress the estimation error and solve the divergence problem. Specifically, it includes the following steps:

[0014] Step 1: Establish a state model of an adaptive navigation system based on sequential state difference;

[0015] Step 2: Establish a measurement model of an adaptive navigation system based on sequential state difference;

[0016] Step 3: Based on the state model and the measurement model, construct a mutation noise detection based on state chi-square test to determine whether an adaptive estimation of the process noise covariance is required ;

[0017] Step 4: If a mutation in the process noise is detected, perform an adaptive estimation of the process noise covariance ;

[0018] Step 5: If no mutation in the process noise is detected, keep the process noise covariance estimation unchanged.

[0019] The beneficial effects of the present invention compared with the prior art are as follows:

[0020] (1) There is no need to face problems related to long-term operation or complex principles;

[0021] (2) It does not require the error trend of the measurement model to be consistent with that of the state model, expanding the selection of the measurement model;

[0022] (3) It is applicable to the case of mutation of the process noise covariance;

[0023] (4) It can effectively suppress the estimation error and solve the divergence problem. Description of the Drawings

[0024] Figure 1 It is a flowchart of the adaptive unscented Kalman filtering method based on sequential state difference of the present invention. Detailed Embodiment

[0025] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0026] An adaptive unscented Kalman filtering method based on sequential state difference of the present invention is a new navigation method that uses state chi-square test to detect mutations in process noise in real time. When a mutation in state noise is detected, based on the dynamic model, the AUKF-SSD method is applied to the state estimation of a Mars spacecraft. This method can effectively suppress the estimation error and solve the divergence problem.

[0027] The method of the present invention is as follows: Taking a Mars spacecraft as an example, construct a state model and a measurement model of an adaptive filtering navigation system, and use mutation noise detection based on state chi-square test to determine whether an adaptive estimation of the process noise covariance is required , when the mutation process noise is detected, based on the kinetic model, the AUKF-SSD method is applied to the state estimation of the Mars spacecraft.

[0028] The present invention will be described in detail below with specific embodiments.

[0029] As Figure 1 shown, the adaptive unscented Kalman filtering method based on sequential state difference (AUKF-SSD) of the present invention includes the following steps:

[0030] Step (1) Establish the state model of the adaptive navigation system based on sequential state difference:

[0031] For the Mars spacecraft, the coordinate system is selected as the Mars-centered inertial coordinate system at epoch J2000.0. Considering the gravitational force of the target planet Mars, the sun, and other planets on the spacecraft, the radiation pressure, the rocket thrust during the orbital maneuver, and the high-precision ephemeris, the navigation system state model of the Mars-centered inertial coordinate system MCI is as follows:

[0032] (1)

[0033] Wherein, and are respectively the position vector and velocity vector of the spacecraft in the inertial coordinate system MCI, is the two-norm of and are respectively the derivatives of the position vector and velocity vector with respect to time, is the gravitational constant of Mars, is the gravitational constant of the sun, is the th gravitational constant of the planet, and are respectively the position vectors of the spacecraft and Mars relative to the sun, and are respectively the position vectors of the spacecraft and Mars relative to the th planet, , , , are respectively , , , the two-norms of is the number of perturbing planets, and the process noise is , including miscellaneous perturbations;

[0034] Assume the state vector at time , The state vector at a moment is is the process function, is the process noise, then the state model in MCI is:

[0035] (2)

[0036] Step (2) Establish the measurement model of the adaptive navigation system based on sequential state difference:

[0037] Detect the X-ray pulsar signal arriving at the spacecraft through the spaceborne detector, and determine the time when the pulse arrives at the spacecraft through the time conversion model . At the same time, use the time conversion model to predict the time of arrival at the solar system barycenter (SSB) . and The difference between them is used as the measurement value for spacecraft navigation. If observing an X-ray pulsar, considering the relativistic and space geometry effects, the measurement model is expressed as:

[0038] (3)

[0039] Among them, represents the position vector of the spacecraft relative to the SSB, is the direction vector from the SSB to the detected pulsar, is the direction vector of the SSB relative to the sun, is the speed of light, is the distance between the SSB and the detected pulsar, is the gravitational constant of the sun, represents the measurement noise.

[0040] If observing multiple pulsars simultaneously, the measurement model can be expressed as:

[0041] (4)

[0042] Among them, is the measurement vector, represents the measurement noise vector, is the number of observed pulsars, is the measurement function.

[0043] Step (3) Based on the state model and the measurement model, construct a mutation noise detection based on state chi-square test to determine whether to adaptively estimate the process noise covariance :

[0044] Assume is the estimate obtained from the prior state and through the unscented Kalman filter (UKF). is the estimate obtained only from the prior state.

[0045] The estimation process of

[0046] (1) Initialization. The initial state estimate value of and covariance are as follows:

[0047] (5)

[0048] where E represents taking the mean.

[0049] (2) Calculate the sigma points for the time update. The sigma points at time are The state variable is dimension. Implementing the time update of the unscented Kalman filter (UKF) requires 2n + 1 sigma points, with the mean value being and the covariance being

[0050] (6)

[0051] where and is a very small positive number, which can be taken as and ;

[0052] (3) Time update: Calculate the one-step state prediction and the one-step prediction mean square error .

[0053] (7)

[0054] where and is updated from the sigma points at time to obtain the sigma points at time , and is the process noise covariance matrix.

[0055] (4) Calculate the sigma points for the measurement update . Implementing the measurement update of the unscented Kalman filter (UKF) requires 2n + 1 sigma points, with the mean value being and the covariance being . Different from the time update, the calculation method of these points is as follows:

[0056] (8)

[0057] (5) Measurement update

[0058] (9)

[0059] Among them, is the measurement function, is the estimated measurement, is the measurement variance, is the covariance between the state quantity and the measurement quantity, is the measurement noise covariance.

[0060] (6) Estimation filter gain , update the state and the corresponding covariance ;

[0061] (10)

[0062] Among them, is the actual measurement.

[0063] The calculation process of

[0064] (1) Initialization. The initial state The estimated value of and the covariance are:

[0065] (11)

[0066] Among them, E represents taking the mean.

[0067] (2) Calculate the sigma points of the prior state. The implementation of the prior state requires 2n + 1 sigma points, The sigma points at time are The mean value is The covariance is

[0068] (12)

[0069] (3) Propagation of the prior state, update the state and the corresponding covariance ;

[0070] (13)

[0071] Among them, is from Obtained by updating the sigma points at a certain moment Sigma points at a certain moment.

[0072] Define the unscented Kalman filter (UKF) estimation error and the state transfer estimation error :

[0073] (14)

[0074] where, is the true state of the spacecraft, and it is defined as:

[0075] (15)

[0076] Based on the basic assumption that the Gaussian white noise has a zero mean and the linear characteristics of Equation (15), it can be deduced that:

[0077] (16)

[0078] Covariance of

[0079] (17)

[0080] where, .

[0081] Therefore, the detection of state mutation consists of a test statistic:

[0082] (18)

[0083] The test statistic follows a chi-square distribution with n degrees of freedom. When , it indicates that a significant mutation has occurred in the state noise. Therefore, the following rule applies:

[0084] (19)

[0085] where, is the threshold determined from the chi-square distribution table;

[0086] As the filtering time increases, the state estimation error and covariance using UKF will gradually stabilize near very small values. However, due to the influence of cumulative errors, the estimation error and covariance using only the state prior information will continue to increase. This causes the covariance in Equation (17) to be determined only by , resulting in the failure of the chi-square test in Equation (19). To solve this problem, it is necessary to use in the form of a fixed time interval and to reset and 。

[0087] In step (4), when the mutation process noise is detected, perform adaptive estimation :

[0088] The constant noise estimator based on SSD can be expressed as:

[0089] (20)

[0090] where, is an auxiliary variable, , , represents the state transition matrix, is the optimal estimated state.

[0091] If defined as:

[0092] (21)

[0093] Then the formula (20) with the weighting coefficient can be expressed as:

[0094] (22)

[0095] where, 。 is the attenuation factor, is the forgetting factor. From formula (22), the recurrence formula can be obtained:

[0096] (23)

[0097] affects the utilization rate of the latest data, the larger, the more latest data is used; the smaller, the more historical data is used. is formula (24):

[0098] (24)

[0099] where, is the forgetting factor, and its non - negative value range is less than 1. In formula (24), is inversely proportional to , so in the state estimation process, can be adjusted by timely adjusting to adjust . Therefore, when the process noise changes significantly, reduce , increase , highlighting the importance of new data. When the process noise changes little, increase , reduce , emphasizing the importance of historical data. Update method:

[0100] (25)

[0101] Finally, the time-varying process noise covariance estimation based on SSD is obtained.

[0102] According to the process noise mutation test index defined by Equation (18) and the threshold . It can be seen from Equations (23), (24), and (25) that the fast time-varying process noise The update process is as follows:

[0103] (26)

[0104] The content not detailed in the specification of the present invention belongs to the prior art well-known to those skilled in the art. It is easy for those skilled in the art to understand that the above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. An adaptive unscented Kalman filtering method based on sequential state difference, characterized in that: The method comprises the following steps: Step 1: Establish a state model of the adaptive navigation system based on sequential state difference; Assume The state vector at time , are the position vector and velocity vector of the spacecraft in the inertial coordinate system MCI, The state vector at time , is a process function, is process noise, then the state model in the inertial coordinate system MCI centered on Mars is: (2) Step 2: Establish a measurement model of the adaptive navigation system based on sequential state difference; Step 3: Based on the state model and measurement model, construct a mutation noise detection based on the state chi-square test to determine whether adaptive estimation of process noise covariance estimation is needed ; Test statistic is a chi-square distribution with n degrees of freedom, when When , it indicates that the state noise has undergone a significant mutation, so the following rules apply: (19) in, is the threshold value determined by the chi-square distribution table; Step 4: If a sudden process noise is detected, perform adaptive estimation of the process noise covariance ; Step 5: If no mutation process noise is detected, keep the process noise covariance estimate constant; The fourth step comprises: The SSD-based constant noise estimator is expressed as: (20) in, is an auxiliary variable, , , represents the state transfer matrix, is the optimal estimation state; If defined: (21) With weighted coefficient Formula (20) can be expressed as: (22) in, , is the attenuation factor, is the forgetting factor. From formula (22), we can get the recursive formula: (23) Affects the utilization of the latest data, The larger it is, the more recent data is used; The smaller it is, the more historical data is used. It is formula (24): (24) in, For the forgetting factor, The value range of is less than 1, and its update method is: (25) Finally, we get the SSD-based time-varying process noise covariance estimation ; Process Noise Covariance Estimation The update process is as follows: (26)。 2. The method of adaptive unscented Kalman filtering based on sequential state difference according to claim 1, characterized in that: The step 1 includes: for the Martian spacecraft, the coordinate system selects the Mars mass center inertial coordinate system of epoch J2000.0, considering the gravity, radiation pressure, rocket thrust during orbital maneuvers and high-precision ephemeris of the target planet Mars, the sun and other planets on the spacecraft, and the state model of the adaptive navigation system of the inertial coordinate system MCI centered on Mars is as follows: (1) in, and are the position vector and velocity vector of the spacecraft in the inertial coordinate system MCI, yes The second norm of and are the time derivatives of the position vector and velocity vector, is the gravitational constant of Mars, is the gravitational constant of the Sun, It is The gravitational constant of the planet, and are the position vectors of the spacecraft and Mars relative to the Sun, and The spacecraft and Mars are relative to the The position vector of the planet, representing the distance , , , They are , , , The second norm of is the number of perturbed planets, and the process noise is , including miscellaneous perturbations.

3. The method of adaptive unscented Kalman filtering based on sequential state difference according to claim 1, characterized in that: The second step comprises: The X-ray pulsar signal arriving at the spacecraft is detected by the onboard detector, and the time when the pulse arrives at the spacecraft is determined by the time conversion model. At the same time, the time conversion model is used to predict the time to reach the solar system center of mass SSB , and The difference between them is used as the measurement value for spacecraft navigation. If an X-ray pulsar is observed, considering the effects of relativity and space geometry, the measurement model is expressed as: (3) in, represents the position vector of the spacecraft relative to the SSB, is the direction vector from the SSB to the detected pulsar, is the direction vector of SSB relative to the sun, is the speed of light, is the distance between the SSB and the detected pulsar, is the gravitational constant of the Sun, represents the measurement noise; If multiple pulsars are observed simultaneously, the measurement model can be expressed as: (4) in, is the measurement vector, represents the measurement noise vector, is the number of observed pulsars, is the measurement function.

4. The method of adaptive unscented Kalman filtering based on sequential state difference according to claim 1, characterized in that: The step three comprises: Assumptions is the estimate obtained by the prior state and the unscented Kalman filter UKF, is an estimate obtained only from the prior state; The estimation process is as follows: (1) Initialize the initial state Estimated value of and covariance ; (2) Calculate the sigma point of the time update , the state variable is Dimension, the time update of the unscented Kalman filter UKF requires 2n+1 sigma points; (3) Time update: one-step prediction of the calculated state And the one-step prediction mean square error ; (4) Calculate the sigma point for measurement update , 2n+1 sigma points are required to realize the measurement update of the unscented Kalman filter UKF; (5) Measurement update ; (6) Estimation of filter gain , update status and the corresponding covariance ; The calculation process is as follows: (1) Initialize the initial state Estimated value of and covariance ; (2) Calculate the sigma point of the prior state , the realization of the prior state requires 2n+1 sigma points; (3) Transfer of prior state and update of state and the corresponding covariance ; Definition of Unscented Kalman Filter UKF Estimation Error and state transfer estimation error : (14) in, is the true state of the spacecraft, defined as: (15) Based on the basic assumption that the mean of Gaussian white noise is zero and the linear characteristics of equation (15), it can be deduced that: (16) The covariance of is: (17) in, ; Therefore, the test for the occurrence of a state mutation consists of the test statistic: (18)。

Images