A Latitude Estimation Method for a Strapdown Inertial Navigation System

Abstract

The present invention provides a latitude estimation method for a strapdown inertial navigation system, belonging to the technical field of strapdown inertial navigation latitude estimation. First, the acquisition data of the strapdown inertial navigation system is obtained; then, an optimal interval integration method is used to construct a latitude estimation error equation based on the obtained data, and a strapdown inertial navigation error equation is constructed according to the obtained data; then, a Kalman filter equation is constructed based on the constructed latitude estimation error equation and the strapdown inertial navigation error equation; the established Kalman filter equation is solved to determine the latitude where the strapdown inertial navigation system is located. The present invention suppresses the short-period error and white noise of the IMU while effectively separating the zero bias of the IMU, well reducing the influence of the inability of the existing integration method to separate the IMU zero bias on the latitude estimation, and further improving the latitude estimation accuracy.

Description

Technical Field

[0001] The present invention relates to a method for estimating the latitude of a strapdown inertial navigation system, and belongs to the technical field of strapdown inertial navigation latitude estimation. Background Art

[0002] Before the initial alignment and navigation solution of a traditional strapdown inertial navigation system (SINS), latitude information with a certain accuracy needs to be input by an external system. However, in special applications such as tunnel measurement, mine exploration, and underwater navigation, the system cannot receive satellite signals, resulting in an inability to accurately obtain latitude information. Therefore, SINS latitude estimation is required.

[0003] In the prior art, the measured values of a gyroscope and an accelerometer can be directly used to calculate the included angle between the angular velocity of the earth's rotation and the acceleration of gravity, but the accuracy requirements for inertial components (Inertial Measurement Unit, IMU) are extremely high. There is also a method of solving the latitude by using the geometric relationship of the change of the gravity vector in the inertial system. Although the indirect alignment method in the inertial system has a strong anti-interference effect, the estimation accuracy is easily affected by measurement noise and the influence of the base shaking. And the measurement noise is smoothed by specific force integration. Under the condition of taking variance as the standard, the optimal interval of specific force integration is studied, and it is proved that integration is an effective way to suppress the short-period error and white noise of the IMU, but the zero bias separation of the IMU cannot be realized, and it is difficult to ensure the latitude estimation accuracy. In addition, there is also a method of distinguishing the positive and negative signs of north and south latitudes in latitude estimation to construct a real-time latitude estimation and rough alignment process; there is also a method of estimating the latitude by setting a sliding window, introducing wavelet denoising and polynomial optimization to suppress the influence of the shaking base linear motion on the initial alignment. However, the zero bias errors of commonly used medium and high-precision fiber optic gyroscopes currently have obvious periodicity, and domestic fiber optic gyroscopes generally have problems of large zero bias noise and poor repeatability, and the zero bias stability generally reaches 0.1° / h to 1° / h. The integration method cannot separate the IMU zero bias, which affects the latitude estimation accuracy. Using the latitude obtained by the integration method directly in the fine alignment process will affect the accuracy of the zero bias estimation of inertial components and also affect the accuracy of subsequent navigation solutions. Summary of the Invention

[0004] The purpose of the present invention is to provide a method for estimating the latitude of a strapdown inertial navigation system to solve the problem of low latitude estimation accuracy caused by the inability of the traditional integration method to realize the zero bias separation of inertial components.

[0005] The present invention provides a method for estimating the latitude of a strapdown inertial navigation system, and the method includes the following steps:

[0006] 1) Obtain the acquisition data of the strapdown inertial navigation system, including the sampling time, the gravity vector acting on the carrier in the body coordinate system and the body inertial system, the included angle of the gravity vector acting on the carrier in the body inertial system at different times, and the angular velocity of the earth's rotation;

[0007] 2) Use the optimal interval integration method to construct a latitude estimation error equation based on the obtained data, and construct a strapdown inertial navigation error equation based on the obtained data;

[0008] The latitude estimation error equation in step 2) is:

[0009]

[0010] In the formula, θ is the integration vector included angle, α is the earth rotation angle, ω he is the angular velocity of the earth's rotation; h is the geocentric inertial system; e is the earth coordinate system, is the north gyro zero bias in the navigation inertial system;

[0011] 3) Construct a Kalman filter equation based on the constructed latitude estimation error equation and strapdown inertial navigation error equation, and the Kalman filter equation contains latitude information;

