A robust method for SINS / DME / DME integrated navigation based on chi-square test

Abstract

The invention discloses a robust method for SINS / DME / DME integrated navigation based on a chi-square test. The method comprises the following steps: establishing a strapdown inertial navigation system (SINS / DME / DME) integrated navigation system and deriving a measurement equation for DME / DME positioning; performing a criterion based on the Mahalanobis distance of residuals in the SINS / DME / DME integrated navigation process and selecting a suitable chi-square test threshold; constructing a robust factor through a three-segment method, weighting a measurement noise matrix, and obtaining an estimated value of the measurement noise; introducing a filter gain to achieve control and adjustment of measurement information. The method can effectively detect abnormal measurement information and achieve weighted processing of the abnormal measurement information.

Description

Technical Field

[0001] The invention relates to the technical field of integrated navigation systems, in particular to a chi-square test-based SINS / DME / DME integrated navigation robustness method. Background Art

[0002] In civil aviation, the onboard navigation system, centered around the strap-down inertial navigation system (SINS), provides the flight management system (FMS) with real-time position, velocity, and attitude information. While the SINS boasts strong autonomy, excellent anti-interference capabilities, and a high update rate, its errors accumulate over time, causing rapid divergence in its dead-reckoning results. This error often requires integration with other navigation methods to mitigate this error. Distance measuring equipment (DME) offers a limited navigation timeframe, a simple structure, and mature technology, but its navigation accuracy is relatively low. During actual flight, when the Global Positioning System (GPS) signal becomes unreliable, the FMS will degrade navigation, prioritizing a SINS / DME / DME combined navigation method to output FM position information to the cockpit. Some researchers have improved the positioning accuracy of the SINS / ground-based radio integrated navigation system by improving the DME positioning model and adding multiple navigation sources for federated filtering. Airborne DME equipment transmits a short electromagnetic pulse interrogation signal to a ground-based DME station at a known location. Upon receiving the interrogation signal, the ground station transmits a reply pulse signal. The time difference between the pulse pairs allows the aircraft to measure the slant range from the ground station. However, in the complex environment of the terminal area near an airport, radio propagation is susceptible to interference from obstacles and adjacent frequency signals, resulting in varying degrees of measurement error in the DME information. To address this measurement anomaly, many researchers have improved adaptive filtering algorithms to adjust the measurement noise matrix in real time, thereby reducing the impact of inaccurate noise statistics.

[0003] Traditional research, however, has focused solely on optimizing the adaptive navigation and positioning accuracy of integrated navigation systems, without considering the detection of measurement anomalies. Furthermore, in actual civil aircraft flight, there are no strict requirements for navigation and positioning accuracy; instead, the navigation system's actual navigation performance (ANP) must meet the required navigation performance (RNP) accuracy requirements. Therefore, the stability requirements for SINS / DME / DME navigation systems in civil aviation operations outweigh the positioning accuracy requirements. Summary of the Invention

[0004] In order to solve the problems existing in the prior art, the purpose of the present invention is to provide a robust method for SINS / DME / DME integrated navigation based on chi-square test, which can effectively detect abnormal measurement information and realize weighted processing of abnormal measurement information.

[0005] To achieve the above object, the technical solution adopted by the present invention is: a robust method for SINS / DME / DME integrated navigation based on chi-square test, comprising the following steps:

[0006] Step 1: Establish a strapdown inertial navigation system (SINS) / DME / DME integrated navigation system and derive the measurement equations for DME / DME positioning.

[0007] Step 2: Select an appropriate chi-square test threshold value based on the Mahalanobis distance of the residuals in the SINS / DME / DME combined navigation process;

[0008] Step 3: Construct a robustness factor using a three-stage method, weight the measurement noise matrix, and obtain an estimated value of the measurement noise;

[0009] Step 4: Input the filter gain to realize the control and adjustment of the measurement information.

[0010] As a further improvement of the present invention, in step 1, establishing a strapdown inertial navigation system SINS / DME / DME integrated navigation system is specifically as follows:

[0011] The system equations of SINS / DME / DME integrated navigation are described as follows:

[0012] X k =F k,k-1 X k-1 +Γ k W k (1)

[0013] In formula (1): X k is the error state at time k, F k / k-1 is the state transition matrix from time k-1 to time k, X k-1 is the error state at the previous moment, Γ k is the noise driving matrix, W k is the system noise matrix;

