A normalization-based inertial navigation 5-parameter coordinate transformation matrix solving method

Abstract

The application discloses a normalization-based inertial navigation 5-parameter coordinate transformation matrix solving method, which comprises the following steps: determining five parameters in two rows of a coordinate transformation matrix as real-time updated variables; adopting an integral formula to perform integral updating calculation on a 5-parameter differential equation to obtain values of the five parameters at t k time; calculating the remaining four parameters of the coordinate transformation matrix to finally obtain coordinate transformation matrix updating and support velocity updating and position updating, so as to improve the precision of inertial navigation. The application takes angular rates output by gyroscopes orthogonally installed on a body of a strapdown inertial system as input information, realizes real-time updating of the inertial navigation coordinate transformation matrix, and simplifies the calculation amount while ensuring the solving precision.

Description

TECHNICAL FIELD

[0001] The application relates to a normalization-based inertial navigation 5-parameter coordinate transformation matrix solving method and belongs to the technical field of inertial navigation. BACKGROUND

[0002] Inertial navigation is widely used in spacecraft, aircraft, ships and other fields, and mainly serves to determine the position, velocity and attitude information of a carrier relative to a navigation system. A strapdown inertial system is directly connected to the carrier, and a coordinate transformation matrix of the carrier coordinate system relative to the navigation system is obtained by measuring the angular velocity through a gyroscope and performing mathematical calculation.

[0003] Currently, methods for determining attitude information include direction cosine kinematic equations, Euler-Krylov angle kinematic equations and quaternion kinematic equations.

[0004] The quaternion parameter and its differential equation used in engineering applications have only four parameters, but the four parameters are only intermediate variables, and nine equations are required to solve the coordinate transformation matrix of the carrier coordinate system relative to the navigation system. In addition, the coordinate transformation matrix described by the quaternion is not uniquely related to the four parameters, because one coordinate transformation matrix can correspond to two different quaternions.

[0005] In comparison, the Euler-Krylov angle kinematic equation has only three parameters, but in the technical field of inertial devices (upper) (China Astronautics Publishing House) on page 46, it is considered that the kinematic equation described by the Euler-Krylov angle has singular points, and the equation will degenerate. A full-attitude motion description method based on Krylov angles is proposed in Chinese patent CN202010333184.9. However, the problem is that when the pitch angle is 90°, the error of the attitude solution is large after discretization processing, resulting in large errors in the speed and position solution.

[0006] The method of using the direction cosine array to represent the rotational motion relationship between two coordinate systems and the attitude matrix is convenient, intuitive and easy to understand, but the parameters of the transformation matrix and its differential equation have nine parameters, and the integral calculation amount is large. In the technical field of inertial devices (lower) (China Astronautics Publishing House) in chapter 18, a coordinate transformation matrix solving method based on the direction cosine kinematic equation and the quaternion kinematic equation is given.

[0007] Under the premise of full attitude, to achieve one-to-one correspondence between the coordinate transformation matrix and the parameters, and to solve the problem of large calculation amount caused by the nine parameters of the direction cosine array, a normalization-based inertial navigation 5-parameter coordinate transformation matrix solving method is proposed in Chinese patent CN202210617894.3, but one of the six conditions needs to be selected as the update equation. SUMMARY

[0008] The technical problem solved by the present application is to overcome the shortcomings of the prior art and provide a normalization-based inertial navigation 5-parameter coordinate transformation matrix solving method, which simplifies the judgment condition and reduces the calculation amount, and improves the rapidity of inertial navigation and flight control.

[0009] The object of the present application is achieved by the following technical solutions:

[0010] A normalization-based inertial navigation 5-parameter coordinate transformation matrix solving method takes the angular rate output by the gyroscope orthogonally mounted on the body of the strapdown inertial system as the input information of the kinematics equation of the 5-parameter coordinate transformation matrix, realizes real-time updating of the inertial navigation coordinate transformation matrix through integral solving, and does not appear singular value in the solving process, thereby ensuring the full attitude of the body coordinate system relative to the navigation coordinate system and the small calculation amount requirement.

