A fast and accurate alignment method of launch coordinate system

Abstract

The application discloses a kind of launch coordinate system fast fine alignment method, belong to the field of inertial navigation of vehicle, according to attitude error equation and velocity error equation, establish inertial device error equation;Select state variable to establish fast fine alignment method state equation;According to the velocity error of satellite navigation system output, according to the actual angular velocity output of gyro in launch system, the angular velocity error of gyro is obtained;With velocity error and angular velocity error as observation, the measurement equation of fast fine alignment method is established;According to fast fine alignment method state equation and fast fine alignment method measurement equation, kalman filtering is carried out to obtain misalignment angle estimated value, and fine alignment is completed.The conventional method is improved, the angular velocity error information is introduced into the measurement of measurement equation in the system alignment model, the external observable information is fully utilized, to improve the observability of system, improve the speed of initial alignment, realize fast alignment.

Description

Technical Field

[0001] This invention relates to the field of aircraft inertial navigation, and more specifically to a method for rapid and precise alignment of a launch coordinate system. Background Technology

[0002] Strapdown inertial navigation systems (SINS) are a commonly used navigation system for aircraft due to their advantages such as strong autonomy, resistance to interference, and comprehensive navigation information. Initial alignment is one of the key technologies of SINS, providing the aircraft with initial position, velocity, attitude, and other parameters, which are prerequisites for subsequent navigation calculations. Alignment can be divided into two stages: coarse alignment and fine alignment. Coarse alignment is characterized by its high speed but lower accuracy, while fine alignment is slower but more accurate. Combining the two stages provides more accurate alignment information for subsequent inertial navigation.

[0003] The navigation coordinate system is the reference coordinate system used by a strapdown inertial navigation system (SINS) when solving for navigation parameters. The launch coordinate system (launch frame) offers unique advantages as the aircraft's navigation coordinate system. For the initial fine alignment process of a strapdown SINS in the launch coordinate system, conventional Kalman filtering methods have poor observability and require approximately 180 seconds for yaw angle alignment, resulting in slow alignment speeds. However, in some scenarios, the aircraft needs to launch as quickly as possible, demanding high initial alignment speeds. Therefore, researching a fast fine alignment method for a strapdown SINS in the launch coordinate system is a key technical challenge in this field. Summary of the Invention

[0004] To address the aforementioned shortcomings in the prior art, this invention provides a method for rapid and precise alignment of the launch coordinate system.

[0005] To achieve the above-mentioned objectives, the technical solution adopted by this invention is as follows:

[0006] A method for rapid and precise alignment of a launch coordinate system includes the following steps:

[0007] S1. Establish the attitude error equation and velocity error equation of the static base strapdown inertial navigation system, and establish the error equation of the inertial device based on the attitude error equation and velocity error equation.

[0008] S2. Based on the established error equations of the inertial devices, select the misalignment angle error, velocity error, gyroscope random constant drift and accelerometer constant zero drift as state variables, and establish the state equations for the rapid precision alignment method.

[0009] S3. Based on the output velocity error of the satellite navigation system, obtain the angular velocity error of the gyroscope based on the actual angular velocity output of the gyroscope under the transmitting system;

[0010] S4. Establish the measurement equations for the rapid precision alignment method using velocity error and angular velocity error as the observables;

[0011] S5. Based on the state equation and measurement equation of the fast fine alignment method, Kalman filtering is performed to obtain the misalignment angle estimate, thus completing the fine alignment.

[0012] Furthermore, the attitude error equation of the static base strapdown inertial navigation system in S1 is expressed as:

[0013]

[0014] Where, φ g This is the misalignment angle error. This is the derivative of the misalignment angle error; Earth's rotational angular velocity at the launch point The antisymmetric matrix; The direction cosine matrix from the projectile coordinate system to the launch coordinate system; This refers to the measurement error of the gyroscope.

[0015] Furthermore, the velocity error equation of the strapdown inertial navigation system under the static base in S1 is expressed as:

[0016]

[0017] in, For the velocity error δV g The differential of F; g To measure the specific force f using an accelerometer under the launch system g The antisymmetric matrix, where f b For measuring specific force using accelerometers in the projectile coordinate system; δf b This refers to the accelerometer measurement error in the projectile coordinate system. φ is the direction cosine matrix from the projectile coordinate system to the launch coordinate system. g This refers to the misalignment angle error.

