CN118443024A - Self-adaptive robust filtering method and system for RTK/INS (real-time kinematic/inertial navigation system) tightly integrated navigation - Google Patents

Abstract

The invention provides a self-adaptive robust filtering method and a self-adaptive robust filtering system for RTK/INS (real-time kinematic/inertial navigation) tightly combined navigation, and belongs to the technical field of communication. According to the invention, the carrier phase double-difference value is calculated through the INS predicted position, the chi-square test is carried out after the square of the Marsh distance of the predicted residual error is calculated, whether the epoch observation vector contains the gross error or not is judged, the gross error is removed according to the minimum detectable deviation, meanwhile, the calculation sequence of the anti-difference factor and the self-adaptive factor is regulated according to whether the INS mechanical arrangement result meets the kinematic constraint or not before filtering, and the multi-factor and single-factor self-adaptive filtering mode is selected according to whether the satellite observation quantity is sufficient or not, so that the damage of integer characteristics of ambiguity parameters is reduced, the characteristics of short-time high precision of the INS are not only provided, the possibility of missed detection and false detection is reduced, the interference of the INS on a dynamic model is reduced, the robustness and the positioning precision of the system are improved, the dynamic environment adaptability of the combined guiding system is improved, and the navigation performance is improved.

Description

Adaptive anti-error filtering method and system for RTK/INS tightly integrated navigation

Technical Field

The present invention relates to the field of communication technology, and in particular to an adaptive anti-error filtering method and system for RTK/INS tightly integrated navigation.

Background technique

The real-time kinematic (RTK) carrier phase differential technology in the Global Navigation Satellite System (GNSS) makes full use of the observation data of the mobile station and the reference station to construct double-difference observations, which can greatly reduce or eliminate some errors. The double-difference observations contain relatively small errors and have the ability to restore the real number solutions obtained from the double-difference observations to integer solutions. The positioning accuracy can reach the centimeter level, and the positioning accuracy is high. However, the GNSS receiver requires at least four satellites to complete the positioning, and the GNSS signal is easily amplified in the obstructed environment such as cities, canyons, tunnels and overpasses due to the interference of the environment, which can easily cause the GNSS receiver to lose lock, affect the navigation accuracy, and in severe cases, there will be problems of being unable to navigate. In addition, there is another inertial navigation system (INS) on the market, which has the ability to independently calculate the position, speed and attitude. Its signal is not easily disturbed by the external environment, has the characteristics of short-term high accuracy, and has a high data update rate. It can provide rich attitude information and is also widely used. However, in actual use, since the positioning of INS is an integral process, its error will accumulate over time, that is, the longer it is used, the worse its positioning accuracy will be. At the same time, when it is used, it is necessary to provide its initial position, speed and attitude first, which is usually used for short-term positioning needs. Therefore, the industry will adopt the RTK/INS combined navigation system to realize the integration of the advantages of RTK and INS and the complementary disadvantages, so as to improve the accuracy of the navigation system and provide users with more accurate, continuous and reliable navigation and positioning information.

At present, with the development of urban infrastructure, there are more and more obstructed environments in cities, which makes the GNSS signals in the RTK/INS integrated navigation system extremely susceptible to multipath effects and non-line-of-sight signal interference, resulting in gross errors in the observations. The Kalman filter used is quite sensitive to observations containing gross errors, that is, individual gross errors will cause deviations in the state estimation of the Kalman filter, which will seriously affect the positioning accuracy of the integrated navigation. Therefore, it is necessary to perform gross error elimination and robust estimation in the integrated navigation process.

Summary of the invention

In view of the above problems existing in the prior art, the present invention aims to provide an adaptive anti-error filtering method and system for RTK/INS tightly integrated navigation, which adjusts the calculation order of anti-error factors and adaptive factors based on kinematic constraints, thereby improving the robustness and positioning accuracy of the integrated navigation system. At the same time, a combination of multi-factor adaptive filtering and single-factor adaptive filtering is adopted according to the number of satellite observations, which effectively reduces the damage to the integer characteristics of the ambiguity parameters and improves the ability of the integrated navigation system to adapt to dynamic environments.

The specific technical solutions are as follows:

The adaptive anti-error filtering method for RTK/INS tightly integrated navigation has the following characteristics and includes the following steps:

Step S1, one-step prediction of Kalman filter state;

Establish a RTK/INS tightly combined discrete state space model, perform one-step state prediction based on the Kalman filter formula, and use the GNSS observation vector and state prediction vector to calculate the prediction residual vector and prediction residual covariance matrix;

Step S2, gross error detection and elimination;

The prediction residual vector is constructed using the mechanical arrangement results of INS, and the square of the prediction residual Mahalanobis distance is calculated, and a chi-square test is performed on it. If the test result falls outside the confidence interval, each observation vector is tested to confirm whether it contains gross errors, realize gross error detection, and eliminate observation vectors containing gross errors;