[0011] Specifically, it comprises:

[0012] calculating the coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system at time t k ;

[0013] calculating the angular velocity of the body coordinate system relative to the navigation coordinate system at time t k ;

[0014] selecting 5 parameters from the coordinate transformation matrix;

[0015] updating and solving the 5 parameters according to the coordinate transformation matrix and the angular velocity to obtain an updated coordinate transformation matrix;

[0016] performing speed updating and position updating according to the updated coordinate transformation matrix.

[0017] Further, in the above solving method, the coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system is

[0018]

[0019] In the formula, a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 and a 33 are 9 parameters of the coordinate transformation matrix.

[0020] The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system at time t k is

[0021]

[0022] wherein a 11,k , a 12,k , a 13,k , a 21,k , a 22,k , a 23,k , a 31,k , a 32,k and a 33,k are nine parameters of the coordinate transformation matrix at time t k .

[0023] Further, in the above solving method, the angular velocity of the body coordinate system relative to the navigation coordinate system at time t k is calculated, specifically as follows:

[0024]

[0025] wherein ω , ω and ω

[0026] are the angular velocities on the body x, y and z axes respectively.

[0027] Further, in the above solving method, five of the nine parameters are selected from the coordinate transformation matrix, specifically as follows:

[0028] The five parameters are selected from a 11,k , a 21,k , a 31,k , the remaining two elements after the absolute value maximum of the three elements a 13,k , a 23,k and a 33,k is excluded.

[0029] Further, in the above solving method, the five parameters are updated according to the coordinate transformation matrix and the angular velocity, and an updated coordinate transformation matrix is calculated, specifically as follows:

[0030] When |a 13,k | is the maximum, the five parameters a 11,k , a 21,k , a 23,k , a 31,k and a 33,k are selected; and the values a k+1 , a k , a 11,k+1 , a 21,k+1 and a 23,k+1 after integration at the next time t 31,k+1 = t 33,k+1 + ΔT.23,k |max, the five parameters a 11,k , a 13,k , a 21,k , a 31,k and a 33,k are selected; at the next time t k+1 = t k + ΔT, the integrated values a 11,k+1 , a 13,k+1 , a 21,k+1 , a 31,k+1 and a 33,k+1 are selected;

[0031] When |a 33,k |max, the five parameters a 11,k , a 13,k , a 21,k , a 23,k and a 31,k are selected; at the next time t k+1 = t k + ΔT, the integrated values a 11,k+1 , a 13,k+1 , a 21,k+1 , a 23,k+1 and a 31,k+1 are selected;

[0032] Further, in the above solving method, when |a 13,k |max, the five parameters a 11,k , a 21,k , a 23,k , a 31,k and a 33,k are selected; at the next time t k+1 = t k + ΔT, the integrated values a 11,k+1 , a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 are selected, and the specific calculation formula is:

[0033]

[0034] The values of a k+1 , a 11,k+1 , a 21,k+1 , a 23,k+1 and a 31,k+1 at time t 33,k+1 are substituted into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:

[0035]

[0036] Further, in the above solving method, the said when |a23,k |max, five parameters a 11,k , a 13,k , a 21,k , a 31,k and a 33,k are selected; and the values a k+1 , a k , a 11,k+1 , a 13,k+1 and a 21,k+1 after integration at the next time t 31,k+1 = t 33,k+1 + ΔT are calculated according to the following formula:

[0037]

[0038] The values a k+1 , a 11,k+1 , a 12,k+1 , a 13,k+1 and a 31,k+1 at time t 32,k+1 are substituted into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:

[0039]

[0040] Further, in the above solving method, when |a 33,k |max, five parameters a 11,k , a 13,k , a 21,k , a 23,k and a 31,k are selected; and the values a k+1 , a k , a 11,k+1 , a 13,k+1 and a 21,k+1 after integration at the next time t 23,k+1 = t 31,k+1 + ΔT are calculated according to the following formula:

[0041]

[0042] The values a k+1 , a 11,k+1 , a 12,k+1 , a 21,k+1 and a 22,k+1 at time t 23,k+1 are substituted into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:

[0043]

[0044] Further, in the above solving method, the speed update and the position update are performed according to the updated coordinate transformation matrix, specifically:

[0045] When p is an inertial system, the navigation equation is:

[0046]

[0047] wherein, is an updated coordinate transformation matrix, is a visual acceleration, is a gravity acceleration, V is an updated speed, T is a transpose of a matrix, and r is an updated position.

[0048] Compared with the prior art, the present application has the following beneficial effects:

[0049] (1) The present application discloses an inertial navigation 5-parameter coordinate transformation matrix solving method based on normalization. Compared with an inertial navigation 5-parameter coordinate transformation matrix solving method based on orthogonality, the number of judgment conditions is reduced from 6 to 3, and the calculation amount is reduced.

[0050] (2) The present application discloses an inertial navigation 5-parameter coordinate transformation matrix solving method. Compared with a 9-parameter direction cosine method, four differential equations are reduced. Compared with a quaternion representation of a coordinate transformation matrix, the relationship between the 5 parameters and the coordinate transformation matrix has uniqueness.

[0051] (3) The present application discloses an inertial navigation 5-parameter coordinate transformation matrix solving method. Compared with a coordinate transformation matrix represented by three Kriele angles, full-attitude singular-free high-precision solving can be realized.

[0052] (4) The present application discloses an inertial navigation 5-parameter coordinate transformation matrix solving method. The structure is simple, and easy to implement in engineering. BRIEF DESCRIPTION OF DRAWINGS

[0053] Figure 1 is a step flowchart of the inertial navigation 5-parameter coordinate transformation matrix solving method of the present application;

[0054] Figure 2 is a relationship diagram of a strapdown inertial system body relative to a navigation coordinate system of the present application;

[0055] Figure 3 is a change process of 9 parameters of a coordinate transformation matrix solved based on the inertial navigation 5-parameter coordinate transformation matrix solving method of the present application during a three-turn rolling of an airplane;

[0056] Figure 4 is a three-dimensional trajectory of airplane motion based on the inertial navigation 5-parameter coordinate transformation matrix solving method of the present application. DETAILED DESCRIPTION

[0057] In order to make the objects, technical solutions and advantages of the present application clearer, the embodiments of the present application will be further described in detail below with reference to the drawings.

[0058] A normalization-based inertial navigation 5-parameter coordinate transformation matrix solving method, comprising: determining 5 parameters in two rows of the coordinate transformation matrix as real-time updated variables; using an integral formula to perform integral update calculation on the 5-parameter differential equation to obtain values of the 5 parameters at t k time; calculating the remaining 4 parameters of the coordinate transformation matrix, and finally obtaining the coordinate transformation matrix update and supporting the velocity update and the position update to improve the precision of the inertial navigation. The present application takes the angular rate output by the gyroscope orthogonally installed on the body of the strapdown inertial system as the input information, realizes the real-time update of the inertial navigation coordinate transformation matrix, and simplifies the calculation amount while ensuring the solving precision.

[0059] As shown in Figure 1 , specifically comprising:

[0060] As shown in Figure 2 , based on a strapdown inertial system fixed to a carrier, a body coordinate system (b system, as a moving system) corresponding to the strapdown inertial system is Ox′y′z′; a navigation coordinate system (p system, as a fixed system) describing the rotational motion of the carrier is Oxyz; the origins of the two coordinate systems coincide. In the rotation process, the angular velocity of the body of the strapdown inertial system relative to the navigation coordinate system is Then, the normalization-based inertial navigation 5-parameter coordinate transformation matrix solving method realizes the following steps:

[0061] (1), the coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system is

[0062]

[0063] In the formula, a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 and a 33 are 9 parameters of the coordinate transformation matrix.

[0064] The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system at t k time is

[0065]