[0018] Furthermore, the error equation for the inertial device in S1 is expressed as:

[0019]

[0020] in, δf represents the measurement error of the gyroscope. b ε represents the accelerometer measurement error in the projectile coordinate system. b This refers to the gyroscope constant drift; w g White noise for angular velocity measurement; To achieve zero bias for the accelerometer constant; w a White noise for force measurement.

[0021] Furthermore, the state equation for the rapid alignment method in S2 is expressed as:

[0022]

[0023] In the formula, Let X be the state vector differential and X be the state vector.

[0024]

[0025] φ g For the misalignment angle error, δV g For velocity error, ε b For gyroscope random constant drift, The accelerometer constant value is zero drift;

[0026] F is the system matrix and

[0027]

[0028] Earth's rotational angular velocity at the launch point The antisymmetric matrix, F g To measure the specific force f using an accelerometer under the launch system g antisymmetric matrix, Let be the direction cosine matrix from the projectile coordinate system to the launch coordinate system, 0 3×3 It is a 3×3 zero matrix;

[0029] G is the noise transfer matrix and

[0030]

[0031] W is the process noise vector and

[0032]

[0033] w g White noise for angular velocity measurement, w a White noise for force measurement.

[0034] Furthermore, the measurement equation in S4 is expressed as:

[0035] Z = HX + V

[0036] Where Z is the measurement vector and

[0037]

[0038] In the formula, δV g For the selected speed error, The triaxial components of the selected velocity error; This is due to gyroscope measurement error. This is the theoretical angular velocity output of the gyroscope in the transmitting system. This represents the actual angular velocity output of the gyroscope in the launching system. and The actual angular velocity output of the gyroscope The three-axis components, ω e A0 is the Earth's rotation angular rate; B is the launch azimuth angle; and B is the latitude of the spacecraft launch point.

[0039] H is the measurement matrix and

[0040]

[0041] In the formula, 0 3×3 It is a 3×3 zero matrix, I 3×3 It is a 3×3 identity matrix. Let M be the direction cosine matrix from the projectile coordinate system to the launch coordinate system, and M be the process matrix.

[0042]

[0043] ω e A0 is the Earth's rotation angular rate; B is the launch azimuth angle; and B is the latitude of the spacecraft launch point.

[0044] V is the measurement noise vector and

[0045] V = [v x v y v z v gx v gy v gz ] T

[0046] v x v y v z v gx v gy and v gz It is Gaussian white noise with zero mean.

[0047] Furthermore, the specific method for performing Kalman filtering based on the state equation and measurement equation of the fast alignment method in S5 is as follows:

[0048] S51. The discretization of the state equation and the measurement equation are expressed as follows:

[0049] X k =Φ k / k-1 X k-1 +Γ k / k-1 W k-1

[0050] Z k =H k X k +Vk

[0051] Among them, X k X is the state vector at time k. k-1 W is the state vector at time k-1. k-1 Z is the process noise vector at time k-1. k H is the position measurement vector at time k. k Let V be the observation matrix at time k. k Let Φ be the measurement noise vector at time k. k / k-1 With Γ k / k-1 Discretization of the state equation and the noise driving matrix;

[0052] S52. Perform one-step state prediction based on the initial state.

[0053]

[0054] in, The convergence value of the state vector at time k-1;

[0055] S53. State estimation using one-step predictions:

[0056]

[0057] Among them, K k This is the filter gain matrix;

[0058]

[0059] Among them, R k Let P be the covariance matrix of the noise measured at time k. k / k-1 The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates.

[0060]

[0061] Among them, Q k-1 Let P be the covariance matrix of the system noise at time k-1. k-1 Estimate the mean square error matrix at time k-1, and estimate the mean square error matrix P at time k. k for

[0062]

[0063] Where I is the identity matrix;

[0064] S54. Given initial values ​​X0 and P0, measure the position vector Z at time k. k The state estimate at time k is obtained by recursion.

[0065] S55, obtained after repeated calculations n times. The convergence value, i.e., the attitude error value. Speed ​​error value

