Combination navigation method based on improved adaptive filtering under colored noise

Abstract

The application discloses a kind of colored noise under the combination navigation method based on improved adaptive filtering.The application first whitens noise to satisfy the basic condition of Kalman filtering, realizes fault detection by constructing new statistics, adaptively changes new measurement noise variance by introducing amplification coefficient, reduces the influence of fault unit on structure, realizes fault isolation.The application can effectively detect and isolate the fault of measurement unit in combination navigation process, and when there is an exception in a certain dimension in a group of measurement, it does not affect the fusion of remaining data, improves the utilization of data, solves the problem of measurement abnormality affecting navigation accuracy, and improves the reliability of combination navigation system.

Description

Technical Field

[0001] This invention relates to the field of multi-sensor integrated navigation technology, and more specifically to an integrated navigation method based on improved adaptive filtering under colored noise. Background Technology

[0002] An inertial navigation system (INS) is an autonomous navigation system capable of operating in various complex environments, including air, land, and underwater. Gyroscopes and accelerometers are the main components of an INS, their measurements representing the angular velocity and specific force of the vehicle relative to inertial space, respectively. Through mechanical programming, the vehicle's attitude, velocity, and position can be calculated. Furthermore, INS does not rely on any external information or radiate any information outwards; it can perform positioning in any environment solely based on the inertial system itself. Therefore, INS possesses advantages such as autonomy and stealth, playing a vital role in national defense, industry, and transportation. The positioning accuracy of an INS during the navigation phase is determined by the accuracy of the inertial devices, the accuracy of the navigation algorithm, and error compensation techniques. With improvements in the accuracy of inertial devices and continuous optimization of navigation algorithms, the navigation and positioning accuracy of INS continues to improve. However, due to the working principle of INS, cumulative errors are unavoidable. While these have little impact in the short term, they gradually increase over extended periods of positioning and navigation, leading to a decrease in navigation and positioning accuracy with increasing operating time. With the development of technology, inertial navigation alone cannot meet the needs of various applications. To solve this problem, integrated navigation technology can be used, which utilizes other sensors as motion information reference sources to correct the inertial navigation system and improve positioning accuracy.

[0003] Integrated navigation offers higher reliability compared to single inertial navigation, and is therefore widely used in various fields. To further improve system robustness, multiple reference sources can be used for integrated navigation. However, the use of auxiliary equipment also brings new problems to the navigation system. When the vehicle is in a complex environment, sensor measurements are subject to external interference. Abnormal measurements can increase the error or even cause divergence in the output of the integrated navigation system. Therefore, fault tolerance in integrated navigation is a key research issue in this field. To improve the system's fault tolerance, faults need to be detected and isolated during integrated navigation. Most fault detection methods use the chi-square test, but in practical applications, measurement noise is mostly colored noise, which alters the statistical characteristics of the noise. The original statistics no longer satisfy the chi-square distribution, requiring the construction of new statistics. Some other existing methods use federated filtering for fault isolation, but in federated filtering, each local filter uses the same reference source, leading to non-independence between the outputs of each filter and making the state estimation suboptimal. Furthermore, federated Kalman filtering is computationally intensive and prone to divergence. In addition, there are methods to isolate faults using sequential adaptive filtering. However, sequential processing of Kalman filtering is only applicable when the measurement noise variance matrix is ​​a diagonal matrix or a constant value. It is no longer applicable when the measurement noise is colored noise. Summary of the Invention

[0004] In view of this, the present invention provides a combined navigation method based on improved adaptive filtering under colored noise, which realizes fault detection by constructing new statistics and realizes fault isolation by adaptively changing the variance of the new measurement noise, thereby improving the reliability of the combined navigation system.

[0005] The integrated navigation method based on improved adaptive filtering under colored noise of the present invention employs a combination of an inertial navigation system and auxiliary measurement equipment as the measurement unit; the integrated navigation method includes:

[0006] Step 1: Establish the state equation and measurement equation of the integrated navigation system; wherein, the state vector of the integrated navigation system is the measurement error of the inertial navigation system and the auxiliary measurement equipment;