[0014] The measurement equation is expressed as:

[0015] Z k =H k X k +V k (2)

[0016] In formula (2): Z k is the measurement error between the two systems, H k is the measurement matrix, X k is the error at time k, V k is the measurement noise matrix;

[0017] The state equation consists of the SINS misalignment angle, velocity error, position error, gyroscope error, and accelerometer error. The error state of the SINS is selected as 15 dimensions, as follows:

[0018]

[0019] In formula (3): Φ, δv n , δp represent the misalignment angle, velocity error, and position error of SINS in the navigation coordinate system, respectively, and ε b is the projection of the gyroscope zero bias in the carrier coordinate system, is the projection of the accelerometer zero bias in the carrier coordinate system.

[0020] The state transfer matrix of the system represents the relationship between the system state at the previous moment and the system state at the current moment. The SINS position error differential equation is as follows:

[0021]

[0022] In the above formulas (4), (5), and (6): is the differential of the latitude and longitude error, δv E ,δv N ,δv U is the velocity error component in the northeast celestial coordinate system, δL, δλ, and δh are the latitude and longitude error components, and R M is the meridian radius, R N is the radius of the Maoyou circle;

[0023] make:

[0024]

[0025] Combining equations (4)-(8) we get:

[0026]

[0027] Similarly, the velocity error differential equation is:

[0028]

[0029] Differential equation for misalignment angle:

[0030]

[0031] In formulas (10) and (11): is the transfer matrix from the carrier coordinate system to the navigation coordinate system; M va 、M vv 、M vp With M aa 、M av 、M ap are the conversion matrices of velocity error differential and misalignment angle differential and the misalignment angle, velocity error and position error of the strapdown inertial navigation platform respectively;

[0032] Combining equations (9), (10), and (11), the state transfer matrix F of the INS / DME / DME integrated navigation system is:

[0033]

[0034] As a further improvement of the present invention, in step 1, the derivation method of the measurement equation is as follows:

[0035] During the flight, the aircraft uses the onboard DME receiver to measure the slant range of the aircraft relative to the two DME stations. At the same time, the DME ground station obtains the measured slant range by using a question-and-answer method with the onboard DME antenna. The slant range error is expressed as:

[0036] δρ=ρ C -ρ D (13)

[0037] In formula (13), δρ is the slant range error, ρ C is the calculated slant distance of the aircraft, ρ D Slant range measured for the DME station;

[0038] The slant range ρ measured by the DME station D It is expressed as the relationship between the true slant range and the DME-related noise:

[0039] ρ D =ρ+δν D (14)

[0040] In formula (14), ρ is the true slant range, δν D is the measurement noise of DME, and δν D The mean of is zero;

[0041] Aircraft calculated slant range ρ C The aircraft position in Cartesian coordinates (x P y P z P ) and DME station position (x D y D z D ) is represented as follows:

[0042]

[0043] Differentiate equation (15) to obtain the error δρ of the slope distance C , the results are as follows:

[0044]

[0045] During the flight, the position of the aircraft in the Earth-centered Earth-fixed coordinate system is (L λ h), and the position of the DME station is (L D λ D h D ), then the ECEF coordinate system is converted to the Cartesian coordinate system, and the conversion relationship is as follows:

[0046] x=(R N +h)·cos L cosλ (17)

[0047] y=(R N +h)·cos L sinλ (18)

[0048] z=(R M +h)·sin L (19)

[0049] Differentiating equations (17), (18), and (19) respectively yields the differential relationship between the ECEF coordinate system and the Cartesian coordinate system:

[0050] δx=-(R N +h)·sinLcosλδL

[0051] -(R N +h)cosLsinλδλ

[0052] +cosLcosλδh(20)

[0053] δy=-(R N +h)·sinLcosλδL

[0054] +(R N +h)cosLcosλδλ

[0055] +cosLsinλδh(21)

[0056] δz=(R M +h)·cosLδL+sinLδh (22)

[0057] make:

[0058]

[0059] M=[M1 M2 M3] (27)

[0060] The slant range error calculated by the aircraft is shown in formula (28):

[0061]

[0062] Combining equations (2), (13), (14), and (28), the error differential equation of the slant range is obtained as shown in equation (29):

[0063]

[0064] The altitude information is measured using a barometric altimeter to obtain the measurement matrix of the SINS / DME / DME integrated navigation system:

[0065]

[0066] As a further improvement of the present invention, the step 2 is specifically as follows:

[0067] For the SINS / DME / DME integrated navigation system, the SINS inference calculation and DME measurement data are fused using the Kalman filter algorithm, which includes two parts: time update and measurement update:

[0068] Time updates include state updates and covariance updates:

[0069] X k / k-1 =F k X k-1 / k-1 (31)

[0070]

[0071] Measurement updates include filter gain, state estimation, and variance estimation:

[0072]

[0073] P k =(IK k H k )P k / k-1 (35)

[0074] Before performing robust adaptive processing, detect abnormal information in advance and then perform filtering processing; let the residual be ε k , which is expressed as follows:

[0075]

[0076] In formula (36), Z k is the measured value; when the measured value fails, ε kDoes not obey Gaussian distribution; when the measured value is fault-free, ε k Conforms to a Gaussian distribution with a mean of zero;

[0077] According to the orthogonality principle, the square of the residual Mahalanobis distance obeys the chi-square distribution, and its chi-square test statistic is as follows:

[0078]

[0079] By setting the chi-square test significance level α, the residual ε is judged k the credibility of

[0080] Let the residual ν k for:

[0081]

[0082] Measurement discriminant function λ k for:

[0083]

[0084] In formula (38), It represents the optimal estimate of the state at the previous moment and is used to eliminate the influence of the covariance error and system noise on the residual at the current moment.

[0085] As a further improvement of the present invention, in step 3, the robustness factor is constructed by a three-stage method as follows:

[0086] In order to solve the problem of abnormal measurement information during filtering, the noise variance R is amplified by the robustness factor. k , adjust the gain matrix K k The value of , reduces the weight of observation information, weakens the optimal estimate of the state at the kth moment The influence of ; thus, formula (33) is rewritten as follows:

[0087]

[0088] In formula (40), T k is the robustness factor;

[0089] The three-stage method is used, corresponding to the weight function's retention zone, weight reduction zone, and elimination zone; its function expression is:

[0090]

[0091] In formula (41): is the standardized new information of the i-th component; k0 and k1 are adjustment coefficients and are constant values, where is calculated as follows:

[0092]

[0093] In formula (42), is the new information sequence of the i-th component up to time k, The distance of the normalized value of the distance of the i-th component is calculated as follows:

[0094]

[0095] In formula (43), med(·) is the median.

[0096] The beneficial effects of the present invention are:

[0097] The present invention detects measurement information anomalies in the system through a chi-square test, calculates the robustness factor using a three-stage method to perform weighted update on the abnormal measurement noise matrix, reduces the impact of abnormal measurement information on the navigation system, and improves the stability of the SINS / DME / DME combined navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0098] Figure 1 is a flow chart of an embodiment of the present invention;

[0099] Figure 2 This is a DOTRA-11D standard departure procedure diagram in an embodiment of the present invention;

[0100] Figure 3 Schematic diagram of the three-dimensional flight trajectory of the DOTRA-11D program in an embodiment of the present invention;

[0101] Figure 4 Schematic diagram of the chi-square test result of a no-fault condition in an embodiment of the present invention;

[0102] Figure 5 Schematic diagram of chi-square test results in a faulty situation according to an embodiment of the present invention;

[0103] Figure 6 This is a schematic diagram of the change in the northeast sky positioning error of the IWKF algorithm;

[0104] Figure 7 Schematic diagram of the change of northeast sky error in an embodiment of the present invention;

[0105] Figure 8 Schematic diagram of slant range measurement by a DME station in an embodiment of the present invention;

[0106] Figure 9 Schematic diagram of the chi-square test result after adding gross errors in an embodiment of the present invention. DETAILED DESCRIPTION

[0107] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0108] Example

[0109] like Figure 1 As shown, a chi-square test-based robust method for SINS / DME / DME integrated navigation includes:

[0110] 1. SINS / DME / DME integrated navigation model:

[0111] During civil aviation operations, when the GPS signal becomes unreliable, navigation downgrade is used to switch to the higher-priority SINS / DME / DME combined navigation mode to output FM position, which is one of the important navigation methods in civil aviation operations.

[0112] The system equations of SINS / DME / DME integrated navigation are described as follows:

[0113] X k =F k,k-1 X k-1 +Γ k W k (1)