[0066] Furthermore, the specific method of fine alignment in S5 is as follows: based on the state estimation result, i.e., the misalignment angle φ g The corresponding quaternion Q k Speed ​​error δV g The inertial navigation system (INS) solution results are corrected and the alignment results are precisely aligned. The specific method is as follows:

[0067] The method for correcting attitude information is as follows:

[0068]

[0069] in, The calculated attitude quaternion, Q is the corrected attitude quaternion. k attitude error The corresponding attitude quaternion;

[0070] The method for correcting speed is as follows:

[0071]

[0072] Among them, V g The corrected speed, To calculate the speed, This represents the speed estimation error value.

[0073] The present invention has the following beneficial effects:

[0074] The conventional method has been improved by incorporating angular velocity error information into the measurement equation in the system alignment model. This fully utilizes external observable information to improve the observability of the system, increase the speed of initial alignment, and achieve rapid alignment. Attached Figure Description

[0075] Figure 1 This is a schematic diagram of the rapid and precise alignment method for the launching system of the present invention.

[0076] Figure 2 This is a schematic diagram of the launch coordinate system according to an embodiment of the present invention.

[0077] Figure 3 This is a schematic diagram showing the relationship between the launch coordinate system and the projectile coordinate system in an embodiment of the present invention. Detailed Implementation

[0078] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.

[0079] To achieve the above objectives, the present invention uses the following coordinate system and transformation relationships:

[0080] Commonly used navigation coordinate systems for strapdown inertial navigation systems include: geocentric inertial coordinate system (i-frame), geocentric-ground-fixed coordinate system (e-frame), launch inertial coordinate system (a-frame), launch coordinate system (g-frame), local horizontal coordinate system (l-frame), and projectile body coordinate system (b-frame). The launch coordinate system and projectile body coordinate system are defined as follows:

[0081] 1. Projective coordinate system, with the origin O. g x is the launch point. g The axis lies in the horizontal plane of the launch point and points in the aiming direction of the launch. g The axis is perpendicular to the horizontal plane of the launch point and points upwards, z g axis and x g axis, y g The axes form a right-handed rectangular coordinate system, and the launch coordinate system is fixed to the Earth. The geographical latitude B0, longitude λ0, altitude h0, and launch azimuth A0 of the launch point determine the relationship between the launch coordinate system and the Earth, such as... Figure 1 As shown.

[0082] 2. The projectile coordinate system has its origin at the center of mass of the aircraft; the x-axis is along the longitudinal axis of the aircraft and points towards the nose of the aircraft; the y-axis is in the longitudinal plane of symmetry of the aircraft, perpendicular to the x-axis and pointing upwards; the z-axis, together with the x-axis and y-axis, forms a right-handed rectangular coordinate system.

[0083] 3. Transformation relationship between launch coordinate system and projectile coordinate system

[0084] The direction cosine matrix from the launch coordinate system to the projectile coordinate system is The attitude angles of the aircraft in the launch coordinate system are determined by the pitch angle. The yaw angle ψ and roll angle γ are described by three Euler angles, with pitch first around the z-axis. The 3-2-1 rotation sequence of y-axis y-yawing ψ and finally x-axis roll γ, and the relationship between the launch coordinate system and the projectile coordinate system, are as follows: Figure 2 As shown, the direction cosine matrix is

[0085]

[0086] In the formula, ψ and γ are the pitch angle, yaw angle, and roll angle of the launching system, respectively.

[0087] A method for rapid and precise alignment of the launch coordinate system, such as Figure 3 As shown, it includes the following steps:

[0088] S1. Establish the attitude error equation and velocity error equation of the static base strapdown inertial navigation system, and establish the error equation of the inertial device based on the attitude error equation and velocity error equation.

[0089] The error equations of the strapdown inertial navigation system include attitude error equations, velocity error equations and position error equations. The attitude error equations and velocity error equations under the static base are shown in equations (2) and (3), respectively.

[0090]

[0091]