Step S3, judging whether kinematic constraints are satisfied;

Determine whether the vehicle satisfies the kinematic constraints. If so, calculate the robustness factor first and then the adaptive factor. Otherwise, calculate the adaptive factor first and then the robustness factor. When the number of observations is sufficient, a multi-factor adaptive method is used. When the number of observations is insufficient, a single-factor adaptive method is used. The observation equivalent weight matrix and the state prediction vector adaptive covariance matrix are constructed based on the adaptive factor and the robustness factor.

Step S4, Kalman filter estimation, and feedback of the corrected predicted state vector;

The Kalman filter gain is updated according to the observation equivalent weight matrix and the state prediction vector adaptive covariance matrix to obtain the optimal estimation of the Kalman filter, and the predicted state is corrected by feedback to improve the integrated navigation positioning accuracy.

In the above-mentioned adaptive anti-error filtering method for RTK/INS tightly integrated navigation, step S1 further includes the following steps:

Step 1.1, establish a RTK/INS tightly coupled discrete state space model including a state model and an observation model;

Step 1.2, update the time according to the state estimation value at the previous moment, and calculate the state one-step prediction vector and the covariance matrix of the predicted state vector;

Step 1.3, use the GNSS observation vector and the state one-step prediction vector to calculate the prediction residual vector and the prediction residual covariance matrix

In the above-mentioned adaptive anti-error filtering method for RTK/INS tightly integrated navigation, step S2 further includes the following steps:

Step 2.1: Use the INS predicted position to calculate the carrier phase double difference, and then subtract it from the GNSS observed phase double difference to obtain the prediction residual vector based on the INS predicted value.

Step 2.2, use Calculate the square of the prediction residual Mahalanobis distance γ _k,k-1 with the prediction residual covariance matrix;

Step 2.3, perform a chi-square test on γ _k,k-1. If γ _k,k-1 falls outside the chi-square test confidence interval, it is determined that the epoch observation vector contains a gross error, otherwise it does not contain a gross error;

Step 2.4: If a gross error is detected in the observation vector, calculate the minimum detectable deviation b _j of each prediction residual, where j = 1, 2…n _k ;

Step 2.5, compare the prediction residual vector The size of each element in and the corresponding minimum detectable deviation _bj , if If , the observation vector is considered to contain no gross errors, otherwise, the observation vector is considered to contain gross errors and is eliminated.

In the above-mentioned adaptive anti-error filtering method for RTK/INS tightly integrated navigation, step S3 further includes the following steps:

Step 3.1, determine whether the result of the current INS mechanical arrangement satisfies the kinematic constraints. If the kinematic constraints are met, the robustness factor is calculated first and then the adaptive factor is calculated; if the kinematic constraints are not met, the adaptive factor is calculated first and then the robustness factor is calculated;

Step 3.2, use the prediction residual vector and the prediction residual covariance matrix Calculate prediction residual statistics And use the prediction residual statistics Calculate the robustness factor λ _ii and calculate the observation equivalent weight matrix based on the robustness factor λ _ii

Step 3.3, for epochs with sufficient number of observations, a multi-factor adaptive filtering method is used to calculate the adaptive factors of the position and velocity vectors;

In step 3.4, a single-factor adaptive filtering method is used for epochs with insufficient number of observations to calculate a unified adaptive factor amplified state prediction covariance matrix.

In the above-mentioned adaptive anti-error filtering method for RTK/INS tightly integrated navigation, step S4 further comprises the following steps:

Step 4.1, using the observation equivalent weight matrix and the state prediction vector adaptive covariance matrix to obtain the equivalent gain matrix K _k ;

Step 4.2, based on the equivalent gain matrix K _k , calculate the state estimate and the state vector post-test covariance matrix;

Step 4.3, correct the position, speed and attitude of the navigation output according to the estimation to improve the positioning accuracy of the integrated navigation.

In the above-mentioned adaptive anti-error filtering method for RTK/INS tightly integrated navigation, step 3.2 further includes the following steps:

Step 3.2.1, use the prediction residual vector and the prediction residual covariance matrix Calculate prediction residual statistics

Step 3.2.2, based on the prediction residual statistics Calculate the robustness factor λ _ii ;

Step 3.2.3, after obtaining the robustness factor λ _ii of each observation vector, construct the observation equivalence weight matrix

In the above-mentioned adaptive anti-error filtering method for RTK/INS tightly integrated navigation, step 3.3 further includes the following steps:

Step 3.3.1: Obtain the robust estimate of the position vector from the observation information of the current epoch

Step 3.3.2: The robust estimate of the position vector and the position prediction vector The difference is calculated to obtain the discrepancy value of each component of the position prediction vector Then calculate the adaptive factor of the position component