[0114] In formula (1): X k is the error state at time k, F k / k-1 is the state transition matrix from time k-1 to time k, X k-1 is the error state at the previous moment, Γ k is the noise driving matrix, W k is the system noise matrix.

[0115] The measurement equation can be expressed as:

[0116] Z k =H k X k +V k (2)

[0117] In formula (2): Z k is the measurement error between the two systems, H k is the measurement matrix, X k is the error at time k, V k is the measurement noise matrix.

[0118] 1.1, Equation of state:

[0119] Integrated navigation systems often use the SINS navigation system as the primary source for dead reckoning, obtaining the aircraft's position, velocity, and attitude information. Other navigation sources are used as supplementary sources to suppress the SINS's accumulated state errors over time and improve overall integrated navigation performance. Therefore, the state equations of the integrated navigation system primarily consist of the SINS error model.

[0120] The state of the system is composed of the SINS misalignment angle, velocity error, position error, gyroscope error, and accelerometer error. The error state of the SINS is selected as 15 dimensions, as follows:

[0121]

[0122] In formula (3): Φ, δv n , δp represent the misalignment angle, velocity error, and position error of SINS in the navigation coordinate system (N system), respectively, and ε b is the projection of the gyroscope zero bias in the carrier coordinate system (b system), is the projection of the accelerometer zero bias in the carrier coordinate system (b system).

[0123] The state transfer matrix of the system represents the relationship between the system state at the previous moment and the system state at the current moment. The following takes the SINS position error differential equation as an example to derive the state transfer matrix. The SINS position error differential equation is as follows:

[0124]

[0125]

[0126] In the above formulas (4), (5), and (6): is the differential of the latitude and longitude error, δv E ,δv N ,δv U is the velocity error component in the Northeast Sky (ENU) coordinate system, δL, δλ, and δh are the latitude and longitude error components, and R M is the meridian radius, R N is the radius of the Maoyou circle.

[0127] You can order:

[0128]

[0129] Combining equations (4)-(8) we can get:

[0130]

[0131] Similarly, the velocity error differential equation is:

[0132]

[0133] Differential equation for misalignment angle:

[0134]

[0135] In formulas (10) and (11): M is the transfer matrix from the carrier coordinate system to the navigation coordinate system. va 、M vv 、M vp With M aa 、M av 、M ap They are the conversion matrices of velocity error differential and misalignment angle differential and the misalignment angle, velocity error and position error of the strapdown inertial navigation platform respectively.

[0136] Combining equations (9), (10), and (11), the state transfer matrix F of the INS / DME / DME integrated navigation system can be obtained as follows:

[0137]

[0138] 1.2. Measurement equation:

[0139] During flight, the aircraft uses its onboard DME receiver to measure the slant range relative to the two DME stations. Simultaneously, the DME ground station uses a question-and-answer method to obtain the measured slant range from the onboard DME antenna. The slant range error can be expressed as:

[0140] δρ=ρ C -ρ D (13)

[0141] In formula (13), δρ is the slant range error, ρ C is the calculated slant distance of the aircraft, ρ D The slant range measured by the DME station.

[0142] The slant range ρ measured by the DME station D It can be expressed as the relationship between the true slant range and the DME-related noise:

[0143] ρ D =ρ+δν D (14)

[0144] In formula (14), ρ is the true slant range, δν D is the measurement noise of DME, and δν D The mean of is zero.

[0145] Aircraft calculated slant range ρ C The aircraft position in Cartesian coordinates (x P y P z P ) and DME station position (x D y D z D ) is represented as follows:

[0146]

[0147] Differentiate equation (15) to obtain the error δρ of the slope distance C , the results are as follows:

[0148]

[0149] During the flight, the position of the aircraft in the earth-centered, earth-fixed (ECEF) coordinate system is (L λ h), and the position of the DME station is (L D λ D h D ), the ECEF coordinate system needs to be converted to the Cartesian coordinate system, and the conversion relationship is as follows:

[0150] x=(R N +h)·cos L cosλ (17)

[0151] y=(R N +h)·cos L sinλ (18)

[0152] z=(R M +h)·sin L (19)

[0153] Differentiating equations (17), (18), and (19) respectively yields the differential relationship between the ECEF coordinate system and the Cartesian coordinate system:

[0154] δx=-(R N +h)·sin L cosλδL-(R N +h)cos L sinλδλ+cos L cosλδh (20)