[0007] The colored measurement noise of the integrated navigation system is whitened, and an improved measurement equation is obtained based on the observation augmentation method.

[0008] Step 2: The measurement unit performs real-time measurements and Kalman filtering based on the state equation and the improved measurement equation; navigation is then performed based on the Kalman filtering results.

[0009] Step 3: Calculate the Mahalanobis distance of the measurement innovation of the improved measurement equation; perform a chi-square test on the squared value of the Mahalanobis distance. If the squared value of the Mahalanobis distance is greater than the set threshold, it is determined that the integrated navigation system has malfunctioned, and proceed to step 4; if the squared value of the Mahalanobis distance is less than or equal to the threshold, it is determined that the integrated navigation system is normal, and return to step 2 to continue to execute Kalman filtering.

[0010] Step 4: Correct the measurement noise variance of the improved measurement equation using the amplification factor; wherein, each element of the measurement vector corresponds to an amplification factor, and the diagonal elements of the corresponding measurement noise variance matrix are corrected respectively; the amplification factor is a number greater than 1; return to step 2 and perform Kalman filtering using the corrected measurement noise variance.

[0011] Preferably, assuming the colorimetric measurement noise is a first-order AR model, the measurement noise satisfies the following equation:

[0012]

[0013] In the formula, V represents the measurement noise of the integrated navigation system; Ψ k,k-1 This is the correlation coefficient matrix for colored noise; It is a zero-mean white noise sequence with variance R and W k Unrelated, W is the process noise of the integrated navigation system, which satisfies Gaussian white noise.

[0014] A better, improved measurement equation obtained using the observation augmentation method is:

[0015]

[0016] In the formula:

[0017]

[0018]

[0019]

[0020] Where X is the state vector of the integrated navigation system; Z is the measurement value; Ψ k,k-1 H is the correlation coefficient matrix for colored noise; H is the measurement matrix of the integrated navigation system; Φ k,k-1 Γ represents the one-step state transition matrix of the integrated navigation system from time k-1 to time k; k,k-1 W represents the noise input matrix from time k-1 to time k after discretization. k The noise in the integrated navigation system is a zero-mean white noise sequence. It is a zero-mean white noise sequence with variance R and W k Irrelevant;

[0021] V k * It is zero-mean white noise, and its variance matrix is:

[0022]

[0023] Where Q is the process noise variance matrix; H represents k The transpose of is ; R is the measurement noise variance matrix.

[0024] because Contains W k Therefore, the two are related, and their cross-covariance matrix is:

[0025]

[0026] Preferably, in step 3, the measurement of new information is as follows: It follows a normal distribution:

[0027] A better method for measuring innovation is the Mahalanobis distance:

[0028]

[0029] In the formula, Let Σ represent the i-th element of the information at time k. k (i,i) represents the i-th diagonal element of matrix Σ at time k.

[0030] Preferably, in step 4, the magnification factor κ i for:

[0031]

[0032] in, T represents the i-th element of the information at time k; α For the set threshold, It is the reciprocal of the set threshold. Indicates taking the matrix The i-th row; Representation matrix The i-th diagonal element.

[0033] Preferably, the auxiliary measuring device is one or more combinations of an odometer, a speedometer, or a GPS.

[0034] Beneficial effects:

[0035] Traditional fault detection and isolation methods are no longer applicable when the measurement noise is colored noise. This invention provides a combined navigation method based on improved adaptive filtering. First, the noise is whitened to meet the basic conditions of Kalman filtering. Fault detection is achieved by constructing new statistics. Then, by introducing an amplification factor, the variance of the new measurement noise is adaptively changed, reducing the impact of faulty units on the structure and achieving fault isolation. This invention can effectively detect and isolate faults in measurement units during combined navigation. Furthermore, when an anomaly occurs in one dimension of a set of measurements, it does not affect the fusion of other data, improving data utilization, solving the problem of measurement anomalies affecting navigation accuracy, and enhancing the reliability of the combined navigation system. Attached Figure Description