Step 3.3.3, from the robust estimate of the position vector The adaptive robust filtering estimate of the position vector at t _k-1 epoch Get a more accurate velocity vector

Step 3.3.4, transform the velocity vector With the predicted velocity vector The calculated predicted velocity component does not match the value Then calculate the adaptive factor component of the velocity component

Step 3.3.5, using the adaptive factor of the position component and the adaptive factors of the velocity components Calculate the state prediction vector adaptive covariance matrix

Step 3.3.6, perform this step when the kinematic constraints are not met, and adapt the covariance matrix according to the state prediction vector Calculate the prediction residual covariance matrix after adaptive processing Then use the new prediction residual covariance matrix Calculate the robustness factor.

In the above-mentioned adaptive anti-error filtering method for RTK/INS tightly integrated navigation, step 3.4 further includes the following steps:

Step 3.4.1, construct prediction residual statistics Depend on Calculate the adaptive factor α _k ;

Step 3.4.2, use the adaptive factor α _k to calculate the state prediction vector adaptive covariance matrix

Step 3.4.3, perform this step when the kinematic constraints are not met, according to the state prediction vector adaptive covariance matrix Calculate the prediction residual covariance matrix after adaptive processing Then use the new prediction residual covariance matrix Calculate the robustness factor.

The adaptive anti-error filtering system of RTK/INS tightly integrated navigation has the following technical features:

The measurement data recording module records the vehicle receiver GNSS observation data, base station GNSS observation data and INS mechanical arrangement results at the current moment, and the recorded data is used for the RTK/INS integrated navigation system;

Observation gross error elimination module, which uses the INS mechanical arrangement results to calculate the double difference phase observation value and eliminates the observation vector containing gross errors in the GNSS observation;

The judgment module is used to judge whether the INS mechanical arrangement results meet the kinematic constraints and whether the number of GNSS observations in the current epoch is sufficient;

The robustness and self-adaptation module is used to construct different observation equivalent weight matrices and prediction residual adaptive covariance matrices according to the judgment results of the judgment module;

Filter module: The filter module is used for adaptive robust Kalman filter calculation and outputs state error estimation results;

The feedback correction module uses the state error estimation result output by the filtering module to correct the position, speed and attitude output by the integrated navigation system.

The positive effects of the above technical solution are:

The above-mentioned adaptive anti-error filtering method and system for RTK/INS tightly integrated navigation calculates the double difference of the carrier phase by using the INS predicted position, and uses this to calculate the square of the predicted residual Mahalanobis distance and then performs a chi-square test to determine whether the epoch observation vector contains gross errors and eliminates gross errors based on the minimum detectable deviation. The short-term high-precision characteristics of INS are effectively utilized, and the possibility of missed detection and false detection is effectively reduced. In addition, before filtering, the calculation order of the anti-error factor and the adaptive factor is adjusted according to whether the INS mechanical arrangement result meets the kinematic constraints, which effectively reduces the interference of the low-cost INS on the dynamic model and improves the robustness and positioning accuracy of the system. In addition, multi-factor and single-factor adaptive filtering methods are adopted according to whether the number of satellite observations is sufficient, which reduces the destruction of the integer characteristics of the ambiguity parameters, improves the dynamic environment adaptability of the combined guidance system, and has a better navigation effect.

BRIEF DESCRIPTION OF THE DRAWINGS

1 is a flow chart of an adaptive anti-error filtering method for RTK/INS tightly integrated navigation of the present invention;

FIG2 is a flow chart of the gross error detection and elimination process in step S2 of the present invention;

FIG3 is a flow chart of the determination of satisfying kinematic constraints in step S3 of the present invention;

FIG4 is a schematic diagram of the structure of the adaptive anti-error filtering system for RTK/INS tightly integrated navigation of the present invention.

In the attached figure: 1. Measurement data recording module; 2. Observation gross error elimination module; 3. Judgment module; 4. Anti-error and adaptive module; 5. Filtering module; 6. Feedback correction module.

Detailed ways

In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the following embodiments are combined with Figures 1 to 4 to specifically illustrate the technical solutions provided by the present invention, but the following content is not intended to limit the present invention.

Fig. 1 is a flow chart of the adaptive anti-error filtering method for RTK/INS tightly integrated navigation of the present invention. As shown in Fig. 1, the adaptive anti-error filtering method for RTK/INS tightly integrated navigation provided by this embodiment includes the following steps: step S1, one-step prediction of Kalman filter state; step S2, gross error detection and elimination; step S3, kinematic constraint judgment; step S4, Kalman filter estimation, and feedback correction of predicted state vector.

Specifically, step S1, one-step prediction of Kalman filter state;

A RTK/INS tightly combined discrete state space model is established, and the state is predicted in one step according to the Kalman filter formula. The prediction residual vector and the prediction residual covariance matrix are calculated using the GNSS observation vector and the state prediction vector.

