An interactive multiple model filtering method suitable for complex motion state estimation

Abstract

The application discloses an interactive multiple model filtering method suitable for complex motion state estimation; a first "current" statistical (CS) dynamic model for describing high dynamic motion, a second "current" statistical CS dynamic model for describing medium dynamic motion and a CV dynamic model for describing low dynamic motion are established; the maximum acceleration of the first CS dynamic model is larger than that of the second CS dynamic model; three sub-filters are established, one sub-filter corresponding to each dynamic model, and each sub-filter being an extended Kalman filter; a Markov transfer matrix among the three dynamic models is established, and automatic switching among the models is realized by using a Markov chain; and the final estimation value and covariance at the current moment are obtained by mixing the model probability corresponding to each model at the current moment and the estimation value and covariance output by each sub-filter at the current moment. The filtering method has a larger dynamic range and maintains high precision and stability under various complex dynamic conditions.

Description

Technical Field

[0001] This invention belongs to the field of satellite navigation, specifically relating to an interactive multi-model adaptive filtering method suitable for estimating complex user motion states. Background Technology

[0002] Kalman filtering and its extended forms are widely used state estimation methods in navigation receivers. Kalman filtering is a state-space and model-based method; its performance depends not only on the observation model but also on the accuracy of the user dynamic model and the statistical characteristics of dynamic noise. Generally, in low-dynamic situations, the state model uses classic constant velocity (CV) or constant acceleration (CA) models. However, when navigation receivers are used for complex motion state estimation, the user's multiple motion modes and unpredictable mode switching make it difficult for the conventional extended Kalman filter's single motion model to match, resulting in a mismatch between the dynamic model and the motion mode. User mode error is the main reason for the performance degradation and even divergence of the Kalman filter; adaptive methods are a major strategy to address this problem.

[0003] To address this, various adaptive Kalman filtering algorithms have been proposed, such as the Bayesian criterion, maximum likelihood estimation, innovation correlation method, and covariance matching method. Currently, research on adaptive Kalman filters is increasingly focused on two aspects: Kalman filtering based on innovation adaptive estimation (IAE) and multi-model adaptive estimation (MMAE). Innovation adaptive (IAE) methods include innovation correlation and covariance methods. Their theoretical basis is that the covariance of the observation's innovation is equal to the sum of the diagonal elements of the innovation covariance matrix. Generally, the estimation error covariance matrix is ​​adjusted by using multiple scaling factors or fading factors based on the difference between the two. A drawback of innovation adaptive methods is that they simultaneously adjust both the observation noise matrix and the system noise matrix, making it difficult to distinguish the influence of these two noise matrices, resulting in relatively poor stability. Multi-model adaptive methods, on the other hand, obtain state estimates by weighting the estimation results of a series of parallel filters with different state models, and can better match multi-mode motion.

[0004] In satellite navigation and positioning, the difficulty in estimating complex motion states lies in the diverse motion characteristics and unpredictable switching of motion modes. Conventional extended Kalman filters, with their single motion model, struggle to match these characteristics, and user mode errors often lead to filter performance degradation or even filter divergence. To better match the various motion modes of complex user motion, this invention proposes an interactive multi-model Kalman filtering method. Summary of the Invention

[0005] The technical problem to be solved by this invention is to propose an interactive multi-model filtering method with a large dynamic range that is suitable for complex motion state estimation, which can provide users with relatively accurate and reliable navigation and positioning services for various motion states.

[0006] The technical solution of the present invention is as follows:

[0007] An interactive multi-model filtering method suitable for complex motion state estimation is proposed, with the following steps:

[0008] (1) Establish a first CS dynamic model for describing high dynamic motion, a second CS dynamic model for describing medium dynamic motion, and a CV dynamic model for describing low dynamic motion; wherein the maximum acceleration value of the first CS dynamic model is greater than the maximum acceleration value of the second CS dynamic model.

