Integrated navigation method based on fault-tolerant Kalman filtering

Specific embodiment one:

[0061] The purpose of the present invention is to provide an integrated navigation method based on fault-tolerant Kalman filtering. It is aimed at the SINS/GPS/Doppler multi-integrated navigation system. The structure diagram of the system is as follows figure 1 Shown. among them figure 2 For the federated Kalman filter structure with reset and image 3 It is a federated Kalman filter structure without reset. The present invention is optimized on the structure of a federated Kalman filter without reset, and a fault-tolerant Kalman filter and a fault detection function are added to one of the sub-filters.

[0062] The technical scheme adopted by the present invention includes the following steps:

[0063] Step 1, choose As a state variable,

[0064] among them: δλ: Inertial navigation system latitude and longitude error;

[0065] δV x ,δV y : The speed error of the inertial navigation system in the east and north directions of the geographic coordinate system;

[0066] ε x ε y ε z : Random error of first-order Markov process of gyro;

[0067] αβλ: platform error angle of east, north and sky direction of strapdown inertial navigation in geographic coordinate system;

[0068] Then, according to the system characteristics, list the state equation of the system:

[0069] The SINS/GPS subsystem selects position and speed as quantity measurement,

[0070] which is

[0071] Where: λ I , V IX , V IY The latitude, longitude, eastward speed, and northward speed calculated by inertial navigation respectively;

[0072] λ G , V GX , V GY The GPS measured latitude, longitude, east speed, and north speed.

[0073] SINS/Doppler subsystem chooses speed as the quantity measurement,

[0074] which is

[0075] Where: V IX , V IY Respectively the eastward speed and the northward speed calculated by the inertial navigation;

[0076] V dX , V dY , Are the eastward velocity and northward velocity measured by Doppler respectively.

[0077] Discretize separately to get

[0078]

[0079] Where: Φ i k/k-1 , H ik , Γ i,k-1 , W i,k-1 , V i,k It is the state transition matrix, measurement matrix, system noise driving matrix, system noise matrix, and measurement noise matrix of the i-th subsystem.

[0080] Step 2. Construct a conventional Kalman filter for state estimation. The conventional Kalman filtering process is divided into two processes: time update and measurement update: use time update to obtain a one-step prediction of the state And variance one-step prediction P k/k-1; Use the measurement update to find the estimated value of the state And the estimated value of variance P k. The specific process is as follows:

[0081] One-step state prediction equation:

[0082] State estimation calculation equation:

[0083] Filter gain equation:

[0084] One-step prediction mean square error equation:

[0085] Estimate the mean square error equation:

[0086] Step 3. Construct a fault detection function for detection:

[0087] Use residual χ 2 In the test method, the residual of the filter is defined as: The forecast value in the formula is

[0088] By the residual r k The test of the mean value can determine whether the system has failed.

[0089] Right r k Make the following binary assumptions:

[0090] H 0 , When there is no fault:

[0091] H 0 , When there is a fault: E{r k }=μ,E{[r k -μ][r k -μ] T }=A k

[0092] Define the fault detection function:

[0093] Where λ k Is χ with m degrees of freedom 2 Distribution, namely λ k ~χ 2 (m). m is the measurement Z k The number of dimensions.

[0094] The failure judgment criteria are:

[0095]

[0096] Where T D Is a preset threshold, which can be determined by the false alarm rate P f OK (when limit false alarm rate P f =α, by P f =P[λ kT D /H 0 ]=α to find T D )

[0097] Step 4: Construct a fault-tolerant Kalman filter for state estimation. Through step 3, the SINS/Doppler estimation of the conventional Kalman filter is detected. If there is a failure, it will return to the fault point and enter the fault-tolerant Kalman filter for state estimation. Next, construct a fault-tolerant Kalman filter, determine the real number γ according to the cost function of the system (γ is a parameter reflecting the robustness of filtering), and bring it into the filter recursive formula to obtain the determined fault-tolerant Kalman filter, and then perform the state estimate.

[0098] For discrete system models:

[0099] Where T k Full rank matrix, Y k Is the state variable X k A linear combination of.

[0100] Let T k (F f ) Means the input sequence The transfer function mapped to the filtered error sequence {e(k)}. Define T k (F f ) H ∞ The norm is:

[0101]

[0102] Where e(k) is the filtering error, namely

[0103] Given the real number γ>0, such that ||T k (F f )|| ∞

[0104]

[0105]

[0106]

[0107]

[0108] among them: In the formula, γ takes the value according to the actual situation.

[0109] Step 5. After the state estimation of each subsystem confirms that there is no fault, the local estimated value of the sub-filter state and its covariance matrix are sent to the main filter for information fusion to obtain a global state estimate. Such as figure 2 As shown, there is a general structure for resetting the Federation Kalman. The federated Kalman filter has six structures. Different structures and the determination of the information distribution coefficient will have different effects on the characteristics of the federated filter (fault tolerance, optimality, calculation amount, etc.). The present invention uses a m = 0, β i =1/N structure such as image 3 As shown, this structure does not require the global estimate of the main filter And its covariance P g Feedback to the sub-filters, since the sub-filters will not affect each other, the fault tolerance of the system can be improved,