More specifically, step S1 further includes:

Step 1.1, establish a RTK/INS tightly coupled discrete state space model including a state model and an observation model.

The state model is: X _k =Φ _k,k-1 X _k-1 +W _k ;

The observation model is: Z _k =H _k X _k +e _k ;

In the above formula, _Xk is the state parameter vector at time _tk ; Φk _,k-1 is the state transfer matrix; Xk _-1 is the state parameter vector at time _tk-1 ; _Wk is the dynamic model noise vector; _Zk is the observation vector; _Hk is the design matrix; _ek is the observation noise vector; in addition, it is assumed that _Wk and _ek are zero-mean Gaussian distributed white noise and are uncorrelated.

Step 1.2, update the time according to the state estimate of the previous moment, and calculate the state one-step prediction vector and the covariance matrix of the predicted state vector:

In the above formula, is the error state vector; Φ is the state transfer matrix; is the predicted state vector covariance matrix; Φ ^T is the transpose of the state transfer matrix; is the system noise covariance matrix; and the subscripts of the above parameters represent the current state or the change of state, for example, is the error state vector of the k-1 epoch, represents the error state vector from epoch k-1 to epoch k.

It is worth pointing out that only the RTK/INS tight combination under the short baseline condition is considered, which is based on the double-difference pseudorange and double-difference carrier phase observations. Therefore, the GNSS-related parameters only include the single-difference ambiguity error parameters between stations. The following RTK/INS tight combination state vector X is established.

In the above formula, represents the misalignment angle error, δv ⁿ represents the velocity error, δr ⁿ represents the attitude error, _δba and δb _g represent the gyro and accelerometer bias errors respectively, and δN _rb represents the single difference ambiguity error vector between stations.

In addition, the above state transfer matrix Φ can be expanded as follows:

In the above formula, I _n×n represents the n×n unit matrix. In addition, Φ _INS can be expanded to: In the above formula,

In addition, in the above formula, Δt is the sampling time interval and its value is 0.01s; is the projection of the rotation rate of the n system relative to the i system in the n system, and in: f ^b is the specific force value output by the accelerometer; (*)× represents the antisymmetric matrix of the orientation quantity; is the conversion matrix from the carrier coordinate system to the navigation coordinate system; _RM and _RN are the curvature radii of the earth meridian and meridian respectively; _νN , _νE , _νD are the velocities of the carrier in the north, east and ground directions under the navigation coordinates respectively. and h represent the latitude and altitude in the traditional earth ellipsoid coordinate system, _ωe represents the angular velocity of the earth's rotation, _gp represents the local gravity projection, _τg and _τa represent the correlation time of the first-order Markov process of the gyroscope and accelerometer, respectively.

In addition, the above system noise covariance matrix It can be expanded as follows:

In the above formula, It can be expanded as follows:

In the above formula, and They represent the gyro noise standard deviation, accelerometer noise standard deviation, gyro bias instability and accelerometer bias instability respectively, and take constant values during calculation. In addition, Δt is the sampling time interval and its value is 0.01s.

in addition, It can be expanded as follows:

In the above formula, σ _N is the random walk noise of the single difference ambiguity, and Δt is the GNSS sampling time interval.

Step 1.3, use the GNSS observation vector and the state one-step prediction vector to calculate the prediction residual vector and the prediction residual covariance matrix at this time,

In the above formula, _Rk is the covariance matrix of the observation vector at time _tk , and H is the design matrix, which is expanded as follows:

In the above formula, E is the sight vector matrix, is the transformation matrix from the position error in the geodetic coordinate system to the position error in the ECEF coordinate system, is the conversion matrix from single-difference ambiguity to double-difference ambiguity, and λ is a constant.

Step S2, gross error detection and elimination;

Fig. 2 is a flow chart of the gross error detection and elimination process in step S2 of the present invention. As shown in Fig. 2, in step S2, the prediction residual vector is constructed using the INS mechanical arrangement result, and the square of the prediction residual Mahalanobis distance is calculated, and a chi-square test is performed on it. If the test result falls outside the confidence interval, each observation vector is tested to confirm whether it contains gross errors, to achieve gross error detection, and to eliminate the observation vector containing gross errors.

Specifically, step S2 includes the following steps:

Step 2.1: Use the INS predicted position to calculate the carrier phase double difference, and then subtract it from the GNSS observed phase double difference to obtain the prediction residual vector based on the INS predicted value. And, the prediction residual vector based on the INS prediction value It can be expressed as follows:

and,

In the above formula, is the prediction residual vector based on the INS prediction value at time k, is the double-difference carrier phase observation value at time k, is the observation function of the carrier phase, is the double difference geometric distance calculated for the INS predicted position, is the double difference integer ambiguity, and λ is a constant.