[0009] (2) Establish three sub-filters, one for each dynamic model; each sub-filter is an extended Kalman filter.

[0010] (3) Establish the Markov transition matrix between the three dynamic models;

[0011] (4) Perform interactive multi-model filtering through the following steps:

[0012] (4.1) Determine the input mixing probability based on the model probability and Markov transition matrix of each model at the previous time step; obtain the input estimate and covariance of each sub-filter at the current time step by weighted recombining the input mixing probability and the estimated value and covariance of each sub-filter output after filtering at the previous time step.

[0013] (4.2) Based on the current observations, the input estimates and covariance, each sub-filter performs extended Kalman filtering in parallel to obtain the estimated values, covariance and innovation of each sub-filter output at the current time;

[0014] (4.3) Determine the matching degree of each model based on the information of each sub-filter, and determine the model probability corresponding to each model at the current time based on the matching degree of each model;

[0015] (4.4) The final estimated value and covariance at the current time are obtained by mixing the model probabilities corresponding to each model at the current time and the estimated values ​​and covariance of each sub-filter output at the current time.

[0016] The Markov transition matrix in step (3) is:

[0017]

[0018] i, j∈{1, 2, 3}, β represents the switching probability of the model. The value of β can be set to 0.05.

[0019] The formula for calculating the mixing probability in step (4.1) is as follows:

[0020]

[0021] Among them, c i For the i-th model m i The prior probability of (k), μ j (k-1) represents the j-th model m obtained after filtering at time k-1. j The model probability of (k-1), μ j / i (k / k-1) represents the model m at time k. i (k) is derived from the model m at time k-1. j The conditional probability derived from (k-1) is the mixed probability of the input.

[0022] The formulas for calculating the input estimate and covariance of each sub-filter at the current time in step (4.1) are as follows:

[0023]

[0024] P is the input estimate of the i-th sub-filter in the filtering process at time k. 0i (k) represents the input covariance of the i-th sub-filter in the filtering process at time k; P represents the output estimate of the j-th sub-filter obtained after filtering at time k-1; j (k-1 / k-1) is the output covariance of the j-th sub-filter obtained after filtering at time k-1.

[0025] The calculation formula for step (4.3) is as follows:

[0026]

[0027] Where, μ i (k) represents the i-th model m in the filtering process at time k. i The model probability of (k), Λ i (k) represents the i-th model m in the filtering process at time k. i The degree of matching of (k); v i (k) represents the information output by the i-th sub-filter in the filtering process at time k, S i (k) is the covariance matrix of the information output by the i-th sub-filter in the filtering at time k.

[0028] The calculation formula for step (4.4) is as follows:

[0029]

[0030] Let P(k / k) be the final estimated value of the filter at time k, and let P(k / k) be the final covariance of the filter at time k. P is the estimated value of the output of the i-th sub-filter at time k. i (k / k) represents the covariance of the output of the i-th sub-filter in the filtering process at time k.

[0031] Each sub-filter includes a state equation and a measurement equation, wherein the state equation is the dynamic model corresponding to that sub-filter; the measurement equation is a linearized equation of pseudorange or pseudorange rate, and its formula is:

[0032] y(k)=Hx(k)+n(k-1)

[0033] In the formula, y(k) is the pseudorange or pseudorange rate observation, n(k-1) is the observation noise, and H is the observation matrix; Where x, y, z l and i represent the position, velocity, acceleration, clock error, and frequency difference of the three axes in the geocentric-geocentric coordinate system, respectively.

[0034] Compared with the prior art, the present invention has the following advantages:

[0035] 1) This method designs a relatively complete set of dynamic models, which basically covers various motion modes of users, such as high, medium and low. Compared with the single-model Kalman filter commonly used in navigation state estimation, the method of this invention has a larger dynamic range.