[0092] In the formula, φ g This is the misalignment angle error. This is the derivative of the misalignment angle error; Earth's rotational angular velocity at the launch point The antisymmetric matrix; The direction cosine matrix from the projectile coordinate system to the launch coordinate system; This refers to the measurement error of the gyroscope. For the velocity error δV g The differential of F; g for The antisymmetric matrix, where f b For measuring specific force using an accelerometer; δf b This refers to the measurement error of the accelerometer.

[0093] The error equation for inertial devices is shown in equation (4).

[0094]

[0095] In the formula, ε b This refers to the gyroscope constant drift; w g White noise for angular velocity measurement; To achieve zero bias for the accelerometer constant; w a White noise for force measurement.

[0096] S2. Based on the established error equations of the inertial devices, select the misalignment angle error, velocity error, gyroscope random constant drift and accelerometer constant zero drift as state variables, and establish the state equations for the rapid precision alignment method.

[0097] Selecting the misalignment angle error φ g Speed ​​error δV ggyroscope random constant drift ε b and accelerometer constant zero bias As the state variable, the state equation is established as shown in equation (5).

[0098]

[0099] In the formula, X is the state vector, as shown in equation (6).

[0100]

[0101] φ g For the misalignment angle error, δV g For velocity error, ε b For gyroscope random constant drift, This is the constant value of the accelerometer with zero drift.

[0102] F is the system matrix, as shown in equation (7).

[0103]

[0104] Earth's rotational angular velocity at the launch point The antisymmetric matrix, F g To measure the specific force f using an accelerometer under the launch system g antisymmetric matrix, Let be the direction cosine matrix from the projectile coordinate system to the launch coordinate system, 0 3×3 It is a 3×3 zero matrix.

[0105] G is the noise transfer matrix, as shown in equation (8).

[0106]

[0107] W is the process noise vector, as shown in equation (9).

[0108]

[0109] w g White noise for angular velocity measurement, w a White noise for force measurement.

[0110] S3. Based on the output velocity error of the satellite navigation system, obtain the angular velocity error of the gyroscope based on the actual angular velocity output of the gyroscope under the transmitting system;

[0111] Under ideal static base conditions, assuming that all axes of the launch coordinate system point in the true forward-upward-right direction, and that the gyroscope measurement has no error, then the angular velocity information output by the gyroscope is the Earth's rotational angular velocity. Its projection on each axis of the launch coordinate system is shown in equation (10).

[0112]

[0113] In the formula, ω e A0 is the Earth's rotation angular rate; B is the launch azimuth angle; and B is the latitude of the launch point.

[0114] In a real alignment environment, after the coarse alignment process is completed, the launch coordinate system (g) is calculated. ′ There will be a misalignment angle φ between the launch coordinate system (g-frame) and the launch coordinate system (g-frame). g gyroscope actual angular velocity output for

[0115]

[0116] In the formula, φ x φ y and φ z The three-axis components of the misalignment angle error φ are φ = [φ x φ y φ z ] T ;ε x ε y and ε z ε is the constant drift of the gyroscope. b The three-axis components; and The actual angular velocity output of the gyroscope The three-axis components. The theoretical angular velocity output of the gyroscope in the launching system.

[0117] S4. Establish the measurement equations for the rapid precision alignment method using velocity error and angular velocity error as the observables;

[0118] Select speed error δV g and angular velocity error For the measurement, the measurement equation is established as shown in equation (12).

[0119] Z = HX + V (12)

[0120] Where Z is the measurement vector, as shown in equation (13).

[0121]

[0122] In the formula, δV g For the selected speed error, The triaxial components of the selected velocity error; This is due to gyroscope measurement error. This is the theoretical angular velocity output of the gyroscope in the transmitting system. This represents the actual angular velocity output of the gyroscope in the launching system. and The actual angular velocity output of the gyroscope The three-axis components, ω e A0 is the Earth's rotation angular rate; B is the launch azimuth angle; and B is the latitude of the launch point.

[0123] H is the measurement matrix and

[0124]

[0125] In the formula, 0 3×3 It is a 3×3 zero matrix, I 3×3 It is a 3×3 identity matrix. M is the direction cosine matrix from the projectile coordinate system to the launch coordinate system, and M is the process matrix, as shown in equation (15).

[0126]

[0127] ω e A0 is the Earth's rotation angular rate; B is the launch azimuth angle; and B is the latitude of the launch point.