[0110]

[0111]

[0112] among them: Are the state estimation of sub-filter 1 and sub-filter 2 respectively;

[0113] P 1 , P 2 Are the variance estimation of sub-filter 1 and sub-filter 2 respectively;

[0114] Is the global state estimation, P g Estimate the covariance matrix for the global state.

[0115] Step 6. Use the estimated value of the federated Kalman global error to correct the navigation parameters output by the inertial navigation system: the present invention uses indirect method filtering, the error is used as a state variable, and the estimated value of the error is obtained through the federated Kalman filter system , And then feed back to the inertial navigation system, and subtract the estimated value of the error from the navigation parameter output by the inertial navigation system to get the estimated value of the final navigation parameter.

Specific embodiment two:

[0116] The purpose of the present invention is to provide an integrated navigation method based on fault-tolerant Kalman filtering. Aiming at the SINS/GPS/Doppler multi-integrated navigation system, the technical scheme adopted by the present invention includes the following steps:

[0117] Step 1, choose As a state variable,

[0118] among them: δλ: Inertial navigation system latitude and longitude error;

[0119] δV x ,δV y : The speed error of the inertial navigation system in the east and north directions of the geographic coordinate system;

[0120] ε x ε y ε z : Random error of first-order Markov process of gyro;

[0121] αβλ: platform error angle of east, north and sky direction of strapdown inertial navigation in geographic coordinate system;

[0122] Then, according to the system characteristics, list the state equation of the system:

[0123] The SINS/GPS subsystem selects position and speed as quantity measurement,

[0124] which is

[0125] Where: λ I , V IX , V IY The latitude, longitude, eastward speed, and northward speed calculated by inertial navigation respectively;

[0126] λ G , V GX , V GY The GPS measured latitude, longitude, east speed, and north speed.

[0127] SINS/Doppler subsystem chooses speed as the quantity measurement,

[0128] which is

[0129] Where: V IX , V IY Respectively the eastward speed and the northward speed calculated by the inertial navigation;

[0130] V dX , V dY , Are the eastward velocity and northward velocity measured by Doppler respectively.

[0131] Discretize separately to get:

[0132]

[0133] Where: Φ i k/k-1 , H ik , Γ i,k-1 , W i,k-1 , V i,k They are the state transition matrix, measurement matrix, system noise driving matrix, system noise matrix, and measurement noise matrix of the i-th subfilter.

[0134] Step 2. Construct a conventional Kalman filter for state estimation. The conventional Kalman filtering process is divided into two processes: time update and measurement update: use time update to obtain a one-step prediction of the state And variance one-step prediction P k/k-1; Use the measurement update to find the estimated value of the state And the estimated value of variance P k. The specific process is as follows:

[0135] One-step state prediction equation:

[0136] State estimation calculation equation:

[0137] Filter gain equation:

[0138] One-step prediction mean square error equation:

[0139] Estimate the mean square error equation:

[0140] Step 3: Build and construct a fault detection function for detection:

[0141] Define fault detection function

[0142] Where: r k Is the residual, namely

[0143] E{r k }=μ,E{[r k -μ][r k -μ] T }=A k.

[0144] λ k Is χ with m degrees of freedom 2 Distribution, namely λ k ~χ 2 (m). m is the measurement Z k The number of dimensions.

[0145] The failure judgment criteria are:

[0146]

[0147] Where T D Is a preset threshold, at a given false alarm rate P f , Can be determined by χ n Distribution to determine.

[0148] Step 4. Construct a fault-tolerant Kalman filter for state estimation: Determine the real number γ according to the cost function of the system (γ is a parameter that reflects the robustness of the filter, and can take a value of 2.5), and put it into the filtering recursive formula to get Determine the fault-tolerant Kalman filter to estimate the state.

[0149] for

[0150] Where T k Full rank matrix, Y k Is the state variable X k A linear combination of.

[0151] Perform the following recursive estimation:

[0152]

[0153]

[0154]

[0155]

[0156] among them:

[0157] Step 5. Send the data of the sub-filter to the main filter for information fusion to obtain a global state estimate:

[0158]

[0159]

[0160] among them: Are the state estimation of sub-filter 1 and sub-filter 2 respectively;

[0161] P 1 , P 2 Are the variance estimation of sub-filter 1 and sub-filter 2 respectively;

[0162] Is the global state estimation, P g Estimate the covariance matrix for the global state.

[0163] Step 6. The estimated value of the global error of the federated Kalman is used to correct the navigation parameters output by the inertial navigation system: the present invention adopts the indirect method of filtering, the error is used as a state variable, and the estimated value of the error is obtained through the federated Kalman system. Then it is fed back to the inertial navigation system, and the estimated value of the final navigation parameter can be obtained by subtracting the estimated value of the error from the navigation parameter output by the inertial navigation system.