[0036] 2) This method selects an adaptive "current" statistical (CS) model to better describe the user's dynamic state. Two sets of "current" statistical models with different parameters are designed to describe the user's high-maneuver and medium-maneuver states respectively. Furthermore, a high-precision CV model is used to match low-dynamic motion, so that this invention maintains high accuracy under various complex dynamic conditions.

[0037] 3) This method uses Markov chains to achieve automatic model switching, making the switching of each model smooth and stable, the filtering operation stable, and avoiding jump errors.

[0038] 4) This method has a wide range of applications and is suitable for satellite navigation in the guidance of various aircraft, spacecraft and missiles with complex motion states. Attached Figure Description

[0039] Figure 1 This is a block diagram of the three-model interactive extended Kalman filter structure;

[0040] Figure 2 Block diagram of extended Kalman filtering in satellite navigation state estimation;

[0041] Figure 3 Extended Kalman filter workflow diagram;

[0042] Figure 4 This is a schematic diagram of the user's flight path. Detailed Implementation

[0043] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0044] The Interactive Multi-Model (IMM) adaptive method obtains the state estimate by weighting the estimation results of a series of parallel filters with different state models, and can better match multi-mode motion. Considering that the difficulty in estimating complex motion states in satellite navigation and positioning lies in the multiple motion characteristics and unpredictable motion mode switching, the IMM method can effectively solve this problem. Complex motion states can generally be covered by multiple dynamic models, and each model needs to automatically switch as the motion mode changes, selecting the user model that best matches the current motion mode. Therefore, the IMM Kalman filtering method is introduced into the field of satellite navigation, and multiple adaptive dynamic models are designed to achieve matching and automatic switching between the dynamic model and the user's motion mode, thereby achieving accurate estimation of the user's complex motion state.

[0045] An interactive multi-model filtering method for complex motion state estimation, as proposed in this invention, is implemented through the following steps:

[0046] Step 1: Selection of model set and design of dynamic model

[0047] In the IMM method, the models used in the model set should cover as many possible motion states as possible for the user, and theoretically, the model set should be as large as possible. However, increasing the number of models not only increases the computational cost but also leads to inconsistencies between models used at the current moment, causing unnecessary competition between models and potentially failing to provide better performance. Therefore, designing a reasonable motion model set is the determining factor for the overall performance of adaptive filtering.

[0048] Generally, a user's complex motion state can be divided into three types: high, medium, and low. Therefore, three models can be selected to correspond to the three motion states respectively. In this invention, the "Current" Statistical (CS) model with its own adaptive characteristics is selected to describe the user's maneuver state. Two sets of "Current" Statistical models with different parameters are selected to match the high and medium dynamic motions respectively. At the same time, the CV model with a small system noise variance is selected to match the low dynamic motion of the carrier, so that the user can maintain a high positioning accuracy in various motion states.

[0049] The design of the model and its corresponding parameters will be explained below.

[0050] In navigation and positioning, the dynamic model represents the user's dynamic equations. Different user dynamic situations require different dynamic models, and describing acceleration is crucial for model building. For simplicity, we will only consider the model in a single direction. The following discussion uses the X-axis direction in a geocentric coordinate system as an example; the situation is the same in other directions.

[0051] 1) Design of the "Current" Statistical (CS) Model

[0052] The state variables of the "current" statistical model are position, velocity, and acceleration. Essentially, it is a time-dependent model of non-zero mean maneuvering acceleration. The "current" probability density of the maneuvering acceleration is described by a modified Rayleigh distribution, with the mean being the predicted value of the "current" acceleration. The relationship between the mean and variance can be used to establish an adaptive algorithm for the mean and variance of maneuvering acceleration. Therefore, the continuous state equation in the X-direction of the user's "current" statistical model is:

[0053]

[0054] Where, x(t), and Position, velocity, acceleration, and jerk in the X direction, respectively. Let be the mean acceleration, which is constant within each sampling period; α be the reciprocal of the maneuver time constant; w(t) be the input excitation noise, with a mean of 0 and a variance of . in Let Variance be the target acceleration.