[0036] Figure 1 To improve the adaptive filtering calculation flowchart.

[0037] Figure 2 The results of combining different methods for navigation. Detailed Implementation

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

[0039] This invention provides a combined navigation method based on improved adaptive filtering under colored noise. Ordinary Kalman filtering is no longer applicable under colored noise; therefore, the measurement noise is first whitened using observation augmentation, thus establishing an improved Kalman filter under colored measurement noise conditions. Then, a new statistic satisfying a chi-square distribution is constructed, and an appropriate threshold is set for fault detection. Finally, the variance of the new measurement noise is adaptively adjusted based on the detection results. If a fault occurs, an amplification factor is calculated to amplify the variance of the new measurement noise, updating the state variables of the improved Kalman filter, thus completing the adaptive filtering for measurement fault tolerance and isolation.

[0040] The flowchart of this invention is as follows Figure 1 As shown, it specifically includes:

[0041] Based on the error models of each measurement unit in the integrated navigation system, a state-space model of the integrated navigation system is established, wherein the state equation of the integrated navigation system is:

[0042]

[0043] In the formula, F is the state matrix of the integrated navigation system; G is the noise input matrix; W is the process noise of the integrated navigation system, which satisfies Gaussian white noise; and X is the state vector of the integrated navigation system.

[0044] The measurement equations for the integrated navigation system are:

[0045] Z = HX + V

[0046] In the formula, H is the measurement matrix of the integrated navigation system, and V is the measurement noise of the integrated navigation system.

[0047] Discretizing the state equation and the measurement equation yields:

[0048] X k =Φ k,k-1 X k-1 +Γ k,k-1 W k-1

[0049] Z k =H k X k +V k

[0050] In the formula, Φ k,k-1 Γ represents the one-step state transition matrix of the integrated navigation system from time k-1 to time k. k,k-1 This represents the noise input matrix from time k-1 to time k after discretization.

[0051] If the process noise W of the integrated navigation system k The sequence is a zero-mean white noise sequence, and the measurement noise V is... k Assuming the colored noise is modeled as a first-order AR model, the measurement noise satisfies the following equation:

[0052]

[0053] In the formula, Ψ k,k-1 The correlation coefficient matrix is ​​for colored noise. It is a zero-mean white noise sequence with variance R and W k Irrelevant.

[0054] A new measurement equation can be obtained by using the observation augmentation method:

[0055]

[0056] In the formula:

[0057]

[0058]

[0059]

[0060] in, It is zero-mean white noise, and its variance matrix is:

[0061]

[0062] Where Q is the process noise variance matrix; H representsk The transpose of is ; R is the measurement noise variance matrix.

[0063] because Contains W k Therefore, the two are related, and their cross-covariance matrix is:

[0064]

[0065] Next, we can process the data according to the Kalman filter equation under the white noise correlation condition. The recursive equation is as follows:

[0066]

[0067]

[0068]

[0069]

[0070]

[0071] P k =(IK k H k )P k,k-1

[0072] New measurement information can be constructed based on the new measurement equation. It follows a normal distribution:

[0073]

[0074] Mahalanobis distance is an efficient method for calculating the similarity between two unknown sample sets. The Mahalanobis distance for measuring innovation is defined as:

[0075]

[0076] In the formula, Let Σ represent the i-th element of the information at time k. k (i,i) represents the i-th diagonal element of matrix Σ at time k. Due to the new... It is zero-mean Gaussian white noise, and we can obtain a statistic that follows a chi-square distribution with 1 degree of freedom:

[0077]

[0078] If an anomaly occurs in the measurement, the squared Mahalanobis distance of the measurement innovation will no longer follow a chi-square distribution. Therefore, fault detection can be performed based on the hypothesis testing principles in mathematical statistics. Set the significance level α for the chi-square test and its corresponding threshold T. αThe confidence probability of the Mahalanobis distance at significance level α can be obtained as follows:

[0079] P([M k (i)] 2 >T α )=α

[0080] Under normal circumstances [M k (i)] 2 >T α It is a low-probability event, its probability is less than α, so when [M k (i)] 2 >T α The system may be considered faulty at this time, and the judgment criteria are as follows:

[0081] If [M] k (i)] 2 >T α The system determines that the corresponding information is abnormal and that there is a malfunction.

[0082] If [M] k (i)] 2 ≤T α The system determined that the corresponding information was normal and that there was no system malfunction.

[0083] When an anomaly in the innovation is detected, fault isolation is required. Measurement noise variance represents the reliability of the measurement value. As the variance increases, the reliability of the measurement value decreases. This leads to a decrease in the Kalman gain, reducing the weight of the innovation in measurement updates, and making state estimation rely more on information provided by state prediction. When an anomaly in the innovation is detected, an amplification factor κ is introduced. i Amplify the variance:

[0084]

[0085] In the formula This represents the corrected variance, and the statistic is calculated as follows:

[0086]

[0087] The magnification factor satisfies the following equation:

[0088]

[0089] It can be solved

[0090]

[0091] The state estimate is completed by measuring and updating the corrected variance.

[0092] Example

[0093] The following explanation will take a combined navigation system that uses an odometer and a velocimeter to assist inertial navigation as an example.

[0094] First, establish a model of the integrated navigation system:

[0095]

[0096] In the formula, F SINS Let G be the state matrix of the inertial navigation system. SINS Let X be the noise input matrix of the inertial navigation system, and let X be the state vector of the integrated navigation system, specifically in the form of:

[0097]

[0098] In the formula φ n δv represents the attitude angle error, δP represents the velocity error, and ε represents the position error. b and Representing the constant drift of the gyroscope and accelerometer, respectively, δθ and δk represents the installation error angle between the load system (b-system) and the vehicle system (m-system). OD and δk LDV δv0 represents the scaling factor error of the odometer and the speedometer, respectively, and δv0 represents the zero-position error of the speedometer.

[0099] The measurement equation is established using the speed output from the odometer and tachometer, as well as the non-integrity constraints, as the measurement information:

[0100] Z = HX + V

[0101] In the formula, For measurement, among which and These represent the speeds output by the odometer and the speedometer, respectively. H is the measurement matrix, and V is the measurement noise. The specific form of H is:

[0102]

[0103]

[0104] In the formula v n × represents vector v n antisymmetric matrix, Indicates taking the matrix The second line.

[0105] 2. Set the initial process noise variance matrix Q, measurement noise variance matrix R, and initial state value X0, and initialize the coefficient matrix J and cross-covariance matrix S. Let k be the current time, and let k = 1.

[0106] Third, during each Kalman filter operation, a time update is first performed. The posterior estimate at time k-1 is used to predict the system state and covariance matrix at time k using the system model. The update formula is as follows:

[0107]

[0108]

[0109] In the formula, Φ is the state transition matrix and Γ is the noise input matrix.

[0110] IV. Measurement of Calculation Quantities and And calculate the Mahalanobis distance M for each dimension of the measurement. k (i)(i=1,2,3,4), the calculation formula is as follows:

[0111]

[0112]

[0113]

[0114]

[0115]

[0116] In the formula, Ψ is the correlation coefficient matrix of colored noise, r(i) represents the i-th element of vector r, H(i,:) represents the i-th row of matrix H, and R(i,i) represents the i-th diagonal element of matrix R.

[0117] V. Set the significance level α and the corresponding threshold T for the chi-square test. α The criteria for determining whether the new information is abnormal are as follows:

[0118] If [M] k (i)] 2 >T α The system determines that the corresponding information is abnormal and that there is a malfunction.

[0119] If [M] k (i)] 2 ≤T α The system determined that the corresponding information was normal and that there was no system malfunction.

[0120] VI. Adaptive adjustment based on judgment results If the new information is normal, then the corresponding remain unchanged. If the new information is abnormal, then through the amplification factor κ i The corresponding enlarge, The formula for calculating the magnification factor is:

[0121]

[0122] VII. Obtaining new The Kalman gain coefficient at time k is then calculated using the following formula:

[0123]

[0124] 8. Update the coefficient matrix J k and the cross-covariance matrix S k The calculation formula is:

[0125]

[0126]

[0127] 9. Perform measurement updates, using the Kalman gain coefficients to update the state, and obtain the posterior estimates of the system state and covariance matrix at time k. The calculation formula is as follows:

[0128]

[0129]

[0130] The obtained system state variables can be used for feedback compensation in navigation calculation. If the navigation state has not ended, let k = k + 1, and start the next Kalman filter from step three.

[0131] Figure 2 The results of integrated navigation using the above steps under conditions of colored measurement noise and system faults are presented. As can be seen from the figures, when a system fault occurs, the integrated navigation method of this invention can effectively detect and isolate the fault, effectively reducing its impact on the results.

[0132] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A combined navigation method based on improved adaptive filtering under colored noise, wherein the measurement unit is a combination of an inertial navigation system and auxiliary measurement equipment; characterized in that, include: Step 1: Establish the state equation and measurement equation of the integrated navigation system; wherein, the state vector of the integrated navigation system is the measurement error of the inertial navigation system and the auxiliary measurement equipment; The colored measurement noise of the integrated navigation system is whitened, and an improved measurement equation is obtained based on the observation augmentation method. Step 2: The measurement unit performs real-time measurements and Kalman filtering based on the state equation and the improved measurement equation; navigation is then performed based on the Kalman filtering results. Step 3: Calculate the Mahalanobis distance of the measurement innovation of the improved measurement equation; perform a chi-square test on the squared value of the Mahalanobis distance. If the squared value of the Mahalanobis distance is greater than the set threshold, it is determined that the integrated navigation system has malfunctioned, and proceed to step 4; if the squared value of the Mahalanobis distance is less than or equal to the threshold, it is determined that the integrated navigation system is normal, and return to step 2 to continue to execute Kalman filtering. Step 4: Correct the measurement noise variance of the improved measurement equation using the amplification factor; wherein, each element of the measurement vector corresponds to an amplification factor, and the diagonal elements of the corresponding measurement noise variance matrix are corrected respectively; the amplification factor is a number greater than 1; return to step 2 and perform Kalman filtering using the corrected measurement noise variance.

2. The integrated navigation method as described in claim 1, characterized in that, If the colorimetric measurement noise is a first-order AR model, then the measurement noise satisfies the following equation: In the formula, This is the correlation coefficient matrix for colored noise.

3. The integrated navigation method as described in claim 2, characterized in that, The improved measurement equation obtained using the observation augmentation method is as follows: In the formula: in, This represents the state vector of the integrated navigation system. This is the correlation coefficient matrix for colored noise; Indicates integrated navigation system Time's up The state transition matrix at time step; After discretization Time's up The noise input matrix at time step; It is zero-mean white noise, and its variance matrix is: in, The process noise variance matrix; express The transpose of the matrix; The measurement noise variance matrix; because Contains Therefore, the two are related, and their cross-covariance matrix is: 。 4. The integrated navigation method as described in claim 3, characterized in that, In step 3, the measurement of new information is as follows: It satisfies the following normal distribution: ; in, The Kalman filter recursive equation is obtained from the following: 。 5. The integrated navigation method as described in claim 4, characterized in that, The Mahalanobis distance for measuring innovation is: In the formula, , express The first moment of the new information One element, express Time matrix The One diagonal element.

6. The integrated navigation method as described in claim 5, characterized in that, In step 4, the magnification factor for: in, express The first moment of the new information One element; For the set threshold, It is the reciprocal of the set threshold. Indicates taking the matrix The OK; Representation matrix The One diagonal element.

7. The integrated navigation method as described in claim 1, characterized in that, The auxiliary measurement device is one or more combinations of an odometer, a speedometer, or a GPS.

Images