Step 2.2, use and the prediction residual covariance matrix The square of the prediction residual Mahalanobis distance γ _k,k-1 is calculated, and γ _k,k-1 can be expressed as:

In the above formula, if there are n _k observations in t _k epoch, then γ _k,k-1 follows a chi-square distribution with n _k degrees of freedom.

Step 2.3, perform a chi-square test on γ _k,k-1 . If γ _k,k-1 falls outside the chi-square test confidence interval, it is determined that the epoch observation vector contains a gross error. Otherwise, it does not contain a gross error. At this time, the chi-square test confidence interval is 1-α ₀ , and,

α ₀ =Pr(γ _k,k-1 >χ _α,0 );

In the above formula, _α0 is the significance level of the gross error, which is a small value. In this embodiment, the preferred value is 0.01, and χα _,0 is the 1- _α0 confidence bound of the chi-square distribution. Pr( _γk,k-1 >χα _,0 ) represents the probability that _γk,k-1 is greater than χα _,0 and the probability value is _α0 .

Step 2.4, if a gross error is detected in the observation vector, calculate the minimum detectable deviation (MDB) of each prediction residual, represented by _bj . At this time, the minimum detectable deviation (MDB) provides an important diagnostic tool to know how large a certain error must be to be correctly detected. At this time, the calculation formula of _bj is as follows:

In the above formula, λ _min is the minimum value of the non-centrality parameter under the given significance level α and test power 1-β, b _j is the minimum detectable deviation, Indicates taking the diagonal elements of the prediction residual covariance matrix, and, and j = 1, 2, ... _nk . In addition, for the alternative hypothesis with a degree of freedom of 1, that is, when detecting whether an observation contains a gross error, when setting α = 0.1% and β = 20%, λ _min is equal to 17.0569.

Step 2.5, compare the prediction residual vector Each element and the corresponding minimum detectable deviation _bj , if If , the observation vector is considered to contain no gross errors, otherwise, the observation vector is considered to contain gross errors and is eliminated.

Step S3, judging whether kinematic constraints are satisfied;

Fig. 3 is a flow chart of the kinematic constraint judgment in step S3 of the present invention. As shown in Fig. 3, it is necessary to judge whether the vehicle satisfies the kinematic constraint. If it does, the robustness factor is calculated first and then the adaptive factor is calculated. Otherwise, the adaptive factor is calculated first and then the robustness factor is calculated. When the number of observations is sufficient, a multi-factor adaptive method is adopted. When the number of observations is insufficient, a single-factor adaptive method is adopted. The observation equivalent weight matrix and the state prediction vector adaptive covariance matrix are constructed according to the adaptive factor and the robustness factor.

Specifically, step S3 includes the following steps:

Step 3.1, determine whether the result of the current INS mechanical arrangement meets the kinematic constraints. If the kinematic constraints are met, the robustness factor is calculated first and then the adaptive factor; if the kinematic constraints are not met, the adaptive factor is calculated first and then the robustness factor.

Step 3.2, use the prediction residual vector and the prediction residual covariance matrix Calculate prediction residual statistics And use the prediction residual statistics Calculate the robustness factor λ _ii and calculate the observation equivalent weight matrix based on the robustness factor λ _ii

More specifically, step 3.2 includes the following steps:

Step 3.2.1, use the prediction residual vector and the prediction residual covariance matrix Calculate prediction residual statistics At this point, the prediction residual statistic is It can be expressed as:

In the above formula, for The i-th element of for The i-th diagonal element of .

Step 3.2.2, based on the prediction residual statistics Calculate the robustness factor λ _ii . At this time, the robustness factor can be expressed as a two-stage function as follows:

In the above formula, c=1.0.

Step 3.2.3, after obtaining the robustness factor λ _ii of each observation vector, construct the observation equivalence weight matrix At this time, the observation equivalence weight matrix It can be expressed as:

In the above formula, R _ii and R _jj are both diagonal elements of the observation vector covariance matrix; R _ij is the non-diagonal element of the observation vector covariance matrix; λ _jj is the value calculated using the jth element when calculating the above robustness factor.

Step 3.3: For epochs with sufficient observations, a multi-factor adaptive filtering method is used to calculate the adaptive factors of the position and velocity vectors. and

More specifically, step 3.3 includes the following steps:

Step 3.3.1, obtain the robust estimate of the position vector from the observation information Z _k of the current epoch at this time, It can be expressed as:

In the above formula, is the design matrix corresponding to the position vector; is the transpose of the design matrix corresponding to the position vector; is the observation equivalence weight matrix.

Step 3.3.2: The robust estimate of the position vector and the position prediction vector The difference is calculated to obtain the discrepancy value of each component of the position prediction vector Then calculate the adaptive factor of the position component The specific calculation is as follows:

At this time, the adaptive factor of the position component for:

In the above formula, is the position prediction vector The variance of each component, and c takes the value of 1.0.

Step 3.3.3, from the robust estimate of the position vector The adaptive robust filtering estimate of the position vector at t _k-1 epoch Get a more accurate velocity vector at this time, It can be expressed as:

Step 3.3.4, transform the velocity vector With the predicted velocity vector The calculated predicted velocity component does not match the value Then calculate the adaptive factor component of the velocity component The specific calculation is as follows:

In the above formula, is the predicted velocity vector The variance of each component, and c takes the value of 1.0.

Step 3.3.5, using the adaptive factor of the position component and the adaptive factors of the velocity components Calculate the state prediction vector adaptive covariance matrix and,

The off-diagonal elements of the position and velocity components are:

The diagonal elements of the position and velocity components are:

Step 3.3.6, perform this step when the kinematic constraints are not met, and adapt the covariance matrix according to the state prediction vector Calculate the prediction residual covariance matrix after adaptive processing Then use the new prediction residual covariance matrix Calculate the robustness factor. At this time, It is expressed as follows:

In the above formula, _Rk is the covariance matrix of the observation vector at time _tk , and _Hk is the design matrix.

In step 3.4, a single-factor adaptive filtering method is used for epochs with insufficient number of observations to calculate a unified adaptive factor amplified state prediction covariance matrix.

More specifically, step 3.4 includes the following steps:

Step 3.4.1, construct prediction residual statistics Depend on Calculate the adaptive factor α _k , then,

In the above formula, is the prediction residual vector; for The transpose of ; c preferably takes a value of 1.0.

Step 3.4.2, use the adaptive factor α _k to calculate the state prediction vector adaptive covariance matrix at this time, The calculation of is as follows:

In the above formula, is the predicted state vector covariance matrix.

Step 3.4.3, perform this step when the kinematic constraints are not met, according to the state prediction vector adaptive covariance matrix Calculate the prediction residual covariance matrix after adaptive processing Then use the new prediction residual covariance matrix Calculate the robustness factor, and, It is expressed as follows:

In the above formula, _Rk is the covariance matrix of the observation vector at time _tk , and _Hk is the design matrix.

Step S4: Kalman filter estimation, and feedback of the corrected predicted state vector.

Specifically, according to the observation equivalence weight matrix Adaptive covariance matrix with state prediction vector Update the Kalman filter gain to obtain the optimal Kalman filter estimate, and perform feedback correction on the predicted state to improve the integrated navigation positioning accuracy.

More specifically, step S4 is performed through the following steps:

Step 4.1, using the observation equivalence weight matrix The adaptive covariance matrix with the state prediction vector The equivalent gain matrix K _k is obtained, and the equivalent gain matrix K _k is calculated as follows:

Step 4.2: Calculate the state estimate based on the equivalent gain matrix K _k and the state vector posterior covariance matrix And the calculation method is as follows:

Step 4.3, correct the position, speed and attitude of the navigation output according to the estimation to improve the positioning accuracy of the integrated navigation.

In addition, the present embodiment also provides an adaptive anti-error filtering system for RTK/INS tightly integrated navigation, and FIG4 is a schematic diagram of the structure of the adaptive anti-error filtering system for RTK/INS tightly integrated navigation of the present invention. As shown in FIG4, the adaptive anti-error filtering system for RTK/INS tightly integrated navigation provided in the present embodiment includes a measurement data recording module 1, an observation gross error elimination module 2, a judgment module 3, an anti-error and adaptive module 4, a filtering module 5, and a feedback correction module 6.

Specifically, the measurement data recording module 1 is used to record the vehicle receiver GNSS observation data, base station GNSS observation data and INS mechanical arrangement results at the current moment, and the recorded data is used for the RTK/INS combined navigation system.

Specifically, the observation gross error elimination module 2 is a structure that uses the INS mechanical arrangement results to calculate the double difference phase observation value and eliminates the observation vector containing gross errors in the GNSS observation.

Specifically, the judgment module 3 is used to judge whether the INS mechanical arrangement result meets the kinematic constraints, such as vehicle speed judgment, and whether the number of GNSS observations in the current epoch is sufficient.

Specifically, the robustness and adaptation module 4 is used to construct different observation equivalent weight matrices and prediction residual adaptive covariance matrices based on the judgment result of the judgment module 3. At this time, the judgment result of the judgment module 3 includes an adaptive factor and a robustness factor.

Specifically, the filtering module 5 is used for adaptive robust Kalman filtering calculation and outputs a state error estimation result.

Specifically, the feedback correction module 6 uses the state error estimation result output by the filtering module 5 to correct the position, speed and attitude output by the integrated navigation system to improve the positioning accuracy of the integrated navigation system.