[0155] δy=-(R N +h)·sin L cosλδL+(R N +h)cos L cosλδλ+cos L sinλδh(21)

[0156] δz=(R M +h)·cosLδL+sinLδh (22)

[0157] You can order:

[0158]

[0159]

[0160] M=[M1 M2 M3] (27)

[0161] The slant range error calculated by the aircraft is shown in formula (28):

[0162]

[0163] Combining equations (2), (13), (14), and (28), the error differential equation of the slant range is obtained as shown in equation (29):

[0164]

[0165] Since the DME station only measures slant range and does not involve altitude information, the barometric altimeter can be used to measure altitude information, and the measurement matrix of the SINS / DME / DME integrated navigation system can be obtained:

[0166]

[0167] 2. Robust algorithm based on chi-square test:

[0168] 2.1 Standard KF algorithm:

[0169] For the SINS / DME / DME integrated navigation system, the SINS inference calculation and DME measurement data are fused using the Kalman filter algorithm, which mainly includes two parts: time update and measurement update.

[0170] Time updates include state updates and covariance updates:

[0171] X k / k-1 =F k X k-1 / k-1 (31)

[0172]

[0173] Measurement updates include filter gain, state estimation, and variance estimation:

[0174]

[0175] P k =(IK k H k )P k / k-1 (35).

[0176] 2.2, Chi-square test:

[0177] Before performing robust adaptive processing, it is necessary to detect abnormal information in advance and then perform filtering processing.

[0178] Let the residual be ε k , which is expressed as follows:

[0179]

[0180] In formula (36), Z k is the measured value. When the measured value fails, ε k Does not obey Gaussian distribution. When the measured value is fault-free, ε k Fitting a Gaussian distribution with mean zero, these residuals are orthogonal everywhere.

[0181] According to the orthogonality principle, the square of the residual Mahalanobis distance obeys the chi-square distribution, and its chi-square test statistic is as follows:

[0182]

[0183] If the measured value is abnormal, the square of the Mahalanobis distance of the residual will not conform to the chi-square distribution with n degrees of freedom, where n is the measurement latitude. The residual ε can be judged by setting the chi-square test significance level α. k For example, when n=2 and the false positive rate is 10 -1 When the corresponding detection threshold value T D =4.61. The residual is judged to be abnormal, otherwise the opposite is true.

[0184] Formula (37) can determine whether the integrated navigation system as a whole has an abnormality during the filtering process, but it cannot distinguish whether the abnormality comes from system information or measurement information.

[0185] Let the residual ν k for:

[0186]

[0187] Measurement discriminant function λ k for:

[0188]

[0189] In formula (38), Represents the optimal estimate of the state at the previous moment. It can effectively eliminate the influence of the current moment covariance error and system noise on the residual.

[0190] 2.3. Calculation of robustness factor:

[0191] When measuring distance, the distance measuring instrument (DME) mainly adopts the inquiry and response mode. The airborne DME transmits an inquiry signal, which is captured by the ground ranging beacon station, delayed by 50us, and then transmitted through the transmitter as a response signal. In the complex environment above the city, the signal is susceptible to pulse interference during the propagation process, which will cause measurement anomalies to the DME measurement information, and in severe cases, it will cause DME station measurement failure. Therefore, to solve the problem of measurement information anomalies during the filtering process, the observation noise variance R can be amplified by the anti-error factor. k , adjust the gain matrix K kThe value of , reduces the weight of observation information, weakens the optimal estimate of the state at the kth moment The influence of . Equation (33) can be rewritten as follows:

[0192]

[0193] In formula (40), T k The robustness factor is constructed by the Huber method, the Danish method and the three-stage method.

[0194] This embodiment adopts a three-stage method, which corresponds to the weight function's retention zone, demotion zone, and elimination zone. This method fully utilizes valid information, restricts the use of suspicious information, and eliminates harmful information. It is a processing solution suitable for processing measurement data. Its function expression is:

[0195]

[0196] In formula (41): is the standardized new information of the i-th component; k0 and k1 are constant adjustment coefficients, k0 is usually 1.0-2.5; k1 is 3.5-8.0, where is calculated as follows:

[0197]

[0198] In formula (42), is the new information sequence of the i-th component up to time k, The distance of the normalized value of the distance of the i-th component is calculated as follows:

[0199]

[0200] In formula (43), med(·) is the median.

[0201] 3. Simulation analysis:

[0202] 3.1、Terminal program selection:

[0203] This example uses the DOTRA-11D terminal area standard departure procedure for Beijing Daxing Airport, as specified in the Aeronautical Information Publication (AIP), for simulation. This procedure passes through six waypoints within the operating range of three DME stations. Without considering DME station selection, two navigation stations, Daxing and Tianjin, were selected for measurement data simulation. The waypoint and DME station information is shown in Table 1. Figure 2This is the DOTRA-11D standard departure procedure chart. The blue marks are the horizontal positions of the waypoints from the initial waypoint AD711, and the red marks are the horizontal positions of the two DME stations from the initial waypoint AD711. Figure 3 It is the three-dimensional ideal trajectory diagram of the departure procedure.

[0204] Table 1 Position information of each waypoint in DOTRA-11D procedure

[0205]

[0206] 3.2, Simulation parameter settings:

[0207] This example does not consider the aircraft's taxiing and takeoff process from the airport runway in the DOTRA-11D program. It only simulates the aircraft's flight trajectory starting from waypoint AD711. Assume that the aircraft's initial position is 39°29'26"N, 116°38'29"E, an altitude of 600m, an initial velocity of 200kt, and an initial heading of 70°. The initial position error in the northeast and celestial directions is rk = [1m1m3m], the initial misalignment angle is αk = [0.5°0.5°0.5°], and the initial velocity error is vk = [0.1m / s0.1m / s0.1m / s]. Table 2 summarizes the specific settings of the navigation system and algorithm-related parameters.

[0208] Table 2 Simulation parameter settings

[0209]

[0210] The system measurement noise matrix R is:

[0211] R=diag([ν δD1 ν δD2 hk] T ) 2 (44)

[0212] The system noise matrix Q is:

[0213] Q=diag([web wdb 0 9×1 ] T ) 2 (45)

[0214] The mean square error matrix P of the initial system state is:

[0215] P=diag([ak vk rk eb db] T ) 2 (46)

[0216] 3.3, Simulation results analysis:

[0217] This embodiment derives the system state equation through the SINS error equation, derives the measurement equation through the DME slant range error model, selects a suitable terminal area departure procedure, and simulates the SINS / DME / DME integrated navigation using the standard Kalman filter KF, the information window Kalman filter (IWKF), and the algorithm of this embodiment.

[0218] Scenario 1: Set a 3000m measurement fault on the DAXING station and the TIANJIN station at the 100th, 900th, and 1400th seconds of simulation time. Figure 4 It can be seen that when the missed detection rate α = 0.1%, the chi-square threshold T D =16.27, the gross errors in the system during the integrated navigation process can be effectively detected, and the values ​​exceeding the threshold are weighted by the robustness factor to reduce the impact of gross errors on the overall performance of the integrated navigation and improve the navigation performance. Figure 5 The results of the chi-square test when a certain level of measurement failure is added to the measurement information are shown. It can be clearly seen that the data at the 100th, 900th, and 1400th seconds of the simulation provide sensitive detection feedback. Figure 6 It can be seen that the robust algorithm based on the innovation window can pull the positioning error back to the normal range during the overall navigation and positioning process, but there will be obvious tailing phenomenon in the east and north error changes at the 100th, 900th, and 1400th seconds, which does not effectively isolate the faulty measurement and has a bad impact on the navigation and positioning results. Figure 7 As shown, the algorithm proposed in this embodiment can effectively isolate faulty measurement situations and ensure the robustness of the navigation process.

[0219] Scenario 2: To evaluate the algorithm's effectiveness in handling measurement errors, random noise with a mean of 0 and a standard deviation of 0.5 nautical miles that conforms to a normal distribution was added to the original measurement data of the two DME stations during the simulation time period of 1500s-1600s. The results are as follows: Figure 8 shown. Figure 9The chi-square test results after adding gross errors are shown. It can be clearly seen that the chi-square test clearly detects gross errors during the 1500s-1600s period. Excessive measurement noise prevents the visualization of changes and distinctions in attitude, position, and other information. To further enhance the intuitive and clear presentation of the results, 50 Monte Carlo simulations were performed to calculate the root mean square error (RMSE) of the standard KF algorithm, the IWKF algorithm, and the algorithm of this embodiment. As shown in Table 3, compared to the standard KF algorithm, the IWKF algorithm improves navigation positioning by 0.55%, 11.82%, and 24.72% in latitude, longitude, and altitude, respectively. The algorithm of this embodiment improves navigation positioning by 4.36%, 19.96%, and 24.98% in latitude, longitude, and altitude, respectively, compared to the standard KF algorithm. Compared to the IWKF algorithm, the algorithm of this embodiment improves navigation positioning by 3.81%, 8.15%, and 0.25% in latitude, longitude, and altitude, respectively. The positioning results of the algorithm in this embodiment are significantly improved compared to the standard KF algorithm, and are partially improved compared to the IWKF algorithm.

