Integrated navigation method based on fault-tolerant Kalman filtering

A Kalman filter and Kalman filter technology, applied in the field of integrated navigation, can solve problems such as fault tolerance performance to be improved, weak anti-interference ability, etc., and achieve the effect of improving the utilization degree and improving the robust performance.

Active Publication Date: 2019-02-22
HARBIN ENG UNIV
3 Cites 20 Cited by

AI-Extracted Technical Summary

Problems solved by technology

[0005] There are many literatures about fault tolerance and federated Kalman, among which Zhang Hailong et al. "Optimized Design of Federated Kalman Filter in Strapdown Inertial Navigation/Global Positioning/Odometer Integrated Navigation System" (Journal of Detection and Control, 2009), Wang Peng et al. "Research on the Filtering Algorithm for Ship Integrated Navigation of Inertial Navigation System/Global Positioning System/Doppler Log" (Science Techn...
View more

Method used

[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 the global state estimation. As shown in Figure 2, there is a general structure of reset federated Kalman. The federated Kalman filter consists of six structures, and 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 adopts a structure without reset and βm=0, βi=1/N as shown in Figure 3, this structure does not need to feed back the global estimated value of the main filter and its covariance Pg to the sub-filter, Since the sub-filters will not affect each other, the fault tolerance of the system can be improved,
[0164] The present invention discloses an integrated navigation method based on fault-tolerant Kalman filtering. This method ...
View more

Abstract

The invention discloses an integrated navigation method based on fault-tolerant Kalman filtering and belongs to the technical field of integrated navigation. The method comprises the following steps:selecting an integrated navigation state quantity to write a state equation, selecting a measuring quantity to write a measuring equation and carrying out discretization; constructing a conventional Kalman filterer for state estimation; constructing a fault detection function for detecting faults; constructing a fault-tolerant Kalman filer to estimate the state; feeding data of sub filters to a primary filter for information fusion to obtain global state estimation; and correcting a navigation parameter output by an inertial navigation system by means of an estimated value of global error of federated kalman. By adding a fault detection link and a fault-tolerant Kalman filter for an SINS/GPS sub system isolated as a result of faults, once the faults happen, fault points are returned to carry out fault-tolerant Kalman filtering again. Therefore, the utilization degree of sub system data is improved, so that the robust performance of the whole system is improved.

Application Domain

Navigation by speed/acceleration measurementsSatellite radio beaconing

Technology Topic

Inertial navigation systemKalman filter +8

Image

  • Integrated navigation method based on fault-tolerant Kalman filtering
  • Integrated navigation method based on fault-tolerant Kalman filtering
  • Integrated navigation method based on fault-tolerant Kalman filtering

Examples

  • Experimental program(3)

Example Embodiment

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.

Example Embodiment

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.

Example Embodiment

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.

PUM

no PUM

Description & Claims & Application Information

We can also present the details of the Description, Claims and Application information to help users get a comprehensive understanding of the technical details of the patent, such as background art, summary of invention, brief description of drawings, description of embodiments, and other original content. On the other hand, users can also determine the specific scope of protection of the technology through the list of claims; as well as understand the changes in the life cycle of the technology with the presentation of the patent timeline. Login to view more.

Similar technology patents

Solar cell panel with self-cleaning mechanism

ActiveCN112769390AImprove utilizationRealize utilization
Owner:宁波光年太阳能科技开发有限公司

Fish feed

Owner:蚌埠市水森林生态农业科技园

Manufacturing method of PCB

ActiveCN105530770Aimprove utilizationImprove signal transmission quality and speed
Owner:DONGGUAN SHENGYI ELECTRONICS

Classification and recommendation of technical efficacy words

  • Improve utilization
  • Improve robust performance

Mycelium stimulation production line

ActiveCN107439228AReduce working hoursImprove utilization
Owner:永州市祥瑞生物科技有限公司

Filtering backstepping ship movement control system based on self-adaption fuzzy estimator

ActiveCN103592846AImprove robust performancesimple form
Owner:哈尔滨船海智能装备科技有限公司

Visual positioning method of mobile manipulator

InactiveCN105196287AImprove robust performanceEnsure material transportation
Owner:INST OF AUTOMATION CHINESE ACAD OF SCI
Who we serve
  • R&D Engineer
  • R&D Manager
  • IP Professional
Why Eureka
  • Industry Leading Data Capabilities
  • Powerful AI technology
  • Patent DNA Extraction
Social media
Try Eureka
PatSnap group products