[0055] Its corresponding discrete state equation is:

[0056]

[0057] in,

[0058]

[0059] T is the sampling time interval.

[0060] The one-step prediction equation can then be obtained as follows:

[0061]

[0062] In the formula,

[0063] And the system's driving noise variance matrix is:

[0064]

[0065] In equation (4),

[0066] q 11 =1 / 2α 5 [1-e -2αT +2αT+2 / 3α 3 T 3 -2α 2 T 2 -4αTe -αT ]

[0067] q 12 =1 / 2α 4 [1+e -2αT -2e -αT +2αTe -αT -2αT+α 2 T 2 ]

[0068] q 13 =1 / 2α 3 [1-e -2αT -2αTe -αT ]

[0069] q 22 =1 / 2α 3 [4e -αT -e -2αT -3+2αT]

[0070] q 23 =1 / 2α 2 [e -2αT -2e -αT +1]

[0071] q 33 =1 / 2α[1-e -2αT ]

[0072]

[0073] in This is a one-step prediction estimate of the acceleration. max Given the known positive upper limit of the target acceleration, a -max These are the negative lower limits of the target acceleration, and they are generally pre-designed prior values ​​based on the actual application.

[0074] As can be seen from equation (4), the user noise variance matrix of the "current" statistical model is about a max and a -max Since a is a quadratic function, the performance of the filter is greatly affected by the maximum and minimum values ​​of acceleration. max and a -max When the absolute value of is small, the positioning accuracy is high, but the filter bandwidth is narrow, the range of user maneuverability it can adapt to is limited, and the response speed is slow. When amax and a -max When the absolute value of acceleration is large, the positioning system can quickly track a wider range of user maneuvers with a larger system variance, relative to expanding the filter bandwidth; however, the positioning accuracy is poor. Therefore, this invention aims to compensate for this deficiency using the IMM algorithm. Two different sets of acceleration extrema {a} are designed. max_b a -max_b}, {a max_m a -max_m} to describe the user's high-maneuverability and medium-maneuverability states respectively, namely:

[0075] • A relatively small maximum acceleration value is {a max_m a -max_m The current statistical model for} is denoted as CSM;

[0076] • A maximum acceleration value is {a max_h a -max_h The "current" statistical model of} is denoted as CSH.

[0077] 2) Constant-rate (CV) model design

[0078] The CV model's state variables are three-dimensional position and velocity. It treats acceleration as a random perturbation, describing it as a white noise process. Its continuous state equation is:

[0079]

[0080] In the formula, x(t), and Let represent the position, velocity, and acceleration along the X-axis in the Earth-centered, Earth-fixed coordinate system, respectively, and let w(t) represent the white noise process. Its discrete state equation is:

[0081]

[0082] In the formula,

[0083] And the noise variance matrix of the system is:

[0084] In the formula, Let w(t) be the variance of the continuous state noise, which is the variance of the acceleration.

[0085] It is evident that CV is matched to the user's uniform linear motion state. In this invention, it is selected into the model set to match the user's low-dynamic motion mode.

[0086] Step 2: Design of the Extended Kalman Filter (EKF)

[0087] Each model in the model set corresponds to a Kalman filter used to estimate the user's motion state. The Kalman filter consists of a state model and an observation model. In satellite navigation and positioning, the observations are pseudorange and pseudorange rate. Since the equations for pseudorange and pseudorange rate measurements are nonlinear, a nonlinear Kalman filter, namely the Extended Kalman Filter (EKF), is used. The EKF block diagram is shown below. Figure 2 As shown, the inputs are pseudorange and satellite position, etc., and the outputs are dynamic parameters and time parameters, etc. The state equation is the dynamic model as described in step 1. The measurement equation is a linearized equation for pseudorange and pseudorange rate, and its general form is:

[0088] y(k)=Hx(k)+n(k-1) (8)