The adaptive anti-error filtering method and system for RTK/INS tightly integrated navigation provided in this embodiment calculates the double difference of the carrier phase through the INS predicted position, and uses this to calculate the square of the predicted residual Mahalanobis distance and then performs a chi-square test to determine whether the epoch observation vector contains gross errors, and eliminates the observation vector containing gross errors after comparing it with the minimum detectable deviation. At the same time, before filtering, the calculation order of the anti-error factor and the adaptive factor is adjusted according to whether the INS mechanical arrangement result meets the kinematic constraints, and the multi-factor and single-factor adaptive filtering methods are selected according to whether the number of satellite observations is sufficient, thereby reducing the destruction of the integer characteristics of the ambiguity parameters, not only having the characteristics of short-term high precision of INS, reducing the possibility of missed detection and false detection, but also reducing the interference of INS on the dynamic model, improving the robustness and positioning accuracy of the system, improving the dynamic environment adaptability of the combined guidance system, and improving the navigation performance.

The above are only preferred embodiments of the present invention, and are not intended to limit the implementation methods and protection scope of the present invention. Those skilled in the art should be aware that all solutions obtained by equivalent substitutions and obvious changes made using the description and illustrations of the present invention should be included in the protection scope of the present invention.

Claims (9)

1. The adaptive robust filtering method for the RTK/INS tightly integrated navigation is characterized by comprising the following steps of:

   Step S1, one-step prediction of a Kalman filtering state;

   Establishing an RTK/INS tightly-combined discrete state space model, carrying out state one-step prediction according to a Kalman filtering formula, and calculating a prediction residual vector and a prediction residual covariance matrix by using a GNSS observation vector and a state prediction vector;

   S2, detecting and rejecting rough differences;

   Constructing a prediction residual vector by using an INS mechanical arrangement result, solving the square of the Marsh distance of the prediction residual, carrying out chi-square test on the prediction residual vector, if the test result falls outside a confidence interval, testing each observation vector, confirming whether the observation vector contains a coarse difference, realizing coarse difference detection, and eliminating the observation vector containing the coarse difference;

   s3, satisfying kinematic constraint judgment;

   Judging whether the vehicle meets kinematic constraint, if so, calculating an anti-difference factor and then calculating an adaptive factor, otherwise, calculating the adaptive factor and then calculating the anti-difference factor, adopting a multi-factor adaptive mode when the number of observations is sufficient, adopting a single-factor adaptive mode when the number of observations is insufficient, and constructing an observation equivalent weight matrix and a state prediction vector adaptive covariance matrix according to the adaptive factor and the anti-difference factor;

   s4, kalman filtering estimation is carried out, and a prediction state vector is fed back and corrected;

   And updating the Kalman filtering gain according to the observation equivalent weight matrix and the state prediction vector self-adaptive covariance matrix to obtain Kalman filtering optimal estimation, and carrying out feedback correction on the prediction state to improve the integrated navigation positioning accuracy.
2. 2. The adaptive robust filtering method for RTK/INS tight integrated navigation according to claim 1, wherein the step S1 further comprises the steps of:

   Step 1.1, establishing an RTK/INS tightly-combined discrete state space model comprising a state model and an observation model;

   Step 1.2, performing time update according to the state estimation value of the previous moment, and calculating a state one-step prediction vector and a prediction state vector covariance matrix;

   Step 1.3, calculating a prediction residual vector by using the GNSS observation vector and the state one-step prediction vector Covariance matrix with prediction residual
3. 3. The adaptive robust filtering method for RTK/INS tight integrated navigation according to claim 2, wherein the step S2 further comprises the steps of:

   step 2.1, calculating carrier phase double difference values by using INS predicted positions, and differencing the carrier phase double difference values with the GNSS observed phase double difference values to obtain a predicted residual vector based on the INS predicted values

   Step 2.2, utilizeCalculating square gamma _k,k-1 of the Mars distance of the prediction residual with the covariance matrix of the prediction residual;

   Step 2.3, carrying out chi-square test on gamma _k,k-1, if gamma _k,k-1 falls outside the chi-square test confidence interval, judging that the epoch observation vector contains the gross error, otherwise, not containing the gross error;

   Step 2.4, if the coarse difference is detected in the observation vector, calculating a minimum detectable deviation b _j of each prediction residual, and j=1, 2 … n _k;

   step 2.5, comparing the prediction residual vector The size of each element of (a) and the corresponding minimum detectable deviation b _j, ifThe observed vector is considered to contain no gross errors, otherwise the observed vector is considered to contain gross errors and is rejected.
