A launch system inertial navigation satellite navigation combined navigation method based on ST-EKF
Patent Information
- Application Number
- CN202611018286.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-07-09
- Publication Date
- 2026-10-13
AI Technical Summary
[0003]在发射系导航体系下，载体常处于高动态运动、电磁干扰复杂、卫星信号遮挡或断续失效的工况，传统的扩展卡尔曼滤波（Extended Kalman Filter，EKF）算法是当前组合导航的主流滤波算法，但该算法存在明显的短板
[0016]本申请提出的一种基于ST-EKF的发射系惯导卫导组合导航方法，建立了ST-EKF误差模型，ST-EKF通过优化状态方程和量测信息的处理，定位与测速精度和姿态估计精度相较传统EKF有显著提升，对于发射系下飞行器的初始对准和姿态控制意义重大，发射系下的飞行器常常面临高动态、强干扰或卫星信号受阻的恶劣环境，而ST-EKF在此类场景下表现出更强的适应性。通过采用与实际导航坐标系（如发射惯性系）速度信息相匹配的模式，ST-EKF能够构建更精确的量测方程，从而提高整个滤波过程的准确性。
Smart Images
Figure CN122900520A_ABST
Abstract
Description
Technical Field
[0001] The embodiments of this application relate to the field of inertial navigation and satellite navigation integrated technology, and particularly to a launch system inertial navigation and satellite navigation integrated method based on ST-EKF. Background Technology
[0002] Inertial navigation and satellite navigation combined technology is the core navigation technology for moving vehicles such as aircraft. Among them, the strapdown inertial navigation system (SINS) has advantages such as strong autonomy, high short-term navigation accuracy and strong anti-interference ability, while the global navigation satellite system (GNSS) can provide high-precision position and velocity information for a long time. The loosely coupled combination mode of the two is widely used in the navigation scenarios of launch system (i.e. launch coordinate system) aircraft.
[0003] In launch-based navigation systems, the carrier often operates under conditions of high dynamic motion, complex electromagnetic interference, and satellite signal blockage or intermittent failure. The traditional Extended Kalman Filter (EKF) algorithm is currently the mainstream filtering algorithm for integrated navigation, but it has significant shortcomings. The traditional EKF algorithm directly constructs the error equation based on the original inertial navigation state variables, retaining the specific force term in the model. However, the violent motion of the carrier under launch leads to large fluctuations and poor stability in the specific force value, easily causing the state variance estimate to deviate from the true value during filtering, resulting in variance inconsistency. Simultaneously, when the moving carrier (aircraft) is under complex conditions and observation information is incomplete, the state variables of the Inertial Measurement Unit (IMU) may become unobservable, further exacerbating filter divergence and leading to a further decrease in navigation accuracy. Most existing improved algorithms only optimize filter parameters or measurement models, failing to reconstruct the system state equations at their root. They cannot effectively avoid the effects of specific force disturbances and IMU unobservability, making them unsuitable for the high-dynamic, weakly observable requirements of launch-based aircraft.
[0004] In view of this, there is an urgent need in this field to propose a new state transformation filtering method to reconstruct the integrated navigation error model, solve the defects of traditional algorithms, and improve the stability and accuracy of inertial navigation / satellite navigation integrated navigation under the launch system. Summary of the Invention
[0005] To address the aforementioned technical problems, embodiments of this application propose a launch-based inertial navigation and satellite navigation integrated navigation method. By employing a state transformation method to remodel the integrated navigation system and replacing the specific force value in the system with a relatively stable gravity value, the method can prevent inconsistent variance estimation in the integrated navigation system and effectively improve the stability and accuracy of launch-based inertial navigation and satellite navigation integrated navigation.
[0006] To achieve the above objectives, embodiments of this application propose a launch-system inertial navigation and satellite navigation integrated navigation method based on ST-EKF. The method includes the following steps: S1, establishing an ST-EKF error model; S2, establishing an inertial navigation model and an inertial navigation error model under the launch system; S3, establishing an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the ST-EKF error model; S4, starting the inertial navigation system and completing preparatory work, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state; S5, updating the time-varying values in the measurement error model, the inertial navigation error model, and the integrated navigation error model in real time according to the navigation parameters of the inertial navigation system, and performing time updates; S6, updating satellite navigation measurements when the satellite navigation signal is valid; S7, correcting the inertial navigation system based on the error estimation results of Kalman filtering.
[0007] To achieve the above objectives, embodiments of this application also propose a launch-system inertial navigation and satellite navigation integrated navigation system based on ST-EKF. The system includes: an ST-EKF establishment module for establishing an ST-EKF error model; an inertial navigation establishment module for establishing an inertial navigation model and an inertial navigation error model under the launch system; an integrated navigation establishment module for establishing an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the ST-EKF error model; a preparation module for starting the inertial navigation system and completing preparation work, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state; a time update module for updating the time-varying values in the measurement error model, the inertial navigation error model, and the integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and performing time updates; a measurement update module for updating satellite navigation measurements when the satellite navigation signal is valid; and a correction module for correcting the inertial navigation system based on the error estimation results of Kalman filtering.
[0008] Optionally, in the current integrated navigation scenario, four coordinate systems are involved: the geocentric coordinate system, the launch coordinate system, the launch inertial coordinate system, and the carrier coordinate system. The geocentric coordinate system, i.e. The system, whose coordinate system origin Centered on Earth axis In the equatorial plane and pointing towards the Prime Meridian axis It is the Earth's axis of rotation and points towards the North Pole. axis In the equatorial plane and with axis , axis Construct a right-handed rectangular coordinate system; Launch coordinate system, i.e., launch system The system, whose coordinate system origin As the launch point, axis Within the horizontal plane of the launch point and pointing in the launch aiming direction. axis Perpendicular to the horizontal plane of the launch point and pointing upwards. axis and axis , axis Construct a right-handed rectangular coordinate system. The system is fixed to the Earth, and the geographical latitude of the launch point ,longitude ,high and launch azimuth Confirmed The relationship between the system and the Earth; Launch inertial coordinate system, i.e. Tie, The moment of launch and The systems coincide, but due to the Earth's rotation, the system fixed on the Earth... The orientation of the system in inertial space changes. System and All are inertial coordinate systems; The carrier coordinate system, i.e. The system, whose coordinate system origin At the center of mass of the aircraft, axis Coinciding with the longitudinal axis of the aircraft axis Located in the longitudinal plane of symmetry of the aircraft and perpendicular to axis , axis , axis and axis , axis This forms a right-handed rectangular coordinate system.
[0009] Optionally, a surrounding axis, axis, Axis rotation The rotation matrices are respectively , , ; (3-1); (3-2); (3-3); based on , , ,get Tie The direction cosine matrix of the system is , Tie The direction cosine matrix of the system is , and They are represented as follows: (3-4); (3-5); in, , , These represent the roll angle, pitch angle, and yaw angle, respectively.
[0010] Optionally, the construction of the velocity error equation for the ST-EKF error model includes: The velocity differential equation in the launching frame is expressed as: (4-1); in, for The differential form of the velocity in the system. For comparison, For gravity, Angular velocity vector The antisymmetric matrix is obtained by gyroscope measurement. for The speed of the tether; By introducing the assumption of a small error angle and using relevant conclusions, we can obtain: (4-2); in, The error angle is... express antisymmetric matrix, It is the identity matrix; Based on equation (4-2), the new velocity error state under the launch system is defined as follows: (4-3); in, For speed error, for The antisymmetric matrix; The velocity error is obtained according to equation (4-3). The differential equation is: (4-4); in, for The differential form, for The differential form; Substituting the attitude error equation and equation (4-2) into equation (4-4), we get: (4-5); in, To achieve zero bias of the equivalent accelerometer random constant, White noise for accelerometer specific force measurement, This represents the random constant drift of the equivalent gyroscope. For measuring the white noise of the gyroscope angular velocity, based on equation (4-5), it can be seen that the velocity error equation no longer has a ratio term. Instead, it is affected by gravity. What it replaced.
[0011] Optionally, the construction of the position error equation and the inertial device error equation of the ST-EKF error model includes: The position error equation is expressed as: (5-1); in, For positional error, for The differential form; Inertial device error modeling includes gyroscope drift and accelerometer drift, and gyroscope drift is included. Modeling the system as random constant plus white noise, with the gyroscope's constant zero bias as the system state, the gyroscope zero bias error equation is: (5-2); Accelerometer drift Modeling the system as a random constant plus white noise, with the accelerometer constant zero bias as the system state, the accelerometer zero bias error equation is: (5-3).
[0012] Optionally, based on the attitude error equation, velocity error equation, position error equation, and inertial device error equation, the Kalman filter state equation for the loosely coupled integrated navigation algorithm of the launch system is derived as follows: (6-1); (6-2); in, For state vectors, for The differential form, For noise driving matrix, This is the process noise vector. Given the mapping matrix, the detailed state equations are as follows: (6-3).
[0013] Optionally, In this system, the velocity and position of the inertial navigation system are expressed as: The velocity and position of the strapdown inertial navigation system under the launch system can be expressed as: (7-1); in, , For the velocity and position output by the inertial navigation system, , This represents the corresponding error when the inertial navigation system outputs velocity and position. , These are the true values of the aircraft's velocity and position. Loosely coupled integrated navigation uses velocity and position as observations. The velocity and position of the satellite navigation system are expressed as follows: (7-2); in, , For the speed and position output by the satellite navigation system, , To calculate the corresponding errors in the output velocity and position of the satellite navigation system, the velocity and position measurement vectors are obtained based on equations (7-1) and (7-2). for: (7-3); Suppose the Kalman filter measurement equation for the loosely coupled integrated navigation algorithm of the launch system inertial navigation satellite is: (7-4); (7-5); Therefore, we obtain The expression is: (7-6).
[0014] Optionally, the discretized forms of the state equation and the measurement equation are as follows: (8-1); (8-2); in, yes The system state at any given moment. and This is a discretized representation of the state equation and the noise driving matrix. The system excitation noise sequence, To measure the noise sequence; (8-3); (8-4); The state at the next moment is estimated using the wave recursion algorithm. The state is predicted in one step as follows: (8-5); The state is estimated as follows: (8-6); in, It is called the filter gain matrix, which is the weight of the observation information during state update; (8-7); in, The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates, representing the uncertainty of the estimate. (8-8); in, This represents the prior error, i.e., the calculation error. , To estimate the mean square error matrix, and Represented as: (8-9); (8-10).
[0015] Optionally, the attitude angle error is calculated using a navigation algorithm. The speed is The location is ; The attitude was corrected to: (9-1); in, The calculated attitude quaternion, The corrected attitude quaternion. For attitude angle error The corresponding quaternion; Speed correction: (9-2); The position is corrected to: (9-3).
[0016] This application proposes a launch-based inertial navigation and satellite navigation integrated navigation method. An ST-EKF error model is established. By optimizing the state equations and processing measurement information, ST-EKF significantly improves positioning, velocity measurement, and attitude estimation accuracy compared to the traditional EKF. This is of great significance for the initial alignment and attitude control of launch-based aircraft, which often face harsh environments with high dynamics, strong interference, or blocked satellite signals. ST-EKF demonstrates stronger adaptability in such scenarios. By adopting a mode that matches the velocity information of the actual navigation coordinate system (such as the launch inertial frame), ST-EKF can construct more accurate measurement equations, thereby improving the accuracy of the entire filtering process. Attached Figure Description
[0017] To more clearly illustrate the technical solutions in the embodiments or related technologies of this application, the accompanying drawings used in the description of the embodiments or related technologies of this application will be briefly introduced below. The following drawings are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. The drawings described herein are only used to explain this application and are not intended to limit this application.
[0018] Figure 1 This is a flowchart of a launch system inertial navigation satellite navigation integrated navigation method based on ST-EKF provided in one embodiment of this application; Figure 2 This is a detailed schematic diagram of a launch system inertial navigation satellite navigation integrated navigation method based on ST-EKF provided in one embodiment of this application; Figure 3 This is a schematic diagram of the launch coordinate system provided in one embodiment of this application; Figure 4 This is a schematic diagram of the structure of a launch system-inertial navigation-satellite navigation integrated navigation system based on ST-EKF, provided in another embodiment of this application. Detailed Implementation
[0019] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the various embodiments of this application will be described in detail below with reference to the accompanying drawings. Those skilled in the art will understand that many technical details have been presented in the embodiments of this application to facilitate better understanding. However, the technical solutions claimed in this application can be implemented even without these technical details and various variations and modifications based on the following embodiments. The division of the following embodiments is for ease of description and should not constitute any limitation on the specific implementation of this application. The following embodiments can be combined with and referenced by each other without contradiction.
[0020] One embodiment of this application proposes a launch system inertial navigation and satellite navigation integrated navigation method based on ST-EKF. The implementation details of the launch system inertial navigation and satellite navigation integrated navigation method based on ST-EKF proposed in this embodiment are described in detail below. The following implementation details are provided for ease of understanding and are not necessary for implementing this solution.
[0021] The specific process of the launch system inertial navigation and satellite navigation integrated navigation method based on ST-EKF proposed in this embodiment can be described as follows: Figure 1 As shown, its implementation details are as follows: Figure 2 As shown, the method includes: S1, Establish the ST-EKF error model.
[0022] S2, establish the inertial navigation model and inertial navigation error model under the launch system.
[0023] S3. Establish an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the ST-EKF error model.
[0024] S4. Activate the inertial navigation system and complete preparatory work, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state.
[0025] S5 updates the time-varying values in the measurement error model, inertial navigation error model, and integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and performs time updates.
[0026] S6 updates satellite navigation measurements when the satellite navigation signal is valid.
[0027] S7, the inertial navigation system is corrected based on the error estimation results of the Kalman filter.
[0028] The implementation details of each part of the ST-EKF-based launch system inertial navigation satellite navigation integrated navigation method proposed in this embodiment are described in detail below.
[0029] 1) Coordinate system and coordinate system transformation.
[0030] In the current integrated navigation scenario, four coordinate systems are involved: the geocentric coordinate system, the launch coordinate system, the launch inertial coordinate system, and the carrier coordinate system.
[0031] The geocentric coordinate system, i.e. The system, whose coordinate system origin Centered on Earth axis In the equatorial plane and pointing towards the Prime Meridian axis It is the Earth's axis of rotation and points towards the North Pole. axis In the equatorial plane and with axis , axis This forms a right-handed rectangular coordinate system.
[0032] Launch coordinate system, i.e., launch system The system, whose coordinate system origin As the launch point, axis Within the horizontal plane of the launch point and pointing in the launch aiming direction. axis Perpendicular to the horizontal plane of the launch point and pointing upwards. axis and axis , axis Construct a right-handed rectangular coordinate system. The system is fixed to the Earth, and the geographical latitude of the launch point ,longitude ,high and launch azimuth Confirmed The relationship between the system and Earth (please refer to...) Figure 3 ).
[0033] Launch inertial coordinate system, i.e. Tie, The moment of launch and The systems coincide, but due to the Earth's rotation, the system fixed on the Earth... The orientation of the system in inertial space changes. System and Both are inertial coordinate systems, but their coordinate origins and coordinate axis orientations differ.
[0034] The carrier coordinate system, i.e. The system, whose coordinate system origin At the center of mass of the aircraft, axis Coinciding with the longitudinal axis of the aircraft axis Located in the longitudinal plane of symmetry of the aircraft and perpendicular to axis , axis , axis and axis , axis This forms a right-handed rectangular coordinate system.
[0035] 2) Rotation matrix.
[0036] Let's surround axis, axis, Axis rotation The rotation matrices are respectively , , , respectively represented as: (3-1); (3-2); (3-3).
[0037] based on , , You can get Tie The direction cosine matrix of the system is , Tie The direction cosine matrix of the system is , and They are represented as follows: (3-4); (3-5); in, , , These represent the roll angle, pitch angle, and yaw angle, respectively.
[0038] 3) ST-EKF error model.
[0039] The construction of the ST-EKF error model includes velocity error equations, position error equations, and inertial device error equations.
[0040] The first is the velocity error equation.
[0041] The velocity differential equation in the launching frame is expressed as: (4-1); in, for The differential form of the velocity in the system. For comparison, For gravity, Angular velocity vector The antisymmetric matrix is obtained by gyroscope measurement. for The speed of the system.
[0042] By introducing the assumption of a small error angle and using relevant conclusions, we can obtain: (4-2); in, The error angle is... express antisymmetric matrix, It is an identity matrix.
[0043] Based on equation (4-2), the new velocity error state under the launch system is defined as follows: (4-3); in, For speed error, for An antisymmetric matrix.
[0044] The velocity error is obtained according to equation (4-3). The differential equation is: (4-4); in, for The differential form, for The differential form of .
[0045] Substituting the attitude error equation and equation (4-2) into equation (4-4), we get: (4-5); in, To achieve zero bias of the equivalent accelerometer random constant, White noise for accelerometer specific force measurement, This represents the random constant drift of the equivalent gyroscope. For measuring the white noise of the gyroscope angular velocity, based on equation (4-5), it can be seen that the velocity error equation no longer has a ratio term. Instead, it is affected by gravity. What it replaced.
[0046] The second is the position error equation.
[0047] The position error equation is expressed as: (5-1); in, For positional error, for The differential form of .
[0048] Finally, there is the error equation for inertial devices.
[0049] Inertial device error modeling includes gyroscope drift and accelerometer drift, and gyroscope drift is included. Modeling the system as random constant plus white noise, with the gyroscope's constant zero bias as the system state, the gyroscope zero bias error equation is: (5-2).
[0050] Accelerometer drift Modeling the system as a random constant plus white noise, with the accelerometer constant zero bias as the system state, the accelerometer zero bias error equation is: (5-3).
[0051] 4) ST-EKF loosely coupled SINS / GNSS integrated navigation algorithm.
[0052] 4-1) ST-EKF loosely coupled integrated navigation state equations.
[0053] Based on the attitude error equation, velocity error equation, position error equation, and inertial device error equation, the Kalman filter state equation for the loosely coupled integrated navigation algorithm of the launch system is derived as follows: (6-1); (6-2); in, For state vectors, for The differential form, For noise driving matrix, This is the process noise vector. Given the mapping matrix, the detailed state equations are as follows: (6-3).
[0054] 4-2) ST-EKF loosely coupled velocity-position measurement equation.
[0055] In this system, the velocity and position of the inertial navigation system are expressed as: The velocity and position of the strapdown inertial navigation system under the launch system can be expressed as: (7-1); in, , For the velocity and position output by the inertial navigation system, , This represents the corresponding error when the inertial navigation system outputs velocity and position. , These are the true values of the aircraft's speed and position.
[0056] Loosely coupled integrated navigation uses velocity and position as observations. The velocity and position of the satellite navigation system are expressed as follows: (7-2); in, , For the speed and position output by the satellite navigation system, , To calculate the corresponding errors in the output velocity and position of the satellite navigation system, the velocity and position measurement vectors are obtained based on equations (7-1) and (7-2). for: (7-3); Suppose the Kalman filter measurement equation for the loosely coupled integrated navigation algorithm of the launch system inertial navigation satellite is: (7-4); (7-5); Therefore, we obtain The expression is: (7-6).
[0057] 4-3) Launch system combined navigation filtering algorithm.
[0058] 4-3-1) Discretization of state equations and measurement equations.
[0059] The discretized forms of the state equation and the measurement equation are as follows: (8-1); (8-2); in, yes The system state at any given moment. and This is a discretized representation of the state equation and the noise driving matrix. The system excitation noise sequence, To measure the noise sequence.
[0060] (8-3); (8-4).
[0061] 4-3-2) Filtering recursive algorithm to estimate the state at the next moment.
[0062] The state at the next moment is estimated using the wave recursion algorithm. The state is predicted in one step as follows: (8-5).
[0063] The state is estimated as follows: (8-6); in, It is called the filter gain matrix, which is the weight of the observation information during state updates.
[0064] (8-7); in, The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates, representing the uncertainty of the estimate. (8-8); in, This represents the prior error, i.e., the calculation error. , To estimate the mean square error matrix, and Represented as: (8-9); (8-10).
[0065] 4-4) ST-EKF integrated navigation filter algorithm feedback correction.
[0066] The attitude angle error calculated using the navigation algorithm is: The speed is The location is .
[0067] The attitude was corrected to: (9-1); in, The calculated attitude quaternion, The corrected attitude quaternion. For attitude angle error The corresponding quaternion.
[0068] Speed correction: (9-2).
[0069] The position is corrected to: (9-3).
[0070] This embodiment proposes a launch-based inertial navigation and satellite navigation integrated navigation method. An ST-EKF error model is established. By optimizing the state equations and processing measurement information, ST-EKF significantly improves positioning, velocity measurement, and attitude estimation accuracy compared to the traditional EKF. This is of great significance for the initial alignment and attitude control of launch-based aircraft, which often face harsh environments with high dynamics, strong interference, or blocked satellite signals. ST-EKF demonstrates stronger adaptability in such scenarios. By adopting a mode that matches the velocity information of the actual navigation coordinate system (such as the launch inertial frame), ST-EKF can construct more accurate measurement equations, thereby improving the accuracy of the entire filtering process.
[0071] The steps described above are merely for clarity in describing the technical solution. In actual implementation, they can be combined into one step, or certain steps can be broken down into multiple steps, as long as they involve the same logical relationship, they are all within the scope of protection of this application. Any insignificant modifications or designs added to the algorithm or process, as long as they do not change the core of the algorithm or process, are also within the scope of protection of this application.
[0072] Another embodiment of this application proposes a launch-based inertial navigation and satellite navigation integrated navigation system based on ST-EKF. The details of the launch-based inertial navigation and satellite navigation integrated navigation system based on ST-EKF proposed in this embodiment are described in detail below. The following content is only for the convenience of understanding and is not necessary for implementing this solution. Figure 4 This is a schematic diagram of the structure of a launch system inertial navigation satellite navigation integrated navigation system based on ST-EKF proposed in this embodiment, including: ST-EKF establishment module M1, inertial navigation establishment module M2, integrated navigation establishment module M3, preparation module M4, time update module M5, measurement update module M6, and correction module M7.
[0073] ST-EKF establishment module M1 is used to establish the ST-EKF error model.
[0074] The inertial navigation establishment module M2 is used to establish the inertial navigation model and inertial navigation error model under the launch system.
[0075] The integrated navigation establishment module M3 is used to establish an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the ST-EKF error model.
[0076] The preparation module M4 is used to start the inertial navigation system and complete preparatory work, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state.
[0077] The time update module M5 is used to update the time-varying values in the measurement error model, inertial navigation error model, and integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and to perform time updates.
[0078] The measurement update module M6 is used to update satellite navigation measurements when the satellite navigation signal is valid.
[0079] The correction module M7 is used to correct the inertial navigation system based on the error estimation results of the Kalman filter.
[0080] It is worth noting that all modules involved in this embodiment are logical modules. In practical applications, a logical module can be a physical module, a part of a physical module, or an organic combination of multiple physical modules. Furthermore, to highlight the innovative aspects of this application, this embodiment does not introduce modules that are not closely related to solving the technical problems proposed in this application. However, this does not mean that other modules are absent from this embodiment.
[0081] It is not difficult to see that this embodiment is a system embodiment corresponding to the above method embodiments, and this embodiment can be implemented in conjunction with the above method embodiments. The relevant technical details and technical effects mentioned in the above method embodiments are still valid in this embodiment, and will not be repeated here to reduce repetition. Accordingly, the relevant technical details mentioned in this embodiment can also be applied to the above method embodiments.
[0082] It will be understood by those skilled in the art that the above embodiments are specific implementations of this application, and in practical applications, various changes can be made in form, detail, and description without departing from the spirit and scope of this application. For those skilled in the art, several improvements and modifications can be made without departing from the principles of this application, and these improvements and modifications are also considered to be within the scope of protection of this application.
Claims
1. A launch-based inertial navigation and satellite navigation integrated navigation method based on ST-EKF, characterized in that, include: S1, Establish the ST-EKF error model; S2, Establish the inertial navigation model and inertial navigation error model under the launch system; S3, Establish an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the ST-EKF error model; S4. Start the inertial navigation system and complete the preparatory work, including air alignment, according to the conventional workflow and working mode of the inertial navigation system, until the aircraft enters the inertial navigation state. S5 updates the time-varying values in the measurement error model, inertial navigation error model, and integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and performs time updates accordingly. S6, when the satellite guidance signal is valid, update the satellite guidance measurement; S7, the inertial navigation system is corrected based on the error estimation results of the Kalman filter.
2. The ST-EKF-based launch system inertial navigation satellite navigation integrated navigation method according to claim 1, characterized in that, In the current integrated navigation scenario, four coordinate systems are involved: the geocentric coordinate system, the launch coordinate system, the launch inertial coordinate system, and the carrier coordinate system. The geocentric coordinate system, i.e. The system, whose coordinate system origin. Centered on Earth axis In the equatorial plane and pointing towards the Prime Meridian axis It is the Earth's axis of rotation and points towards the North Pole. axis In the equatorial plane and with axis , axis Construct a right-handed rectangular coordinate system; Launch coordinate system, i.e., launch system The system, whose coordinate system origin. As the launch point, axis Within the horizontal plane of the launch point and pointing in the launch aiming direction. axis Perpendicular to the horizontal plane of the launch point and pointing upwards. axis and axis , axis Construct a right-handed rectangular coordinate system. The system is fixed to the Earth, and the geographical latitude of the launch point ,longitude ,high and launch azimuth Confirmed The relationship between the system and the Earth; Launch inertial coordinate system, i.e. Tie, The moment of launch and The systems coincide, but due to the Earth's rotation, the system fixed on the Earth... The orientation of the system in inertial space changes. System and All are inertial coordinate systems; The carrier coordinate system, i.e. The system, whose coordinate system origin. At the center of mass of the aircraft, axis Coinciding with the longitudinal axis of the aircraft axis Located in the longitudinal plane of symmetry of the aircraft and perpendicular to axis , axis , axis and axis , axis This forms a right-handed rectangular coordinate system.
3. The ST-EKF-based launch system inertial navigation satellite navigation integrated navigation method according to claim 2, characterized in that, Let's surround axis, axis, Axis rotation The rotation matrices are respectively , , ; （3-1）； （3-2）； （3-3）； based on , , ,get Tie The direction cosine matrix of the system is , Tie The direction cosine matrix of the system is , and They are represented as follows: （3-4）； （3-5）； in, , , These represent the roll angle, pitch angle, and yaw angle, respectively.
4. The ST-EKF-based launch system inertial navigation satellite navigation integrated navigation method according to claim 3, characterized in that, The construction of the velocity error equation for the ST-EKF error model includes: The velocity differential equation in the launching frame is expressed as: （4-1）； in, for The differential form of the velocity in the system. For comparison, For gravity, Angular velocity vector The antisymmetric matrix is obtained by gyroscope measurement. for The speed of the tether; By introducing the assumption of a small error angle and using relevant conclusions, we can obtain: （4-2）； in, The error angle is... express antisymmetric matrix, It is the identity matrix; Based on equation (4-2), the new velocity error state under the launch system is defined as follows: （4-3）； in, For speed error, for The antisymmetric matrix; The velocity error is obtained according to equation (4-3). The differential equation is: （4-4）； in, for The differential form, for The differential form; Substituting the attitude error equation and equation (4-2) into equation (4-4), we get: （4-5）； in, To achieve zero bias of the equivalent accelerometer random constant, White noise for accelerometer specific force measurement, This represents the random constant drift of the equivalent gyroscope. For measuring the white noise of the gyroscope angular velocity, based on equation (4-5), it can be seen that the velocity error equation no longer has a ratio term. Instead, it is affected by gravity. What it replaced.
5. The ST-EKF-based launch system inertial navigation satellite navigation integrated navigation method according to claim 4, characterized in that, The construction of the position error equation and inertial device error equation of the ST-EKF error model includes: The position error equation is expressed as: （5-1）； in, For positional error, for The differential form; Inertial device error modeling includes gyroscope drift and accelerometer drift, and gyroscope drift is included. Modeling the system as random constant plus white noise, with the gyroscope's constant zero bias as the system state, the gyroscope zero bias error equation is: （5-2）； Accelerometer drift Modeling the system as a random constant plus white noise, with the accelerometer constant zero bias as the system state, the accelerometer zero bias error equation is: （5-3）。 6. The ST-EKF-based launch system inertial navigation satellite navigation integrated navigation method according to claim 5, characterized in that, Based on the attitude error equation, velocity error equation, position error equation, and inertial device error equation, the Kalman filter state equation for the loosely coupled integrated navigation algorithm of the launch system is derived as follows: （6-1）； （6-2）； in, For state vectors, for The differential form, For noise driving matrix, This is the process noise vector. Given the mapping matrix, the detailed state equations are as follows: （6-3）。 7. A launch system inertial navigation satellite integrated navigation method based on ST-EKF according to claim 6, characterized in that, In this system, the velocity and position of the inertial navigation system are expressed as: The velocity and position of the strapdown inertial navigation system under the launch system can be expressed as: （7-1）； in, , For the velocity and position output by the inertial navigation system, , This represents the corresponding error when the inertial navigation system outputs velocity and position. , These are the true values of the aircraft's velocity and position. Loosely coupled integrated navigation uses velocity and position as observations. The velocity and position of the satellite navigation system are expressed as follows: （7-2）； in, , For the speed and position output by the satellite navigation system, , To calculate the corresponding errors in the output velocity and position of the satellite navigation system, the velocity and position measurement vectors are obtained based on equations (7-1) and (7-2). for: （7-3）； Suppose the Kalman filter measurement equation for the loosely coupled integrated navigation algorithm of the launch system inertial navigation satellite is: （7-4）； （7-5）； Therefore, we obtain The expression is: （7-6）。 8. A launch system inertial navigation satellite integrated navigation method based on ST-EKF according to claim 7, characterized in that, The discretized forms of the state equation and the measurement equation are as follows: （8-1）； （8-2）； in, yes The system state at any given moment. and This is a discretized representation of the state equation and the noise driving matrix. The system excitation noise sequence, To measure the noise sequence; （8-3）； （8-4）； The state at the next moment is estimated using the wave recursion algorithm. The state is predicted in one step as follows: （8-5）； The state is estimated as follows: （8-6）； in, It is called the filter gain matrix, which is the weight of the observation information during state update; （8-7）； in, The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates, representing the uncertainty of the estimate. （8-8）； in, This represents the prior error, i.e., the calculation error. , To estimate the mean square error matrix, and Represented as: （8-9）； （8-10）。 9. A launch system inertial navigation satellite integrated navigation method based on ST-EKF according to claim 8, characterized in that, The attitude angle error calculated using the navigation algorithm is: The speed is The location is ; The attitude was corrected to: （9-1）； in, The calculated attitude quaternion, The corrected attitude quaternion. For attitude angle error The corresponding quaternion; Speed correction: （9-2）； The position is corrected to: （9-3）。 10. A launch-based inertial navigation and satellite navigation integrated navigation system based on ST-EKF, characterized in that, include: The ST-EKF establishment module is used to establish the ST-EKF error model. The inertial navigation system establishment module is used to establish the inertial navigation model and inertial navigation error model in the launch system. The integrated navigation establishment module is used to establish an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the ST-EKF error model. The preparation module is used to start the inertial navigation system and complete preparations, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state. The time update module is used to update the time-varying values in the measurement error model, inertial navigation error model, and integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and to perform time updates. The measurement update module is used to update satellite navigation measurements when the satellite navigation signal is valid. The correction module is used to correct the inertial navigation system based on the error estimation results of the Kalman filter.