[0012] 4) Solve the established Kalman filter equation to determine the latitude where the strapdown inertial navigation system is located.

[0013] The latitude estimation method of the strapdown inertial navigation system proposed by the present invention uses the optimal interval integration method to construct a latitude estimation error equation, suppresses the IMU short-period error and white noise through the optimal interval integration method, constructs a strapdown inertial navigation error equation, and constructs a Kalman filter equation using the latitude estimation error equation and the strapdown inertial navigation error equation to solve the latitude error, which can effectively separate the IMU zero bias, well reduce the influence of the existing integration method that cannot separate the IMU zero bias on the latitude estimation, and further improve the latitude estimation accuracy.

[0014] Further, the Kalman filter equation in step 3) is:

[0015]

[0016]

[0017] In the formula, X k is the state equation, Z k is the measurement equation, F1 is the state equation matrix in the strapdown inertial navigation error equation, the superscript ~ indicates that the value has an error, the superscript b represents that the value is in the "right-front-up" body coordinate system, the superscript b0 represents that the value is in the body inertial system, φ E , φ N , φ Uare the misalignment angles in the east, north, and zenith directions, δv E , δv N , δv U are the velocity errors in the east, north, and zenith directions, are the gyro biases of the x, y, and z axes in the "right - front - up" vehicle coordinate system, are the accelerometer biases of the x, y, and z axes in the "right - front - up" vehicle coordinate system, v n is the vehicle velocity solved by SINS, is the attitude matrix from the b - frame to the b0 - frame; f b is the specific force information, △t is the sampling interval, α is the earth rotation angle, and are the velocity differentials at times t1 and t2 in the b0 - frame, n is the total number of epochs, the superscript n0 indicates that the value is in the navigation inertial frame, I is the identity matrix, and L is the latitude.

[0018] Further, to achieve the adaptive estimation of the measurement noise when it continuously decreases with the extension of the integration time, step 4) uses the Sage - Husa adaptive filter to solve the Kalman filter equation.

[0019] Further, the specific calculation process of the solution is as follows:

[0020] A. Calculate the predicted state vector and its covariance matrix P k,k-1 :

[0021]

[0022] where Q k-1 is the system noise covariance matrix, and k is the current filtering time;

[0023] B. Calculate the filtering gain matrix K k :

[0024]

[0025] where R k is the measurement noise covariance matrix;

[0026] C. Calculate the state vector and its covariance matrix P k :

[0027]

[0028] P k =(I - K k H2)P k,k-1

[0029] where \(I\) is the identity matrix;

[0030] D. Measurement noise adaptive estimation:

[0031] R 4,4 = (1 - β k )R 4,4 + β k △R 4,4

[0032]

[0033] where is a deformation of the Kalman filter innovation vector; \(R\) 4,4 is the 4th element of the main diagonal of the measurement noise covariance matrix, i.e., the variance of the optimal interval integration method latitude The weighting factor can be obtained from the fading factor \(b\), with \(β_0 = 1\).

[0034] Furthermore, the value of \(b\) ranges from 0.9 to 0.999. Brief Description of the Drawings

[0035] Figure 1 is the flowchart of the latitude estimation of the strapdown inertial navigation system of the present invention;

[0036] Figure 2 is the schematic diagram of the gravity vector latitude estimation;

[0037] Figure 3 is the schematic diagram of the integral latitude estimation;

[0038] Figure 4 is the 30s latitude estimation error of the optimal interval integration method in Experiment 1;

[0039] Figure 5 is the inertial system alignment misalignment angle supported by the integral method latitude estimation in Experiment 1;

[0040] Figure 6 is the comparison of the latitude estimation errors between the method of the present invention and the optimal interval integration method in Experiment 1;

[0041] Figure 7 is the gyro zero bias and mean error estimation results of the method of the present invention in Experiment 1;

[0042] Figure 8 is the traditional fine alignment zero bias estimation result supported by the integral method latitude in Experiment 1;

[0043] Figure 9 is the comparison of the fine alignment celestial misalignment angle estimation results in Experiment 1;

[0044] Figure 10 is the comparison of the 50 - time latitude estimation errors at the end of alignment in Experiment 1;

[0045] Figure 11 is the Allan variance of the random noise of the gyro x-axis in Experiment 2;

[0046] Figure 12(a) is the latitude estimation error of the measured data in Xining in Experiment 2;

[0047] Figure 12(b) is the latitude estimation error of the measured data in Lhasa in Experiment 2;