[0066] In the formula, a11,k , a 12,k , a 13,k , a 21,k , a 22,k , a 23,k , a 31,k , a 32,k and a 33,k are 9 parameters of the coordinate transformation matrix at time t k

[0067] (2) According to the angular velocity output by the gyroscope mounted on the body of the strapdown inertial system, the angular velocity of the body relative to the navigation system at time t k

[0068] (3) Assuming that the sampling time is ΔT, the absolute values of a 13,k , a 23,k and a 33,k are sorted from small to large, and a 11,k , a 21,k and a 31,k in the coordinate transformation matrix are selected, as well as the remaining two elements after the maximum absolute value in the three elements a 13,k , a 23,k and a 33,k is excluded, as five parameters, the five parameters are updated and calculated in the following three cases, and the coordinate transformation matrix is calculated;

[0069] (3.1) When |a 13,k | is the maximum, five parameters a 11,k , a 21,k , a 23,k , a 31,k and a 33,k are selected; the values of a 11,k+1 , a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 after integration at the next time t k+1 = t k + ΔT are calculated, and the specific calculation formula is:

[0070]

[0071] Substitute the values of a k+1 , a 11,k+1 , a 21,k+1 , a 23,k+1 and a 31,k+1 at time t 33,k+1 into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system

[0072] ​​

[0073] (3.2), when |a 23,k | is maximum, select five parameters a 11,k , a 13,k , a 21,k , a 31,k and a 33,k ; the values of a k+1 , a k , a 11,k+1 , a 13,k+1 and a 21,k+1 after integration at the next time t 31,k+1 = t 33,k+1 + ΔT are as follows:

[0074]

[0075] Substitute the values of a k+1 , a 11,k+1 , a 12,k+1 , a 13,k+1 and a 31,k+1 at time t 32,k+1 into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:

[0076]

[0077] (3.3), when |a 33,k | is maximum, select five parameters a 11,k , a 13,k , a 21,k , a 23,k and a 31,k ; the values of a k+1 , a k , a 11,k+1 , a 13,k+1 and a 21,k+1 after integration at the next time t 23,k+1 = t 31,k+1 + ΔT are as follows:

[0078]

[0079] Substitute the values of a k+1 , a 11,k+1 , a 12,k+1 , a 21,k+1 and a 22,k+1 at time t 23,k+1 into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:

[0080]

[0081] (4) Update the coordinate transformation matrix calculated according to the integral, and support velocity update and position update to improve the precision of inertial navigation.

[0082] The gyroscopes mounted on the body of the strapdown inertial system are three single-degree-of-freedom gyroscopes or two double-degree-of-freedom gyroscopes.

[0083] The updated coordinate transformation matrix in step (4) is taken as the input of a velocity differential equation and the specific acceleration and the gravitational acceleration , and after integral calculation, the updated velocity V is obtained. The updated velocity V is taken as the input of a position differential equation , and after integral calculation, the updated position r is obtained.

[0084] Embodiment:

[0085] Firstly, the 5-parameter coordinate transformation matrix calculation method of the present application only has 5 differential equations to be calculated by integral, while the 9-parameter direction cosine matrix needs to calculate the following 9 differential equations.

[0086]

[0087] Therefore, the 5-parameter coordinate transformation matrix calculation method of the present application reduces the calculation amount compared with the 9-parameter direction cosine method.

[0088] In addition, compared with the 5-parameter coordinate transformation matrix calculation method based on orthogonality, the judgment conditions are reduced from 6 conditions: ① |a 13,k |max, |a 23,k |min, ② |a 13,k |max, |a 33,k |min, ③ |a 23,k |max, |a 13,k |min, ④ |a 23,k |max, |a 33,k |min, ⑤ |a 33,k |max, |a 13,k |min, ⑥ |a 33,k |max, |a 23,k |min, to 3 conditions: ① |a 13,k |max, ② |a 23,k |max, ③ |a 33,k |max, thereby reducing the calculation amount.