[0089] In the formula, y(k) represents the observation quantity, n(k-1) represents the observation noise, and H is the observation matrix. The state variables here have a total of 11 dimensions, namely: Where x, y, z l and i represent the position, velocity, acceleration, clock error, and frequency difference of the three axes in the ECEF coordinate system (Geocentric Earth-Fixed Coordinate System), respectively. When y(k) is a pseudorange observation, the observation matrix is:

[0090] H = [e] i1 0 0 e i2 0 0 e i3 0 0 1 0] m×11 (i = 1, 2, ..., m, where m is the total number of visible stars)

[0091] When y(k) is a pseudorange rate observation, the observation matrix is:

[0092] H = [0 0 e] i1 0 0 e i2 0 0 e i3 0 1] m×11 (i = 1, 2, ..., m, where m is the total number of visible stars)

[0093] Where e i1 e i2 e i3 The direction cosine from the satellite to the user.

[0094] Observation noise variance matrix: R(k)=E[n(k)n(j)] T (9)

[0095] R k This is a random noise matrix consisting of pseudorange or pseudorange rate, typically differentiated by system, with each navigation system corresponding to different statistical values. The flowchart of the extended Kalman filter is shown below. Figure 3As shown, this filtering method is a commonly used filtering method in this field. The filtered output at time k includes a state estimate. (and (meaning the same thing), and the estimated error covariance matrix P k (This has the same meaning as P(k / k)).

[0096] Step 3: Automatic switching method between models

[0097] The interactive multi-model filtering method estimates the state as a deterministic mixture of estimates from multiple parallel-running sub-filters. Assuming N sub-filters correspond to N different models, a Markov chain is used to control the switching between models. For example, at time k-1, model m... j (k-1) model m at time k i (k) transition, the transition probability is defined as:

[0098] P{m i (k)|m j (k-1)}=π ji i,j∈{1,2,…,N} (10)

[0099] This invention designs a time-invariant Markov chain to switch three dynamic models. Its transition probability matrix is ​​designed with a large number of diagonal elements, and the sum of elements in each row is 1, to achieve better state estimation. The transition probabilities are assumed to be as follows:

[0100]

[0101] Where i, j ∈ {1, 2, 3}, and β represents the model switching probability. The value of β can be determined empirically. If the value of β is large, the switching between models will be frequent, which will lead to instability of the model probabilities under noisy conditions. Conversely, if a small value of β is used, the three models will not be able to compensate for each other well.

[0102] Step 4: Interactive Multi-Model Filter Workflow

[0103] like Figure 1 As shown, an interactive multi-model filter generally includes four steps: input interaction, parallel filtering computation of sub-filters, probability update, and output estimation mixing.

[0104] 1) Input Interaction

[0105] Input interaction also initializes each filter. The estimated values ​​and covariances of each sub-filter at time k-1 are weighted and recombined to obtain the current input value for each filter. That is:

[0106]

[0107] The input mixture probability is obtained from Bayes' theorem.

[0108]

[0109] c i For model m i The prior probability of (k) needs to be set to an initial value during the first filtering. It can be evenly distributed among the models, with a total probability of 1. j (k-1) represents the model m obtained after the (k-1)th filtering. j (k-1) model probabilities, π ji As described in step 4. j / i (k / k-1) represents the model m at time k. i (k) is derived from the model m at time k-1. j (k-1) conditional probability derived from the transition.

[0110] 2) Parallel filtering of each sub-filter

[0111] Each sub-filter performs extended Kalman filtering in parallel when the observation y(k) is received, as described in step 3, such as for model m. i (k), whose output has a state estimate Estimation error covariance matrix P i (k / k), new information v i (k) and the new information covariance matrix S i (k).

[0112] 3) Model probability update

[0113] Model m i (k) The probability function of the filter, which measures the accuracy of filter i based on the distribution of the filter's innovation, i.e., the measurement model m. i The degree of matching of (k) can be defined as:

[0114]

[0115] Λ i (k) follows a normal distribution, in which case model m i The probability of (k) is,

[0116]

[0117] In the formula This is the normalization constant.

[0118] The model with the highest probability is the best-matching model at present.

[0119] 4) Output estimation mixture

[0120] Output state estimate:

[0121] Output covariance matrix:

[0122] Model estimation:

[0123] As can be seen, the interactive multi-model filtering method uses Markov chains to achieve automatic switching between models, and its state estimate is the mixture of the sub-filters. Therefore, the interactive multi-model filtering method plays a dual role in model selection and state estimation.

[0124] The invention is further illustrated below with specific numerical values. Taking a GPS system as an example, the user trajectory uses a flight trajectory from an actual flight database, and noise errors are removed through fitting, such as... Figure 4 As shown, the actual trajectory and the fitted trajectory largely overlap. The observation data were obtained by injecting flight trajectory simulation using a Spirent simulator. This trajectory represents the entire flight path of an aircraft from Harbin to Beijing, including various motion states such as takeoff, climb, en route, descent, and approach. The total flight time is 7150 seconds, with a sampling interval of 1 second. The interactive multi-model filtering method of this invention was used to estimate the aircraft's state throughout the entire flight, and the results were compared with single-model filtering.

[0125] The selection of the model set and the design of the models are based on steps 1 and 2. Furthermore, the parameters of each model are designed according to the characteristics of the specific maneuvering scenario and empirical values. The model parameters for this flight scenario are designed as follows:

[0126] •CV model: The variances of noise acceleration during low-speed maneuvers correspond to the three directions in the Cartesian coordinate system, respectively.

[0127] • CSM model: a max_m ={30, 30, 20}, corresponding to the positive upper limits of the medium-speed acceleration in the x, y, and z directions of the Cartesian coordinate system, respectively, a -max_m ={-30, -30, -20}, which corresponds to the three negative lower limits of acceleration. The maneuver frequency α is set to 1 / 60.

[0128] • CSH model: a max_h ={80, 80, 60}, representing the three positive upper limits of higher maneuvering acceleration, a -max_h ={-80, -80, -60}, which corresponds to the three negative lower limits of higher maneuvering acceleration, and the maneuvering frequency α is 1 / 30.

[0129] The observation equation in step 3 is calculated from equation (8) based on the pseudorange, pseudorange rate, and the position of visible stars, and is updated in real time. The pseudorange observation noise variance matrix is:

[0130] The pseudorange rate observation noise variance matrix is:

[0131] m represents the total number of visible stars.

[0132] Furthermore, setting the Markov matrix β value in step 3 to 0.05 allows for timely switching between models when flight modes change. This also increases the probability difference between matched and unmatched models. After designing the model and its parameters, state estimation and model probability updates can be performed according to the process in step 4.

[0133] The interactive multi-model Kalman filter (IMM EKF) designed in this invention was then validated. Comparing the IMM EKF with the conventional single-model EKF, the analysis showed that the IMM EKF reduced the 95% eastward velocity error by 69.1% compared to the CV EKF, and by 51.7% compared to the CS EKF. During the en-route phase, the CV EKF can achieve navigation accuracy similar to the IMM EKF, but its error is significant during takeoff or descent. In terms of horizontal positioning accuracy limits, the CV EKF is an order of magnitude larger than the IMM EKF, indicating that the CV EKF cannot provide sufficient bandwidth for maneuvering states. The CS-based EKF can match user maneuvering states well during takeoff and landing; however, its drawback is a large positioning error during low maneuvering or en-route phases. Therefore, the adaptive IMM EKF designed in this invention can provide relatively accurate and reliable navigation and positioning services for users in various motion states.