4. 4. The method for adaptive robust filtering for RTK/INS integrated navigation according to claim 3, wherein the step S3 further comprises the steps of:

   Step 3.1, judging whether the current INS mechanical arrangement result meets the kinematic constraint, and calculating an robust factor and then a self-adaptive factor when the kinematic constraint is met; when the kinematic constraint is not satisfied, calculating the self-adaptive factor and then calculating the robust factor;

   Step 3.2, using the prediction residual vector Covariance matrix with prediction residualCalculating prediction residual statisticsUsing prediction residual statistics in parallelCalculating an anti-difference factor lambda _ii, and calculating an observation equivalent weight matrix according to the anti-difference factor lambda _ii

   Step 3.3, calculating the self-adaptive factors of the position and the speed vectors by adopting a multi-factor self-adaptive filtering mode for the calendar with sufficient observation quantity;

   And 3.4, calculating a unified adaptive factor amplification state prediction covariance matrix by adopting a single factor adaptive filtering mode for the epochs with insufficient observation quantity.
5. 5. The adaptive robust filtering method for RTK/INS tight integrated navigation of claim 4, wherein the step S4 further comprises the steps of:

   Step 4.1, obtaining an equivalent gain matrix K _k by using an observation equivalent weight matrix and a state prediction vector self-adaptive covariance matrix;

   Step 4.2, calculating a state estimation and a state vector post-test covariance matrix based on the equivalent gain matrix K _k;

   and 4.3, correcting the position, speed and gesture of the navigation output according to the estimation, and improving the positioning accuracy of the integrated navigation.
6. 6. The adaptive robust filtering method for RTK/INS tight integrated navigation of claim 4, wherein the step 3.2 further comprises the steps of:

   step 3.2.1 using the prediction residual vector Covariance matrix with prediction residualCalculating prediction residual statistics

   Step 3.2.2, based on the prediction residual statisticsCalculating an anti-difference factor lambda _ii;

   step 3.2.3, after obtaining the robust factor lambda _ii of each observation vector, constructing an observation equivalent weight matrix
7. 7. The adaptive robust filtering method for RTK/INS tight integrated navigation of claim 6, wherein step 3.3 further comprises the steps of:

   step 3.3.1, obtaining robust estimation value of the position vector from the observation information of the current epoch

   Step 3.3.2, robust estimation of position vectorAnd a position prediction vectorDifference to obtain the disagreement value of each component of the position prediction vectorThen calculate the adaptive factor of the position component

   Step 3.3.3, estimating the value from the robust of the position vectorPosition vector adaptive robust filter estimate with t _k-1 epochObtaining a more accurate velocity vector

   Step 3.3.4, velocity vector

   And a predicted velocity vectorDifference, calculating predicted speed component disagreement valueThen calculate the adaptive factor component of the velocity component

   Step 3.3.5 using the adaptive factor of the position componentAnd an adaptive factor for the velocity componentComputing a state prediction vector adaptive covariance matrix

   Step 3.3.6, performing this step when the kinematic constraint is not satisfied, adapting the covariance matrix according to the state prediction vectorCalculating a prediction residual covariance matrix after self-adaption processingThen using new prediction residual covariance matrixAnd calculating an robust factor.
8. 8. The adaptive robust filtering method for RTK/INS tight integrated navigation of claim 7, wherein the step 3.4 further comprises the steps of:

   step 3.4.1, constructing prediction residual statistics From the following componentsCalculating an adaptive factor alpha _k;

   step 3.4.2, calculating a state prediction vector adaptive covariance matrix using the adaptive factor alpha _k

   Step 3.4.3, performing this step when the kinematic constraint is not satisfied, adapting the covariance matrix according to the state prediction vectorCalculating a prediction residual covariance matrix after self-adaption processingThen using new prediction residual covariance matrixAnd calculating an robust factor.
9. An adaptive robust filtering system for RTK/INS tight integrated navigation, characterized in that the adaptive robust filtering method for RTK/INS tight integrated navigation as described in any one of claims 1-7 comprises:

   The system comprises a measurement data recording module, a navigation system and a navigation system, wherein the measurement data recording module records GNSS (Global navigation satellite System) observation data of a vehicle-mounted receiver, GNSS observation data of a base station and INS mechanical arrangement results at the current moment, and the recorded data are used for an RTK/INS integrated navigation system;

   The observation coarse difference eliminating module is used for calculating a double-difference phase observation value by using an INS mechanical arrangement result and eliminating an observation vector containing coarse differences in GNSS observation;

   The judging module is used for judging whether the INS mechanical arrangement result meets kinematic constraint and whether the current epoch GNSS observation quantity is sufficient or not;

   The robust and adaptive module is used for constructing different observation equivalent weight matrixes and prediction residual error adaptive covariance matrixes according to the judging result of the judging module;

   the filtering module is used for self-adaptive robust Kalman filtering calculation and outputting a state error estimation result;

   and the feedback correction module corrects the position, the speed and the gesture output by the integrated navigation system by using the state error estimation result output by the filtering module.