[0089] Secondly, although the 5-parameter coordinate transformation matrix calculation method of the present application has one more differential equation than the quaternion method, the coordinate transformation matrix represented by the quaternion

[0090] ​

[0091] In the formula, λ is an element of the scalar part of the quaternion, and ρ1, ρ2 and ρ3 are three elements of the vector part of the quaternion.

[0092] The coordinate transformation matrix must correspond to no less than two groups of parameters: λ, ρ1, ρ2, ρ3; -λ, -ρ1, -ρ2, -ρ3, which causes the non-unicity of the relationship between the coordinate transformation matrix and the rotation relationship of the carrier relative to the navigation system. However, the 5-parameter coordinate transformation matrix of the present application ensures the unicity of the relationship between the 5 parameters and the coordinate transformation matrix.

[0093] Finally, the data of a certain full attitude motion of an aircraft (Full Attitude Navigation Solution Method Based on Extended Krylov Angle, Journal of Chinese Inertial Technology, Vol. 29 No. 5, 2021) is solved by using the 5-parameter coordinate transformation matrix solving method of the present application. The solved 9 parameters are shown in Figure 3 The three-dimensional trajectory of the aircraft motion solved by navigation is shown in Figure 4 It can be seen that the large attitude high maneuvering motion process of the aircraft is well reproduced, which shows that the 5-parameter coordinate transformation matrix solving method of the present application can realize full attitude solution and is beneficial to flight control.

[0094] The contents not described in detail in the specification of the present application are the known technology of those skilled in the art.

[0095] Although the present application has been disclosed with the above preferred embodiments, it is not intended to limit the present application, and any person skilled in the art can make possible changes and modifications to the technical solutions of the present application by using the disclosed methods and technical contents without departing from the spirit and scope of the present application. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present application, which does not depart from the technical solutions of the present application, belongs to the protection scope of the technical solutions of the present application.

Claims

1. A method for solving a normalized inertial navigation 5 parameter coordinate transformation matrix, characterized by, Comprise: The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system at time t is calculated. k t. Computing the angular velocity of the body coordinate system relative to the navigation coordinate system at time t k t; select 5 parameters from the coordinate transformation matrix; update the 5 parameters according to the coordinate transformation matrix and angular velocity, and obtain an updated coordinate transformation matrix; update velocity and position according to the updated coordinate transformation matrix; The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system is where a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 and a 33 are nine parameters of the coordinate transformation matrix; At t k The coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system is where a 11,k , a 12,k , a 13,k , a 21,k , a 22,k , a 23,k , a 31,k , a 32,k and a 33,k are nine parameters of the coordinate transformation matrix at time instant t k . t k the angular velocity of the body coordinate system relative to the navigation coordinate system at the instant wherein, respectively the angular velocity on the body x, y, z axes; The way of selecting 5 parameters from 9 parameters in the coordinate transformation matrix is: Selecting a 11,k , a 21,k and a 31,k from the coordinate transformation matrix and the remaining two elements from among the three elements a 13,k , a 23,k and a 33,k with the largest absolute values are used as the five parameters; when|a 13,k At its maximum, select 5 parameters a. 11,k a 21,k a 23,k a 31,k and a 33,k ; at the next moment t k+1 =t k The value of +ΔT after integration is a 11,k+1 a 21,k+1 a 23,k+1 a 31,k+1 and a 33,k+1 The calculation formula is: Put t k+1 a 11,k+1 , a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 values into the following formula, and solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system Delta T is the sampling time, and sign() is the sign function.