[0048] Figure 13(a) is the initial yaw angle estimation of the measured data in Xining in Experiment 2;

[0049] Figure 13(b) is the initial yaw angle estimation of the measured data in Lhasa in Experiment 2. Specific implementation manners

[0050] The following further describes the specific implementation manners of the present invention with reference to the accompanying drawings.

[0051] The present invention provides a method for estimating the latitude of a strapdown inertial navigation system. The specific process is as Figure 1 shown. First, the acquisition data of the strapdown inertial navigation system is obtained; then, according to the obtained data, an optimal interval integration method is used to construct a latitude estimation error equation, and a strapdown inertial navigation error equation is constructed according to the obtained data; then, a Kalman filter equation is constructed according to the constructed latitude estimation error equation and the strapdown inertial navigation error equation; the established Kalman filter equation is solved to determine the latitude where the strapdown inertial navigation system is located. The present invention suppresses the short-period error and white noise of the IMU, realizes the effective separation of the IMU zero bias, well reduces the influence of the inability of the existing integration method to separate the IMU zero bias on the latitude estimation, and further improves the latitude estimation accuracy.

[0052] Step 1. Data acquisition

[0053] The present invention obtains data according to a strapdown inertial navigation system. Mainly, the gyroscopes and accelerometers directly installed on the carrier in the strapdown inertial navigation system are used for data sampling. Among them, the gyroscopes are used to measure the angular velocity of the carrier, and the accelerometers are used to measure the components of the acceleration of the carrier's motion relative to the inertial coordinate system in the carrier coordinate system. The strapdown inertial navigation system includes three-axis accelerometers, which respectively measure the acceleration information in the right, front, and up directions of the carrier. When the carrier is stationary, the measured values of the three-axis accelerometers are the local three-dimensional gravity vector, expressed as [0 0g] T , where g represents the magnitude of the local gravitational acceleration.

[0054] Such as Figure 2As shown, in the strapdown inertial navigation system, the carrier is in the "right - front - up" carrier coordinate system (b - system). When starting the latitude estimation, the "right - front - up" carrier coordinate system (b - system) is frozen as the carrier inertial system (b0 - system). During the rotation of the earth, the projection of the gravity vector g acting on the carrier in the b0 - system constantly changes in direction. From the included angle θ between at times t1 and t2 and the earth rotation angle α, the carrier latitude L can be obtained. The calculation of the carrier latitude L is as follows: Assume the earth is a rotating ellipsoid, the intersection point of the carrier gravity vector and the earth axis is O′, the length from the carrier to O′ is R, point P is at time t1, and point P′ is at time t2. Figure 2 The following geometric relationships exist for each line segment in

[0055]

[0056] Solving gives:

[0057]

[0058] In the formula, θ and α are solved by the following formula:

[0059]

[0060] In the formula: ω he is the earth's angular velocity; h is the geocentric inertial system; e is the earth coordinate system.

[0061] Under static base or swaying base, the linear velocity of the SINS is approximately 0. From the SINS velocity differential equation, the gravity vector of the carrier in the b0 - system can be obtained as:

[0062]

[0063] In the formula: is the attitude matrix from the b - system to the b0 - system; f b is the specific force information. According to the attitude differential equation:

[0064]

[0065] Among them is calculated in real - time from the gyro output value

[0066] Step 2. Construct the latitude estimation error equation and the strapdown inertial navigation error equation

[0067] 1. Latitude estimation error equation

[0068] ​The present invention constructs a latitude estimation error equation by using the optimal interval integration method. Integration is an effective way to suppress the short-period errors and white noise of the IMU. On the premise of using variance as the evaluation criterion, considering that the denominator cannot be too small to cause calculation singularity, the highest accuracy of θ can be achieved when the integration interval of the gravity vector is (t2 - t1) / 2. Let t1 = 0, t2 = n△t, where n is the total number of epochs and △t is the sampling interval. i = 0, …, n represents the epoch order. After discretizing the integration, it is converted into a summation formula, and the integration of the gravity vector represents the velocity increment △v, that is

[0069]

[0070] where are the velocity increments at times t1 and t2 in the b0 system, respectively, and f t b is the specific force information at time t.

[0071] Convert Equation (3) to

[0072]

[0073] At this time, the corresponding α = ω he ·n△t / 2, and the integration process is as Figure 3 shown, whereby the effective smoothing of the specific force information can be achieved.