[0220] Table 3 Position error RMSE results

[0221]

[0222]

[0223] This embodiment addresses the vulnerability of SINS / DME / DME to signal interference in the complex environment of the terminal area and proposes a robust filtering algorithm based on the chi-square test. First, the Mahalanobis distance of the residuals during the SINS / DME / DME integrated navigation process is used as a criterion to select an appropriate chi-square test threshold. Then, a robustness factor is constructed using a three-segment method, and the measurement noise matrix is ​​weighted to obtain an estimate of the measurement noise. Finally, the filter gain is incorporated to achieve control and regulation of the measurement information. Furthermore, in the event of sudden measurement information failures, the algorithm of this embodiment can sensitively detect abnormal information and isolate the faulty information to prevent it from affecting the combined positioning results, thereby enhancing the robustness of the filter. In the event of gross errors in the measurement information, the algorithm of this embodiment can provide a more optimal positioning result for the integrated navigation system while ensuring filter stability. The effectiveness of the algorithm proposed in this embodiment was verified by combining two experimental scenarios, providing certain practical reference value.

[0224] The above-described embodiments merely represent specific implementations of the present invention. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art would be able to make numerous variations and improvements without departing from the spirit of the present invention, and all such variations and improvements fall within the scope of protection of the present invention.

Claims

1. A robust method for SINS / DME / DME integrated navigation based on chi-square test, characterized in that: The following steps are involved: Step 1: Establish a strapdown inertial navigation system (SINS) / DME / DME integrated navigation system and derive the measurement equations for DME / DME positioning. Step 2: Select an appropriate chi-square test threshold value based on the Mahalanobis distance of the residuals in the SINS / DME / DME combined navigation process; The step 2 is specifically as follows: For the SINS / DME / DME integrated navigation system, the SINS inference calculation and DME measurement data are fused using the Kalman filter algorithm, which includes two parts: time update and measurement update: Time updates include state updates and covariance updates: X k / k-1 =F k X k-1 / k-1 (31) The measurement update includes filter gain, state estimation, and variance estimation: P k =(I-K k H k )P k / k-1 (35) Before performing robust adaptive processing, detect abnormal information in advance and then perform filtering processing; let the residual be ε k , which is expressed as follows: In formula (36), Z k is the measured value; when the measured value fails, ε k Does not obey Gaussian distribution; when the measured value is fault-free, ε k Conforms to a Gaussian distribution with a mean of zero; According to the orthogonality principle, the square of the residual Mahalanobis distance obeys the chi-square distribution, and its chi-square test statistic is as follows: By setting the chi-square test significance level α, the residual ε is judged k the credibility of Let the residual ν k for: Measurement discriminant function λ k for: In formula (38), Represents the optimal estimate of the state at the previous moment, which is used to eliminate the influence of the covariance error and system noise on the residual at the current moment; Step 3: Construct a robustness factor using a three-stage method, weight the measurement noise matrix, and obtain an estimated value of the measurement noise; In step 3, the robustness factor is constructed using the three-stage method as follows: In order to solve the problem of abnormal measurement information during filtering, the noise variance R is amplified by the robustness factor. k , adjust the gain matrix K k The value of , reduces the weight of observation information, weakens the optimal estimate of the state at the kth moment The influence of ; thus, formula (33) is rewritten as follows: In formula (40), T k is the robustness factor; The three-stage method is used, corresponding to the weight function's retention zone, weight reduction zone, and elimination zone; its function expression is: In formula (41): is the standardized new information of the i-th component; k0 and k1 are adjustment coefficients and are constant values, where is calculated as follows: In formula (42), is the new information sequence of the i-th component up to time k, The distance of the normalized value of the distance of the i-th component is calculated as follows: In formula (43): ·med(·) is the median; Step 4: Input the filter gain to realize the control and adjustment of the measurement information.