[0128] V is the measurement noise vector, as shown in equation (16).

[0129] V = [v x v y v z v gx v gy v gz ] T (16)

[0130] In the formula, v x v y v z v gx v gy and v gz It is Gaussian white noise with zero mean.

[0131] S5. Based on the state equation and measurement equation of the fast fine alignment method, Kalman filtering is performed to obtain the misalignment angle estimate, thus completing the fine alignment.

[0132] The specific method for performing Kalman filtering based on the state equation and measurement equation of the fast alignment method is as follows:

[0133] S51. The discretization of the state equation and the measurement equation are expressed as follows:

[0134] X k =Φ k / k-1 X k-1 +Γ k / k-1 W k-1 (17)

[0135] Z k =H k X k +V k (18)

[0136] Among them, X k X is the state vector at time k. k-1 W is the state vector at time k-1. k-1 Z is the process noise vector at time k-1. k H is the position measurement vector at time k. k Let V be the observation matrix at time k. k Let Φ be the measurement noise vector at time k. k / k-1 With Γ k / k-1 Discretization of the state equation and the noise driving matrix;

[0137] S52. Perform one-step state prediction based on the initial state.

[0138]

[0139] in, The convergence value of the state vector at time k-1;

[0140] S53. State estimation using one-step predictions:

[0141]

[0142] Among them, K k This is the filter gain matrix;

[0143]

[0144] Among them, R k Let P be the covariance matrix of the noise measured at time k. k / k-1 The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates.

[0145]

[0146] Among them, Q k-1 Let P be the covariance matrix of the system noise at time k-1. k-1 Estimate the mean square error matrix at time k-1, and estimate the mean square error matrix P at time k. k for

[0147]

[0148] Where I is the identity matrix;

[0149] S54. Given initial values ​​X0 and P0, measure the position vector Z at time k. k The state estimate at time k is obtained by recursion.

[0150] S55, obtained after repeated calculations n times. The convergence value, i.e., the attitude error value. Speed ​​error value

[0151] Precision alignment includes corrections to attitude and velocity, among which

[0152] (1) Attitude correction:

[0153] Based on the relationship between the true transformation matrix and the calculated transformation matrix It can be obtained

[0154]

[0155] remember Equation 24 can be written as Calculated using quaternions

[0156]

[0157] in The calculated attitude quaternion, Q is the corrected attitude quaternion. k The attitude error φ g The corresponding quaternion.

[0158] (2) Speed ​​correction:

[0159]

[0160] Among them, V g The corrected speed, To calculate the speed, This represents the speed estimation error value.

[0161] Specific embodiments have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

[0162] Those skilled in the art will recognize that the embodiments described herein are intended to help the reader understand the principles of the invention, and should be understood that the scope of protection of the invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific modifications and combinations based on the technical teachings disclosed in this invention without departing from the spirit of the invention, and these modifications and combinations are still within the scope of protection of this invention.

Claims

1. A method for rapid and precise alignment of a launch coordinate system, characterized in that, Includes the following steps: S1. Establish the attitude error equation and velocity error equation of the static base strapdown inertial navigation system, and establish the error equation of the inertial device based on the attitude error equation and velocity error equation. S2. Based on the established error equations of the inertial devices, select the misalignment angle error, velocity error, gyroscope random constant drift and accelerometer constant zero drift as state variables, and establish the state equations for the rapid precision alignment method. S3. Based on the output velocity error of the satellite navigation system, obtain the angular velocity error of the gyroscope based on the actual angular velocity output of the gyroscope under the transmitting system; S4. Establish the measurement equations for the rapid precision alignment method using velocity error and angular velocity error as the observables. The measurement equations are expressed as follows: in, For measurement vector and In the formula, For the selected speed error, The three-axis components of the selected velocity error; This is due to gyroscope measurement error. This is the theoretical angular velocity output of the gyroscope in the transmitting system. This represents the actual angular velocity output of the gyroscope in the launching system. , and The actual angular velocity output of the gyroscope The three-axis components, This is the Earth's rotational angular rate; This refers to the launch azimuth angle; B The latitude of the launch point of the aircraft; The measurement matrix and In the formula, for Zero matrix, for identity matrix Let the direction cosine matrix be the distance from the projectile coordinate system to the launch coordinate system. It is a process matrix and This is the Earth's rotational angular rate; This refers to the launch azimuth angle; B The latitude of the launch point of the aircraft; To measure the noise vector and , , , , and It is Gaussian white noise with zero mean; S5. Based on the state equation and measurement equation of the fast fine alignment method, Kalman filtering is performed to obtain the misalignment angle estimate, thus completing the fine alignment.