[0074] Project the integration vector △v n in the navigation system into the navigation inertial system n0. Assuming that the zero bias of the equivalent inertial element is a constant, combining Equation (6) and Equation (7) gives Equation (8):

[0075]

[0076] In the formula, the superscript ~ indicates that the value has an error, the superscript n indicates that the value is in the navigation system, and the superscript n0 indicates that the value is in the navigation inertial system. Ignoring the second-order small quantities of the gyroscope and accelerometer zero biases, since the initial alignment time is not long and the carrier vibration is not severe, it is considered that is a constant, For and have the same influence. Let Project to the navigation inertial system n 10 at t / 4, and it is obtained that the accelerometer zero bias does not affect the latitude estimation when the carrier vibration is not large. Calculate the velocity integration:

[0077]

[0078] Substitute Equation (9) and Equation (10) into Equation (8) to obtain Equation (11):

[0079]

[0080] Among them, are the gyro zero biases in the north, east, and up directions in the navigation inertial system.

[0081] First-order Taylor expansion Substituting Equation (11) gives Equation (12):

[0082]

[0083] Differentiating both sides of Equation (2) and substituting Equations (2) and (12) gives the relationship between δL and δθ as Equation (13):

[0084]

[0085] From Figure 3 It can be seen that the included angle θ of the integral vector corresponds to the earth's rotation angle α = ω he t / 2. Considering sin(θ / 2) = cosLsin(α / 2), Equation (14) can be obtained as:

[0086]

[0087] 2. Strapdown inertial navigation error equation

[0088] The present invention constructs a strapdown inertial navigation error equation through a Kalman filter model, and the specific process is as follows:

[0089] The traditional Kalman filter fine alignment state equation for a static base is composed of a simplified SINS error equation, and the measurement equation is provided by zero velocity. Its state space model is as follows:

[0090]

[0091] In Equation (15):

[0092]

[0093] In the formula, is the earth's angular velocity on the n coordinate system, φ E , φ N , φ U are the misalignment angles in the east, north, and up directions, δv E , δv N , δv U are the velocity errors in the east, north, and up directions, are the gyro zero biases of the x, y, and z axes in the "right - front - up" vehicle coordinate system, are the accelerometer zero biases of the x, y, and z axes in the "right - front - up" vehicle coordinate system, fn The specific force for acceleration measurement is projected in the navigation system, []× represents the skew-symmetric matrix of a three-dimensional vector, and v n is the vehicle velocity solved by the SINS. It can be seen that the local latitude error is not considered in the traditional static base alignment. At the same time, the latitude observation quantity is obtained by the optimal interval integration method instead of velocity integration, and the position differential equation in the mechanical arrangement is not applicable here.

[0094] Step 3. Construct the Kalman filter equation

[0095] Combine the latitude estimation error equation of the optimal interval integration method and the strapdown inertial navigation error equation to construct the Kalman filter equation, and model the latitude into the state equation (Equation (15)). The specific process is as follows:

[0096] First, rewrite Equation (8) in the b0 system to obtain Equation (17):

[0097]

[0098] Similar to Equations (12) and (13), we can get:

[0099]

[0100] where is the latitude state quantity obtained by one-step prediction of the filter, is the latitude of the optimal interval integration method. Due to the first-order approximation of , the above equation is only applicable to IMUs with relatively high accuracy. In summary, the new filter state space model is as follows:

[0101]

[0102] where:

[0103]

[0104] where the measurement equation is composed of the velocity error deduced from the strapdown inertial navigation error equation and the latitude of the optimal interval integration method in Equation (18) .

[0105] Step 4. Solve the Kalman filter equation

[0106] The present invention uses Sage-Husa adaptive filtering to solve the Kalman filter equation. Since the observation noise of obtained by integrating the gravity vector decreases continuously with the increase of the integration time, it is necessary to perform adaptive estimation of the measurement noise. The specific solution process is as follows:

[0107] A. Calculate the predicted state vector and its covariance matrix P k,k-1 :

[0108]

[0109] Wherein, Q k-1 is the system noise covariance matrix, and k is the current filtering time;

[0110] B. Calculate the filtering gain matrix K k :

[0111]

[0112] Wherein, R k is the measurement noise covariance matrix;

[0113] C. Calculate the state vector and its covariance matrix P k :

[0114]

[0115] D. Adaptive estimation of measurement noise:

[0116]

[0117] Wherein, is a deformation of the Kalman filter innovation vector; R 4,4 is the 4th element of the main diagonal of the measurement noise covariance matrix, that is, the variance of the optimal interval integration method latitude ; The weighting factor can be obtained from the fading factor b, and β0 = 1. b is the fading factor, usually taken between 0.9 and 0.999.

[0118] The latitude of the strapdown inertial navigation system can be determined through the above steps. In order to verify the reliability of the Kalman filter equation constructed by the method of the present invention in latitude calculation, the following two groups of experiments are carried out for verification.

[0119] Experiment 1: Simulation experiment

[0120] In Experiment 1, the optimal interval integration method and the method of the present invention are used to estimate the latitude error of the simulation data. In the experiment, it is assumed that the accuracy of the inertial components is: gyro constant zero bias ε b = 0.02° / h, gyro random drift accelerometer constant zero bias accelerometer random offset The sampling frequency is 100Hz. The latitude of the simulation data is 30°, the longitude is 110°, the altitude is 380m, and the initial attitude angle is [0°, 0°, 0°].

[0121] First, the data of a stationary base with a duration of 900 s was simulated. The optimal interval latitude estimation and inertial frame rough alignment were performed using the data of the first 30 s. The latitude estimation and attitude estimation results are as Figure 4 and Figure 5 shown. The latitude estimation error of the optimal interval integration method for 30 s can reach 0.2°, which is sufficient to support the inertial frame rough alignment. The horizontal misalignment angles of the inertial frame rough alignment for 30 s are all below 0.01°, and the vertical misalignment angle is below 1°, reaching the limit accuracy of the inertial frame alignment. The latitude estimation result and the rough alignment attitude can both be used as initial values in the next Kalman filter equation.

[0122] Then, the subsequent data was used to compare the latitude errors calculated by the method of the present invention and the optimal interval integration method. The comparison results are as Figure 6 shown. Optimal Integration is used to represent the optimal interval integration method, and KF Alignment is used to represent the method of the present invention. It can be clearly seen from Figure 6 that the latitude error obtained by the method of the present invention is significantly smaller than that obtained by the optimal interval integration method, which proves that the method of the present invention effectively improves the latitude estimation accuracy.

[0123] At the same time, the gyro bias estimation results in the method of the present invention are as Figure 7 shown. It can be seen that the observability of the equivalent north gyro bias is relatively strong, and the bias can be estimated more accurately. The zero bias estimation result of the optimal interval integration method is as Figure 8 shown. It can be seen that the equivalent north gyro bias estimation is inaccurate. By comparing Figure 7 and Figure 8 it shows that the method of the present invention can better separate the inertial element zero bias, thereby improving the latitude estimation accuracy. Figure 9 shows the vertical misalignment angle errors of the two methods. It can be seen that the initial alignment attitude outputs are all close to the limit accuracy of the stationary base alignment. Therefore, the main purpose of this method is to improve the accuracy of local latitude estimation and provide a relatively accurate initial position reference for the next navigation solution. For further analysis, 50 groups of Monte Carlo simulation experiments were carried out. The results are as Figure 10 shown. The statistical characteristics of the experimental results are shown in Table 1. The statistical results are expressed by the mean and root mean square error (RMSE). It can be seen that the latitude accuracy obtained by the alignment of the method of the present invention has been greatly improved, which is significantly higher than that obtained by the optimal interval integration method. The RMSE of the present invention is reduced by more than 0.1° compared with the optimal interval integration method.

[0124] Table 1:

[0125]

[0126] Experiment 2: Measured data

[0127] A domestic Kehua fiber optic inertial navigation of a certain model was adopted, and tests were carried out at a certain place in Lhasa with a latitude of 29.6627° and a certain place in Xining with a latitude of 36.6171° respectively. Some technical indicators of the fiber optic inertial navigation are shown in Table 2. The Allan variance analysis was used to determine the filtering random model. Figure 11 It is the analysis result of the x-axis of the gyroscope, in which the bias instability coefficient is 0.016° / h, and the angle random walk coefficient is

[0128] Table 2:

[0129]