2. The robust method for SINS / DME / DME integrated navigation based on chi-square test according to claim 1, characterized in that: In step 1, the strapdown inertial navigation system SINS / DME / DME integrated navigation system is established as follows: The system equations of SINS / DME / DME integrated navigation are described as follows: X k =F k,k-1 X k-1 +Γ k W k (1) In formula (1): X k is the error state at time k, F k / k-1 is the state transition matrix from time k-1 to time k, X k-1 is the error state at the previous moment, Γ k is the noise driving matrix, W k is the system noise matrix; The measurement equation is expressed as: Z k =H k X k +V k (2) In formula (2): Z k is the measurement error between the two systems, H k is the measurement matrix, X k is the error at time k, V k is the measurement noise matrix; The state equation consists of the SINS misalignment angle, velocity error, position error, gyroscope error, and accelerometer error. The error state of the SINS is selected as 15 dimensions, as follows: In formula (3): Φ, δv n , δp represent the misalignment angle, velocity error, and position error of SINS in the navigation coordinate system, respectively, and ε b is the projection of the gyroscope zero bias in the carrier coordinate system, is the projection of the accelerometer zero bias in the carrier coordinate system; The state transfer matrix of the system represents the relationship between the system state at the previous moment and the system state at the current moment. The SINS position error differential equation is as follows: In the above formulas (4), (5), and (6): is the differential of the latitude and longitude error, δv E ,δv N ,δv U is the velocity error component in the northeast celestial coordinate system, δL, δλ, and δh are the latitude and longitude error components, and R M is the meridian radius, R N is the radius of the Maoyou circle; make: Combining equations (4)-(8) we get: Similarly, the velocity error differential equation is: Differential equation for misalignment angle: In formulas (10) and (11): is the transfer matrix from the carrier coordinate system to the navigation coordinate system; M va 、M vv 、M vp With M aa 、M av 、M ap are the conversion matrices of velocity error differential and misalignment angle differential and the misalignment angle, velocity error and position error of the strapdown inertial navigation platform respectively; Combining equations (9), (10), and (11), the state transfer matrix F of the INS / DME / DME integrated navigation system is:

3. The robust method for SINS / DME / DME integrated navigation based on chi-square test according to claim 2, characterized in that: In step 1, the derivation method of the measurement equation is as follows: During the flight, the aircraft uses the onboard DME receiver to measure the slant range of the aircraft relative to the two DME stations. At the same time, the DME ground station obtains the measured slant range by using a question-and-answer method with the onboard DME antenna. The slant range error is expressed as: δρ=ρ C -r D (13) In formula (13), δρ is the slant range error, ρ C is the calculated slant distance of the aircraft, ρ D Slant range measured for the DME station; The slant range ρ measured by the DME station D It is expressed as the relationship between the true slant range and the DME-related noise: r D =p+sn D (14) In formula (14), ρ is the true slant range, δν D is the measurement noise of DME, and δν D The mean of is zero; Aircraft calculated slant range ρ C The aircraft position in Cartesian coordinates (x P y P z P ) and DME station position (x D y D z D ) is represented as follows: Differentiate equation (15) to obtain the error δρ of the slope distance C , the results are as follows: During the flight, the position of the aircraft in the Earth-centered Earth-fixed coordinate system is (L λ h), and the position of the DME station is (L D λ D h D ), then the ECEF coordinate system is converted to the Cartesian coordinate system, and the conversion relationship is as follows: x=(R N +h)·cosLcosλ (17) y=(R N +h)·cosLsinλ (18) z=(R M +h)·sinL (19) Differentiating equations (17), (18), and (19) respectively yields the differential relationship between the ECEF coordinate system and the Cartesian coordinate system: δx=-(R N +h)·sinLcosλδL -(R N +h)cosLsinλδλ +cosLcosλδh(20) δy=-(R N +h)·sinLcosλδL +(R N +h)cosLcosλδλ +cosLsinλδh(21) δz=(R M +h)·cosLδL+sinLδh (22) make: M=[M1 M2 M3] (27) The slant range error calculated by the aircraft is shown in formula (28): Combining equations (2), (13), (14), and (28), the error differential equation of the slant range is obtained as shown in equation (29): The altitude information is measured using a barometric altimeter to obtain the measurement matrix of the SINS / DME / DME integrated navigation system:

Images