Specific embodiment three:

[0164] The invention discloses an integrated navigation method based on fault-tolerant Kalman filtering. This method is aimed at an inertial/satellite/Doppler integrated navigation system, which consists of two sub-filters and a main filter. The invention carries out a fault-tolerant design for the inertial/satellite navigation subsystem, and adds a fault-tolerant Kalman filter to improve its anti-interference ability. When the system state model and the prior information are known, the conventional Kalman filter is first used for state estimation, and then a fault detection is performed. When there is no fault, it is directly sent to the main filter for data fusion; if a fault occurs, The fault-tolerant Kalman filter is used to replace the sub-filter, and return to the fault point to perform state estimation again. The output data undergoes a fault detection again, and when there is no fault, it is sent to the main filter for data fusion. When there is a fault, the subsystem is directly isolated, thereby reducing the risk of system misjudgment, improving the utilization of navigation subsystem data, and improving the robustness of the integrated navigation system.

[0165] The integrated navigation method based on fault-tolerant Kalman filter is characterized by:

[0166] Step 1. Select the integrated navigation state quantity column to write the state equation, select the quantity measurement column to write the measurement equation, and perform discretization processing;

[0167] Step 2. Construct a conventional Kalman filter for state estimation;

[0168] Step 3. Construct a fault detection function for fault detection;

[0169] Step 4. Construct a fault-tolerant Kalman filter for state estimation;

[0170] Step 5. Send the data of each sub-filter to the main filter for information fusion to obtain a global state estimate;

[0171] Step 6. The estimated value of the global error of the federated Kalman is used to correct the navigation parameters output by the inertial navigation system.

[0172] Select in step 1 As a state variable of the reference system,

[0173] among them: δλ: Inertial navigation system latitude and longitude error;

[0174] δV x ,δV y : The speed error of the inertial navigation system in the east and north directions of the geographic coordinate system;

[0175] ε x ε y ε z : Random error of first-order Markov process of gyro;

[0176] αβλ: platform error angle of east, north and sky direction of strapdown inertial navigation in geographic coordinate system;

[0177] Then, according to the system characteristics, list the state equation of the navigation system.

[0178] The SINS/GPS sub-filter selects position and velocity as quantity measurement,

[0179] which is

[0180] Where: λ I , V IX , V IY The latitude, longitude, eastward speed, and northward speed calculated by inertial navigation respectively;

[0181] λ G , V GX , V GY The GPS measured latitude, longitude, east speed, and north speed.

[0182] The SINS/Doppler sub-filter selects speed as the quantity measurement,

[0183] which is

[0184] Where: V IX , V IY Respectively the eastward speed and the northward speed calculated by the inertial navigation;

[0185] V dX , V dY , Are the eastward velocity and northward velocity measured by Doppler respectively.

[0186] Then discretize the state equation and measurement equation of the system to obtain the state space model of the system.

[0187] In step 2, a conventional Kalman filter is constructed for state estimation: Kalman filter is divided into two processes, time update and measurement update. Use time update to find one-step prediction of state And variance one-step prediction P k/k-1; Use the measurement update to find the estimated value of the state And the estimated value of variance P k.

[0188] In step 3, construct a fault detection function for detection:

[0189] Define the fault detection function: Where: r k Is the residual, A k Is the residual r k Variance.

[0190] λ k Is χ with m degrees of freedom 2 Distribution, namely λ k ~χ 2 (m). m is the measurement Z k The number of dimensions.

[0191] The failure judgment criteria are:

[0192]

[0193] Where T D Is a preset threshold, at a given false alarm rate P f , Can be determined by χ n Distribution to determine.

[0194] In step 4, a fault-tolerant Kalman filter is constructed for state estimation: the real number γ is determined according to the cost function of the system (γ is a parameter reflecting the robustness of the filtering, which can be set to 2.5), and it is taken into the filtering recursive formula to obtain Determine the fault-tolerant Kalman filter to estimate the state.

[0195] In step 5, the data of the sub-filter is sent to the main filter for information fusion to obtain the global state estimation:

[0196]

[0197]

[0198] among them: Are the state estimation of sub-filter 1 and sub-filter 2 respectively;

[0199] P 1 , P 2 Are the variance estimation of sub-filter 1 and sub-filter 2 respectively;

[0200] Is the global state estimation, P g Estimate the covariance matrix for the global state.

[0201] In step 6, the estimated value of the global error of the federated Kalman is used to correct the navigation parameters output by the inertial navigation system: the present invention adopts the indirect method of filtering, the error is used as a state variable, and the estimated value of the error is obtained through the federated Kalman system. Then it is fed back to the inertial navigation system, and the estimated value of the final navigation parameter can be obtained by subtracting the estimated value of the error from the navigation parameter output by the inertial navigation system.