[0130] Both experiments were carried out under a quasi-static base, and 900s of data was used for verification. The true latitude value was measured by static GNSS as a comparison. Figure 12(a) and 12(b) are the latitude estimation errors of the two methods at the two experimental sites. It can be seen that the latitude estimation errors obtained by the method of the present invention are all smaller than those of the optimal interval integration method. It can be seen that the latitude accuracy and stability of the method of the present invention in the measured data are both better than those of the optimal interval integration method, which is consistent with the results obtained from the simulation data. Among them, in the Xining experiment, the latitude error of the method of the present invention converges to less than 0.05°, and the standard deviation (Standard Deviation, Std) is 0.019°. The latitude error of the optimal interval integration converges to less than -0.2°, and the Std is 0.034°. In the Lhasa experiment, the latitude error of the method of the present invention converges to less than 0.1°, and the Std is 0.0529°. The optimal interval integration method converges to less than 0.3°, and the Std is 0.0779°.

[0131] Figures 13(a) and 13(b) are the initial alignment yaw angle results at the two experimental sites. The reference value is provided by the 2000s Kalman filter precise alignment when the latitude is the true value. It can be seen that the yaw angle of the local measured data converges to the vicinity of the reference value, indicating that the method of the present invention can achieve better attitude estimation results.

Claims

1. A latitude estimation method for a strapdown inertial navigation system, characterized in that The method includes the following steps: 1) Obtain the acquisition data of the strapdown inertial navigation system, including the sampling time, the gravity vector acting on the carrier in the carrier coordinate system and the carrier inertial system, the included angle of the gravity vectors acting on the carrier in the carrier inertial system at different times, and the earth's angular velocity of rotation; 2) Use the optimal interval integration method to construct a latitude estimation error equation based on the obtained data, and construct a strapdown inertial navigation error equation based on the obtained data; The latitude estimation error equation is: where θ is the included angle of the integration vector, α is the earth rotation angle, and ω he is the earth rotation angular velocity; h is the geocentric inertial system; e is the earth coordinate system, is the north gyro zero bias in the navigation inertial system; 3) Construct a Kalman filter equation based on the constructed latitude estimation error equation and the strapdown inertial navigation error equation, and the Kalman filter equation contains latitude information; 4) Solve the established Kalman filter equation to determine the latitude where the strapdown inertial navigation system is located.

2. The latitude estimation method of the strapdown inertial navigation system according to claim 1, characterized in that, The Kalman filter equation in step 3) is: where X k is the state equation, Z k is the measurement equation, F1 is the state equation matrix in the strapdown inertial navigation error equation. The superscript ~ indicates that the value has an error, the superscript b represents that the value is in the "right - front - up" vehicle coordinate system, and the superscript b0 represents that the value is in the vehicle inertial system. φ E , φ N , φ U are the misalignment angles in the east, north, and up directions. δv E , δv N , δv U are the velocity errors in the east, north, and up directions. are the gyro biases of the x, y, and z axes in the "right - front - up" vehicle coordinate system. are the accelerometer biases of the x, y, and z axes in the "right - front - up" vehicle coordinate system. v n is the vehicle velocity calculated by SINS. is the attitude matrix from the b - system to the b0 - system; f b is the specific force information, △t is the sampling interval, α is the earth rotation angle. and are the velocity differentials at times t1 and t2 in the b0 - system. n is the total number of epochs. The superscript n0 represents that the value is in the navigation inertial system. I is the identity matrix, and L is the latitude.

3. The latitude estimation method of the strapdown inertial navigation system according to claim 1, characterized in that In step 4), the Sage-Husa adaptive filter is used to solve the Kalman filter equation.

4. The latitude estimation method of the strapdown inertial navigation system according to claim 3, characterized in that The specific calculation process of the solution is: A. Calculate the predicted state vector and its covariance matrix P k,k-1 : where Q k-1 is the system noise covariance matrix, and k is the current filtering time; B. Calculate the filtering gain matrix K k : where R k is the measurement noise covariance matrix; C. Calculate the state vector and its covariance matrix P k : P k =(I - K k H2)P k,k-1 In the formula, I is the identity matrix; D. Adaptive estimation of measurement noise: R 4,4 = (1 - β k )R 4,4 + β k ΔR 4,4 In the formula, is the deformation of the Kalman filter innovation vector; R 4,4 is the 4th element of the main diagonal of the measurement noise covariance matrix, that is, the variance of the optimal interval integration method latitude ; the weighting factor can be obtained from the fading factor b, and β0 = 1.

5. The latitude estimation method of the strapdown inertial navigation system according to claim 4, characterized in that, The value of b ranges from 0.9 to 0.999.

Images