2. A method for solving a normalized inertial navigation 5 parameter coordinate transformation matrix, characterized in that, Comprise: The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system at time t is calculated. k t is calculated. calculating the angular velocity of the body coordinate system relative to the navigation coordinate system at time t k t; select 5 parameters from the coordinate transformation matrix; update the 5 parameters according to the coordinate transformation matrix and angular velocity, and obtain an updated coordinate transformation matrix; update velocity and position according to the updated coordinate transformation matrix; The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system is where a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 and a 33 are nine parameters of the coordinate transformation matrix; At time t k The coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system is where a 11,k , a 12,k , a 13,k , a 21,k , a 22,k , a 23,k , a 31,k , a 32,k and a 33,k are nine parameters of the coordinate transformation matrix at time t k . t k the angular velocity of the body coordinate system relative to the navigation coordinate system at the instant wherein, respectively the angular velocity on the body x, y, z axes; The way of selecting 5 parameters from 9 parameters in the coordinate transformation matrix is: Selecting a 11,k , a 21,k and a 31,k from the coordinate transformation matrix and the remaining two elements from among the three elements a 13,k , a 23,k and a 33,k with the largest absolute values are taken as the five parameters; When |a 23,k | is maximum, five parameters a 11,k , a 13,k , a 21,k , a 31,k and a 33,k are selected; the calculation formula of the integrated values a k+1 , a k , a 11,k+1 , a 13,k+1 and a 21,k+1 at the next moment t 31,k+1 = t 33,k+1 + ΔT is: Put t k+1 a 11,k+1 , a 12,k+1 , a 13,k+1 , a 31,k+1 and a 32,k+1 values into the following formula, and solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system: Delta T is the sampling time, and sign() is the sign function.

3. A method for solving the matrix of coordinate transformation of inertial navigation 5 parameters based on normalization, characterized in that, Comprise: The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system at time t is calculated. k t. calculating the angular velocity of the body coordinate system relative to the navigation coordinate system at time t k t; select 5 parameters from the coordinate transformation matrix; update the 5 parameters according to the coordinate transformation matrix and angular velocity, and obtain an updated coordinate transformation matrix; update velocity and position according to the updated coordinate transformation matrix; The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system is where a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 and a 33 are nine parameters of the coordinate transformation matrix; At t k The coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system is where a 11,k , a 12,k , a 13,k , a 21,k , a 22,k , a 23,k , a 31,k , a 32,k , and a 33,k are nine parameters of the coordinate transformation matrix at time t k . t k the angular velocity of the body coordinate system relative to the navigation coordinate system at the instant wherein, respectively the angular velocity on the body x, y, z axes; The way of selecting 5 parameters from 9 parameters in the coordinate transformation matrix is: Selecting a 11,k , a 21,k , and a 31,k from the coordinate transformation matrix and the remaining two elements from among the three elements a 13,k , a 23,k , and a 33,k with the largest absolute values as the five parameters When |a 33,k | is maximum, five parameters a 11,k , a 13,k , a 21,k , a 23,k and a 31,k are selected; the calculation formula of the integrated values a k+1 , a k , a 11,k+1 , a 13,k+1 and a 21,k+1 at the next moment t 23,k+1 = t 31,k+1 + ΔT is: Put t k+1 a 11,k+1 , a 12,k+1 , a 21,k+1 , a 22,k+1 and a 23,k+1 values into the following equation to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system Delta T is the sampling time, and sign() is the sign function.

4. The method of claim 1 to 3, wherein, Comprise: select 5 parameters from the coordinate transformation matrix; wherein is the updated coordinate transformation matrix, is the visual acceleration, is the gravitational acceleration, V is the updated velocity, T is the transpose of a matrix, and r is the updated position.

5. The method of claim 1 to 3, wherein, update the 5 parameters according to the coordinate transformation matrix and angular velocity, and obtain an updated coordinate transformation matrix; 6. The method of claim 1 to 3, wherein, update velocity and position according to the updated coordinate transformation matrix; The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system is The way of selecting 5 parameters from 9 parameters in the coordinate transformation matrix is: Delta T is the sampling time, and sign() is the sign function. Update velocity and position according to the updated coordinate transformation matrix, comprising: When p is the inertial system, the navigation equation is: The gyroscopes installed on the body of the strapdown inertial system are 3 single-degree-of-freedom gyroscopes. The gyroscopes installed on the body of the strapdown inertial system are 2 double-degree-of-freedom gyroscopes.

Images