2. The rapid and precise alignment method for a launch coordinate system according to claim 1, characterized in that, The attitude error equation of the static base strapdown inertial navigation system in S1 is expressed as follows: in, This is the misalignment angle error. This is the derivative of the misalignment angle error; Earth's rotational angular velocity at the launch point The antisymmetric matrix; The direction cosine matrix from the projectile coordinate system to the launch coordinate system; This refers to the measurement error of the gyroscope.

3. The rapid and precise alignment method for a launch coordinate system according to claim 1, characterized in that, The velocity error equation for the strapdown inertial navigation system under the static base in S1 is expressed as follows: in, For speed error The differential; Measure specific force using accelerometers in the launch system The antisymmetric matrix, where For measuring specific force using accelerometers in the projectile coordinate system; This refers to the accelerometer measurement error in the projectile coordinate system. The direction cosine matrix from the projectile coordinate system to the launch coordinate system; This refers to the misalignment angle error.

4. The rapid and precise alignment method for a launch coordinate system according to claim 1, characterized in that, The error equation for the inertial device in S1 is expressed as follows: in, This is due to gyroscope measurement error. This refers to the accelerometer measurement error in the projectile coordinate system. This refers to the gyroscope's constant drift. White noise for angular velocity measurement; The accelerometer constant is zero bias; White noise for force measurement.

5. The rapid and precise alignment method for a launch coordinate system according to claim 1, characterized in that, The state equation for the rapid alignment method in S2 is expressed as follows: In the formula, For the state vector differentiation, It is a state vector and This is the misalignment angle error. For speed error, For gyroscope random constant drift, The accelerometer constant value is zero drift; The system matrix and Earth's rotational angular velocity at the launch point antisymmetric matrix, Measure specific force using accelerometers in the launch system antisymmetric matrix, Let the direction cosine matrix be the distance from the projectile coordinate system to the launch coordinate system. for Zero matrix; It is a noise transfer matrix and The process noise vector and White noise for angular velocity measurement White noise for force measurement.

6. The rapid and precise alignment method for a launch coordinate system according to claim 1, characterized in that, The specific method for performing Kalman filtering based on the state equation and measurement equation of the fast alignment method in S5 is as follows: S51. The discretization of the state equation and the measurement equation are expressed as follows: in, yes k The state vector at time t, yes The state vector at time t, yes The process noise vector at time step, for k Position measurement vector at time, for k The observation matrix at time, for k The measurement noise vector at time step, and Discretization of the state equation and the noise driving matrix; S52. Perform one-step state prediction based on the initial state. : in, for The convergence value of the state vector at time step; S53. State estimation using one-step predictions: in, This is the filter gain matrix; in, for k The covariance matrix of the noise measured at any time. The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates. in, for The covariance matrix of the system noise at any given time. for Estimate the mean square error matrix at each time step. k Time-estimation mean square error matrix for in, For unit array; S54, Given initial values and ,according to k Position measurement vector at time The result is obtained by recursion. k State estimation at time 1 ; S55, after repeated calculation n This time received The convergence value, i.e., the attitude error value. Speed ​​error value .

7. The rapid and precise alignment method for a launch coordinate system according to claim 6, characterized in that, The specific method for fine alignment in S5 is as follows: based on the state estimation result, i.e., the misalignment angle... The corresponding quaternion Speed ​​error The inertial navigation system (INS) solution results are corrected and the alignment results are precisely aligned. The specific method is as follows: The method for correcting attitude information is as follows: in, The calculated attitude quaternion, The corrected attitude quaternion. attitude error The corresponding attitude quaternion; The method for correcting speed is as follows: in, The corrected speed, To calculate the speed, This represents the speed estimation error value.

Images