[0134] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. An interactive multi-model filtering method for complex motion state estimation in satellite navigation, characterized in that, The steps are as follows: (1) Establish a first CS dynamic model for describing high dynamic motion, a second CS dynamic model for describing medium dynamic motion, and a CV dynamic model for describing low dynamic motion; wherein the maximum target acceleration of the first CS dynamic model is greater than the maximum target acceleration of the second CS dynamic model. CV dynamic model: The variances of noise acceleration during low-speed maneuvers correspond to the three directions in the Cartesian coordinate system, respectively. The user noise variance matrix of the CS dynamic model is about the positive upper bound of the target acceleration a. max And the negative lower limit of the target acceleration a -max A quadratic function; First CS dynamic model: a max_h ={80, 80, 60}, which correspond to the positive upper limits of target acceleration in the x, y, and z directions of the Cartesian coordinate system, respectively; a -max_h = {-80, -80, -60}, which correspond to the negative lower limit of the target acceleration in the x, y, and z directions of the Cartesian coordinate system, respectively, and the maneuver frequency α is 1 / 30; Second CS dynamic model: a max_m ={30, 30, 20}, corresponding to the positive upper limits of target acceleration in the x, y, and z directions of the Cartesian coordinate system, respectively, a -max_m ={-30, -30, -20}, which correspond to the negative lower limits of the target acceleration in the x, y, and z directions of the Cartesian coordinate system, respectively, and the maneuver frequency α is set to 1 / 60; (2) Establish three sub-filters, one for each dynamic model; each sub-filter is an extended Kalman filter. (3) Establish the Markov transition matrix between the three dynamic models; (4) Perform interactive multi-model filtering through the following steps: (4.1) Determine the input mixing probability based on the model probability and Markov transition matrix of each model at the previous time step; obtain the input estimate and covariance of each sub-filter at the current time step by weighted recombining the input mixing probability and the estimated value and covariance of each sub-filter output after filtering at the previous time step. (4.2) Based on the current observation, the input estimate and the covariance, each sub-filter performs extended Kalman filtering in parallel to obtain the estimated value, covariance and innovation of each sub-filter output at the current time; (4.3) Determine the matching degree of each model based on the information of each sub-filter, and determine the model probability corresponding to each model at the current time based on the matching degree of each model; (4.4) The final estimated value and covariance at the current time are obtained by mixing the model probabilities corresponding to each model at the current time, the estimated values ​​and covariance of each sub-filter output at the current time; The Markov transition matrix in step (3) is (π) ij ) 3×3 : i, j∈{1, 2, 3}, β represents the model switching probability; the value of β is 0.05; The formula for calculating the mixing probability in step (4.1) is as follows: Among them, c i For the i-th model m i The prior probability of (k), μ j (k-1) represents the j-th model m obtained after filtering at time k-1. j The model probability of (k-1), μ j / i (k / k-1) represents the model m at time k. i (k) is derived from the model m at time k-1. j (k-1) The conditional probability derived from the transition, i.e., the mixed probability of the input; The calculation formula for step (4.3) is as follows: Where, μ i (k) represents the i-th model m in the filtering process at time k. i The model probability of (k), Λ i (k) represents the i-th model m in the filtering process at time k. i The degree of matching of (k); v i (k) represents the information output by the i-th sub-filter in the filtering process at time k, S i (k) is the covariance matrix of the innovation output of the i-th sub-filter in the filtering at time k; where This is a normalization constant; Each sub-filter includes a state equation and a measurement equation, wherein the state equation is the dynamic model corresponding to that sub-filter; the measurement equation is a linearized equation of the pseudorange rate, and its formula is: y(k)=Hx(k)+n(k-1) In the formula, y(k) is the pseudorange rate observation, n(k-1) is the observation noise, and H is the observation matrix; H=[0 0 e i1 0 0 e i2 00 e i3 0 1] m×11 i = 1, 2, ..., m, where m is the total number of visible stars. in These represent the three axes of position, velocity, acceleration, clock error, and frequency difference in the geocentric-geostatic coordinate system. The variance matrix of the pseudorange rate observation noise is:

Images