A transfer alignment method and device based on a Lie group left-invariant error model

Abstract

This invention discloses a transfer alignment method and device based on a left-invariant error model of Lie groups, belonging to the field of inertial navigation systems. The method first establishes a left-invariant error state model between the master and sub-inertial navigation systems based on Lie group theory; secondly, it constructs attitude and velocity error differential equations that simultaneously consider the effects of lever arm effects and installation error angles; then, it selects attitude and velocity measurements defined by the Lie group and establishes corresponding measurement equations; finally, it designs an unscented Kalman filter for the nonlinear state equations and measurement equations to jointly estimate parameters such as misalignment angle, velocity error, lever arm vector, installation error angle, and inertial navigation zero bias. The transfer alignment method proposed in this invention not only fully considers the actual lever arm errors between the master and sub-inertial navigation systems, improving the completeness of system error modeling, but also uses the local navigation coordinate system as the reference coordinate system, which is closer to actual engineering needs and has good versatility and engineering application prospects.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation systems, and in particular to a transfer alignment method and device based on a Lie group left-invariant error model. Background Technology

[0002] In the field of modern navigation, moving base alignment technology plays a crucial role in aviation, marine, automotive, and other fields. However, some key issues still exist in this technology, limiting its accuracy and reliability in complex environments.

[0003] Traditional transfer alignment modeling methods often neglect the impact of lever arm effects and installation errors on the system. In real-world operational scenarios, measurement errors caused by lever arm effects and measurement deviations due to installation errors can lead to significant discrepancies between the transfer alignment model and the actual system state, making it difficult to meet current demands for high-precision navigation. Meanwhile, the commonly used extended Kalman filter has significant limitations in nonlinear system estimation. Because it only performs first-order linearization on the nonlinear equations and relies on the degree of local nonlinearity, it is prone to filter divergence in strongly nonlinear scenarios, resulting in inaccurate state estimation.

[0004] Transfer alignment, a commonly used initial alignment technique for moving bases, works by using navigation information provided by a high-precision master inertial navigation system (INS) to help a low-precision sub-INS quickly and accurately complete initial alignment and device error calibration. Currently, existing transfer alignment methods primarily use measurement parameter matching and computational parameter matching methods, leveraging the real-time output information of both INS systems to estimate the modeled error state in real time. However, these methods have significant drawbacks: numerical accuracy is difficult to guarantee, and errors generated during calculation accumulate during state propagation. This results in the system state transition matrix typically including the sub-INS attitude matrix. In complex, highly maneuverable scenarios, the applicability of the navigation parameter model output by the low-precision sub-INS decreases, significantly reducing the reliability of transfer alignment, and ultimately failing to meet the actual positioning accuracy requirements of the sub-INS.

[0005] Lie group theory can unify attitude changes and translations within the same space, greatly improving the spatial inconsistency problem and laying the theoretical foundation for constructing a more scientific and reasonable transfer alignment error model. Based on the advantages of Lie group theory, transfer alignment technology largely decouples inertial navigation calculation parameters from the system model, opening up new directions for the development of transfer alignment technology. However, research in this field is still not mature enough: on the one hand, existing Lie group-based transfer alignment techniques generally do not consider the actual link arm errors between the master and sub-inertial navigation systems during system modeling, resulting in an insufficient consideration of the actual sources of system errors and thus requiring further improvement in estimation accuracy; on the other hand, due to the group affine property of Lie groups, existing methods mostly use the Earth coordinate system as the reference coordinate system, while commonly used navigation parameters usually refer to the local navigation coordinate system. The method of updating the state using the Earth coordinate system is only suitable for polar navigation, limiting the value of existing Lie group transfer alignment methods in practical applications.

[0006] Given the numerous problems with existing transfer alignment techniques, it is urgent to develop a Lie group transfer alignment method that comprehensively considers the sources of systematic errors, possesses high alignment accuracy, and can be directly applied to practical scenarios. Summary of the Invention

[0007] The purpose of this invention is to propose a transfer alignment method based on a right-invariant error model of Lie groups to solve the problems mentioned in the background. Based on the left-group error theory under Lie groups, a left-invariant error state model is established between the master and sub-inertial navigation systems. Based on the exponential mapping relationship between attitude error and misalignment angle in the left-invariant error state, an attitude error differential equation is constructed. An auxiliary velocity vector is constructed, and a velocity error differential equation considering the lever arm and installation error angle is established. Attitude and velocity measurements under Lie groups are selected, and measurement equations are established. For the nonlinear measurement equations, UKF is used for measurement updates.

[0008] This invention provides a transfer alignment method based on a Lie group left-invariant error model, comprising:

[0009] Step 1: Define the coordinate system;

[0010] Step 2: Based on the left-invariant error theory under Lie groups, an auxiliary velocity vector is introduced to establish an error state model between the master and sub-inertial navigation systems;

[0011] Step 3: Based on the exponential mapping relationship between attitude error and misalignment angle in the left-invariant error state, construct the differential equation of attitude error of the sub-inertial navigation system;

[0012] Step 4: Considering the lever arm error caused by the misalignment of the main and sub-inertial navigation systems, construct the velocity error differential equation of the sub-inertial navigation system;

[0013] Step 5: Based on the relationship between the attitude matrices obtained from the main and sub-inertial navigation systems, construct the attitude measurement equations of the sub-inertial navigation system that take into account the installation error angle;

[0014] Step 6: Based on the relationship between the auxiliary velocities of the main and sub-inertial navigation systems, construct the velocity measurement equation for the sub-inertial navigation system;

[0015] Step 7: First, construct the system's error state vector based on attitude error, velocity error, gyroscope bias, accelerometer bias, lever arm error, and mounting angle error. Simultaneously, construct the system noise vector based on the measurement errors of the sub-inertial navigation gyroscope and accelerometer. Next, based on the differential equations of each error state, and combining the system state vector and noise vector, construct the state differential equations of the sub-inertial navigation system. Further, based on the attitude and velocity measurement equations of the sub-inertial navigation system, construct the error state space equations of the sub-inertial navigation system. Finally, organize and discretize the system to obtain a complete discrete-time error state space model.

[0016] Step 8: For the discrete-time error state-space model, the unscented Kalman filter algorithm is used to accurately estimate the error state, thereby completing the master and sub-inertial navigation transfer alignment.

[0017] Furthermore, in step 1, the defined coordinate system is as follows:

[0018] i-frame: Geocentric inertial coordinate system, or simply inertial coordinate system;

[0019] e-system: Earth-centered, Earth-fixed coordinate system, or simply Earth coordinate system;

[0020] n-system: The "East-North-Sky" geographic coordinate system is used as the navigation coordinate system; the navigation coordinate system is the reference coordinate system used by the inertial navigation system when solving for navigation parameters;

[0021] m-frame: The main inertial navigation coordinate system, with the geometric center of the main inertial navigation as the origin, and the three axes x, y, and z pointing to the right, front, and top directions of the carrier, respectively;

[0022] S-frame: Sub-inertial navigation coordinate system, with the geometric center of the sub-inertial navigation as the origin, and the three axes x, y, and z pointing to the right, front, and top directions of the carrier, respectively.

[0023] Furthermore, in step 2, establishing the right-invariant error state model between the master and sub-inertial navigation systems specifically involves:

[0024] Step 2.1: Construct the auxiliary velocity vector

[0025]

[0026] Wherein, the symbol (·×) is This is the projection of the Earth's rotational angular velocity onto the navigation coordinate system. The projection of the velocity of the sub-inertial navigation system relative to the Earth coordinate system onto the navigation coordinate system; The projection of the sub-inertial navigation system's position relative to the Earth coordinate system onto the navigation coordinate system;

[0027] Step 2.2: The state χ∈SE(3) in the Lie group space is:

[0028]

[0029] in, This is the rotation matrix from the navigation coordinate system to the sub-inertial navigation coordinate system, which can be used to represent the sub-inertial navigation attitude;

[0030] Step 2.3: Based on the calculation status The deviation from the true state χ is defined by the left-invariant error as:

[0031]

[0032] in, The state of the Lie group contains errors; To calculate the rotation matrix from the sub-inertial navigation coordinate system to the navigation coordinate system; For auxiliary velocity vector; This is an estimate of the auxiliary velocity vector; The attitude error is the left-invariant error. The velocity error is a left-invariant error.

[0033] Furthermore, in step 3, the differential equation for the attitude error of the sub-inertial navigation system is specifically as follows:

[0034] Step 3.1: Based on the relationship between the equivalent rotation vector and the rotation matrix, the attitude error is defined as:

[0035]

[0036] Where φ is the calculated equivalent rotation vector from the sub-inertial navigation coordinate system s′ to the sub-inertial navigation coordinate system s, i.e., the misalignment angle error; I 3×3 It is a 3x3 identity matrix;

[0037] Step 3.2: Attitude differential equations of the sub-inertial navigation system:

[0038]

[0039] in, For the gyroscope data of the sub-inertial navigation system, This is the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system onto the actual navigation frame.

[0040] Step 3.3: The differential equation for the attitude error of the sub-inertial navigation system is:

[0041]

[0042] in, These are sampled values ​​of the sub-inertial gyroscope data; This refers to the error amount of the gyroscope data from the inertial navigation system.

[0043] Furthermore, in step 4, the differential equation for the velocity error of the sub-inertial navigation system is:

[0044] Step 4.1: Calculate the speed error:

[0045]

[0046] in, To assist in speed error;

[0047] Step 4.2: Considering the influence of the lever arms between the master and sub-inertial navigation systems, the positions of the master and sub-inertial navigation systems can be modeled as follows:

[0048]

[0049] in, The projection of the lever arm between the main inertial navigation system and the sub-inertial navigation system in the coordinate system of the main inertial navigation system, i.e., the lever arm; The projection of the position of the sub-inertial navigation coordinate system relative to the Earth coordinate system onto the Earth coordinate system; The projection of the position of the main inertial navigation coordinate system relative to the Earth coordinate system onto the Earth coordinate system; The projection of the position of the sub-inertial navigation coordinate system relative to the main inertial navigation coordinate system onto the Earth coordinate system; This is the rotation matrix from the Earth coordinate system to the principal inertial navigation coordinate system;

[0050] Step 4.3: Based on the auxiliary velocity vector and the main and sub-inertial navigation positions of the linkage arm, the relationship between the auxiliary velocities of the main and sub-inertial navigation systems and the linkage arm is obtained as follows:

[0051]

[0052] in, This is the rotation matrix from the navigation coordinate system to the master inertial navigation coordinate system. The projection of the angular velocity of the main inertial navigation coordinate system relative to the inertial coordinate system onto the main inertial navigation coordinate system;

[0053] Step 4.4: The differential equation for the auxiliary velocity is:

[0054]

[0055] Among them, G n g is the projection of gravitational acceleration in the real navigation frame;n This is the projection of gravitational acceleration onto the actual navigation frame. Centripetal force; The projection of the position of the main inertial navigation coordinate system relative to the Earth coordinate system onto the actual navigation coordinate system;

[0056] Step 4.5: The differential equation for the velocity error of the sub-inertial navigation system is calculated as follows:

[0057]

[0058] in, φ represents the sampled value of the sub-inertial navigation gyroscope data; φ is the misalignment angle of the sub-inertial navigation system. These are sampled values ​​from the sub-inertial accelerometer data; This refers to the error quantity of the sub-inertial accelerometer data.

[0059] Furthermore, in step 5, the attitude matrix of the main inertial navigation system obtained during navigation system operation is used... attitude matrix of sub-inertial navigation The equations for calculating the attitude measurement of the sub-inertial navigation system are as follows:

[0060] z obφ =eul(Z obφ )=g(φ,μ)(13)

[0061]

[0062] Here, eul(·) represents the Euler angle corresponding to the direction cosine matrix, and g(φ,μ) is a nonlinear function of the misalignment angle φ and the installation error angle μ of the sub-inertial navigation system.

[0063] Furthermore, in step 6, the auxiliary velocity of the main inertial navigation system is calculated based on the operation of the navigation system. auxiliary velocity estimates of the inertial navigation system The equation for calculating the velocity measurement of the sub-inertial navigation system is as follows:

[0064]

[0065] in, This represents the estimated value of the rotation matrix from the navigation coordinate system to the sub-inertial navigation coordinate system; This represents the rotation matrix from the navigation coordinate system to the master inertial navigation coordinate system; The value is an estimated value of the lever arm vector at time k-1.

[0066] Furthermore, in step 7, specifically:

[0067] Step 7.1: Construct the state vector of the sub-inertial navigation system;

[0068] Step 7.1.1: Construct the lever arm vector and installation error angle using constant value modeling;

[0069]

[0070] Step 7.1.2: Set the state variables ε, Extending μ to the left-invariant error state, the 18-dimensional state variable x of the filtering model is:

[0071]

[0072] Where φ is the misalignment angle of the sub-inertial navigation system, dv L Here, ε represents the velocity error, and ε represents the gyroscope's zero bias. To achieve zero bias in the accelerometer, The lever arm between the master and sub-inertial navigation systems, μ is the installation error angle between the master and sub-inertial navigation systems;

[0073] Step 7.2: Construct the noise vector of the sub-inertial navigation system;

[0074] Step 7.2.1: Construct the measurement error of the sub-inertial gyroscope in Equation (6) using constant zero bias and white noise modeling. Measurement error of neutron inertial accelerometer in equation (12)

[0075]

[0076] Among them, w g and w a Zero-mean Gaussian white noise; for gyroscope zero bias ε and accelerometer zero bias ε Modeled as constant with zero bias,

[0077] Step 7.2.2: The 6-dimensional state noise w of the filtered model is:

[0078] w = [w g w a ] T (19)

[0079] Step 7.3: Construct the state-space equations of the sub-inertial navigation system;

[0080] Step 7.3.1: Based on the attitude error differential equation (6) and the velocity error differential equation (12), determine the transfer matrix F and the noise matrix G as follows:

[0081]

[0082] Step 7.3.2: Based on the attitude measurement equation (13), determine the attitude measurement generation function h(x) as follows:

[0083] h(x)=g(φ,μ)(22)

[0084] Step 7.3.3: Based on the velocity measurement equation (15), determine the velocity measurement matrix H as follows:

[0085]

[0086] Step 7.3.4: Combining the attitude measurement function h(x) from equation (22) and the velocity measurement matrix H from equation (23), the state-space equation of the sub-inertial navigation system is obtained:

[0087]

[0088] Among them, v obφ For attitude measurement noise, v obv For speed measurement noise;

[0089] Step 7.4: Discretize the state-space equations to obtain:

[0090]

[0091] Where Δt is the discrete time; Φ k,k-1 Γ is the discrete transition matrix; k-1 The discrete noise matrix; v obφ,k For attitude discrete measurement noise, v obv,k This is noise in discrete velocity measurements.

[0092] Furthermore, in step 8, state estimation is achieved using an unscented Kalman filter, specifically as follows:

[0093] Step 8.1: Construct 2n+1 σ points for time updates:

[0094]

[0095] Step 8.2: The weights of each σ point are calculated as follows:

[0096]

[0097] Where, ω (m) ω is the weight used when calculating the mean. (c) The weights are used to calculate the covariance; the parameter β is used to introduce prior information about the non-Gaussian distribution, and the parameter λ = α. 2 (n+K)-n is the proportionality coefficient, λ∈[0,1], K=0, n=18, and the distribution of σ points near the mean can be determined by parameters α and K;

[0098] Step 8.3: Incorporate point σ into state propagation:

[0099]

[0100] in, For the predicted Sigma point;

[0101] Step 8.4: Calculate the predicted mean based on steps 8.1 to 8.3. and the predicted mean square error P k|k-1 ;

[0102]

[0103] Step 8.5: For the attitude measurement equation and velocity measurement equation, a sequential update method is used to update the measurements, then the measurement z... k =z obφ,k or z k =z obv,k ;

[0104] Reconstruct 2n+1 σ points and update the measurements:

[0105]

[0106] Step 8.6: Substituting the newly constructed σ point into the measurement equations of the sub-inertial navigation system, we get:

[0107] z i,k|k-1 =h(χ i,k|k-1 )i=0,1,2…,2n(32)

[0108] Among them, z i,k|k-1 For the predicted measurement σ point;

[0109] Step 8.7: Calculate the predicted mean. and predicted covariance and the cross-covariance between state and measurement

[0110]

[0111] Step 8.8: Calculate the filter gain K k Combined measurement z k Obtain the filtered state estimate and variance P k|k :

[0112]

[0113] The present invention also provides a computer device / equipment / system, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the transfer alignment method based on the left-invariant error model of the Lie group described above.

[0114] The present invention also provides a computer-readable storage medium having a computer program / instructions stored thereon, which, when executed by a processor, implements the steps of the transfer alignment method based on the left-invariant error model of the Lie group described above.

[0115] The present invention also provides a computer program product, including a computer program / instruction that, when executed by a processor, implements the steps of the transfer alignment method based on the left-invariant error model of the Lie group as described above.

[0116] The beneficial effects of this invention are as follows:

[0117] The proposed transfer alignment method based on the Lie group left-invariant error model can decouple the parameters in the system equations from the inertial navigation system (INS) solution results, reducing the cumulative error caused by the INS solution. At the same time, by adopting a modeling method based on the lever arm and installation error angle, it fully considers the installation relationship between the main and sub-INS, further improving the accuracy of transfer alignment and providing high-precision initial navigation values ​​for the sub-INS. Furthermore, considering the nonlinearity of the measurement equations, it uses UKF for measurement updates, significantly improving the performance of transfer alignment and showing broad prospects for engineering applications. Attached Figure Description

[0118] Figure 1 A flowchart of a transfer alignment method for an inertial navigation system based on a Lie group model is provided for an embodiment of the present invention.

[0119] Figure 2 This is a schematic diagram of the coordinate systems and placement of two inertial navigation systems provided in an embodiment of the present invention;

[0120] Figure 3 The state estimation algorithm flow provided in the embodiments of the present invention;

[0121] Figure 4 This is an alignment result diagram showing the installation error angle transfer of the main and sub-inertial navigation systems provided in an embodiment of the present invention.

[0122] Figure 5 This diagram shows the alignment result of the master and slave inertial navigation system arms in an embodiment of the present invention. Detailed Implementation

[0123] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.

[0124] This invention discloses a transfer alignment method based on a Lie group left-invariant error model, such as... Figure 1As shown, firstly, a left-invariant error model based on a Lie group is constructed; secondly, each navigation computer performs inertial navigation calculations based on sensor data; nextly, considering the error sources in the transfer alignment in the navigation coordinate system, differential equations between states are constructed to establish a transfer alignment model based on a Lie group description; finally, an unscented Kalman filter algorithm is used to accurately estimate the modeled states, completing the transfer alignment, including:

[0125] Step 1: Define the coordinate system. The coordinate system of this invention is defined as follows:

[0126] Geocentric inertial coordinate system (i-frame): The origin of the geocentric inertial coordinate system is the center of the Earth. The x-axis and y-axis lie in the Earth's equatorial plane, with the x-axis pointing to the vernal equinox (one of the intersections of the equatorial plane and the ecliptic plane with the celestial sphere). The z-axis is the Earth's rotation axis and points to the North Pole. The output of the inertial sensor is based on this coordinate system.

[0127] Geocentric-fixed coordinate system (e-frame): The geocentric-fixed coordinate system, also known as the Earth coordinate system, is simply called the e-frame. The Earth coordinate system is fixed to the Earth, with its origin at the Earth's center of mass. The x-axis and y-axis lie in the equatorial plane, with the x-axis pointing towards the Prime Meridian. The z-axis coincides with the Earth's rotation axis and points towards the North Pole. The x, y, and z axes form a right-handed rectangular coordinate system. The angular motion of the e-frame relative to the i-frame is equal to the Earth's rotational angular rate ω. ie .

[0128] Navigation coordinate system (n-system): The navigation coordinate system is simply referred to as the n-system. The navigation coordinate system is the reference coordinate system used by the inertial navigation system when solving for navigation parameters; any coordinate system can be used as the navigation coordinate system. This invention selects the "Northeast Sky (ENU)" geographic coordinate system as the navigation coordinate system.

[0129] The master inertial navigation system (m-frame) coordinate system, abbreviated as m-frame, is a calibrated three-axis orthogonal coordinate system. Its origin is at the geometric center of the master inertial navigation system, and it satisfies the "right-front-up" three-axis definition of the carrier coordinate system along the inertial navigation system's outer shell. The gyroscope data of the master inertial navigation system is denoted as... Accelerometer data is recorded as

[0130] Sub-inertial navigation coordinate system (S-frame): The sub-inertial navigation coordinate system, abbreviated as S-frame, is a calibrated three-axis orthogonal coordinate system. Its origin is at the geometric center of the sub-inertial navigation system, and it satisfies the "right-front-up" three-axis definition of the carrier coordinate system along the inertial navigation shell. The gyroscope data of the sub-inertial navigation system is denoted as... Accelerometer data is recorded as

[0131] Step 2: Based on Lie group theory, establish a state-space model of the left-invariant error. The Lie group state χ∈SE(3) can be expressed as:

[0132]

[0133] in, This represents the rotation matrix from the navigation coordinate system to the sub-inertial navigation coordinate system. The auxiliary velocity vector is represented by Equation 10. Similarly, the inverse of the Lie group state can be expressed as:

[0134]

[0135] Based on the calculation status The deviation from the true state χ is defined by the left-invariant error as:

[0136]

[0137] in, To calculate the rotation matrix from the sub-inertial navigation coordinate system to the navigation coordinate system; This is an estimate of the auxiliary speed; The attitude error is the left-invariant error. The velocity error is a left-invariant error.

[0138] Step 3: Construct the attitude error differential equation. First, based on the relationship between the equivalent rotation vector and the rotation matrix, the attitude error is defined as:

[0139]

[0140] Where φ is the calculated equivalent rotation vector from the sub-inertial navigation coordinate system s′ to the sub-inertial navigation coordinate system s, i.e., the misalignment angle error; I 3×3 It is a 3x3 identity matrix;

[0141] Taking the derivative of both sides of the equation with respect to time, we have:

[0142]

[0143] According to the attitude differential equation The above formula can be further simplified to:

[0144]

[0145] in, For the measurement error of the inertial gyroscope, neglecting second-order small quantities, we have:

[0146]

[0147] because It depends only on the position of the n-system, ignoring the differences caused by the different positions of the master inertial navigation system. Taking antisymmetry on both sides simultaneously yields the differential equation for attitude error:

[0148]

[0149] Step 4: Construct the velocity error equation, based on Step 2, let Differentiating both sides of the equation, we have:

[0150]

[0151] Among them, the auxiliary velocity vector The construction method is as follows:

[0152]

[0153] Considering the influence of the lever arms between the master and sub-inertial navigation systems, the positions of the master and sub-inertial navigation systems can be modeled as follows:

[0154]

[0155] Differentiating both sides of equation (11) with respect to time, we have:

[0156]

[0157] Substituting the auxiliary velocity vector represented by equation (10) into equation (12), we can eliminate... and The relationship between the auxiliary velocities of the main and sub-inertial navigation systems can be obtained:

[0158]

[0159] Differentiate both sides of the above equation with respect to time and substitute them into the equation. There is:

[0160]

[0161] Equation (14) combined with the auxiliary velocity vector differential equation and the expression for gravitational acceleration It can be expanded as follows:

[0162]

[0163] Substituting the above equation into equation (9) and rearranging, we get:

[0164]

[0165] in It can be transformed into:

[0166]

[0167] Where μ is the equivalent rotation vector from the m-system to the s-system, substituting equation (17) into equation (16), and reducing the small amount Ignoring this, we can conclude that:

[0168]

[0169] Considering The angular velocity of Earth's rotation, squared to the order of magnitude 10. -10 The minute amount can be ignored in the formula, and simplification yields:

[0170]

[0171] Step 5: Construct attitude measurement equations. The attitude information that the navigation system can acquire during operation includes the attitude matrix of the main inertial navigation system. attitude matrix of sub-inertial navigation The result of multiplying the two can be used as an attitude measurement; therefore, the attitude measurement under the Lie group is selected as follows:

[0172]

[0173] but:

[0174] z obφ =eul(Z obφ )=g(φ,μ)(21)

[0175] Here, eul(·) represents the Euler angle corresponding to the direction cosine matrix, and g(φ,μ) is a nonlinear function of the misalignment angle φ and the installation error angle μ of the sub-inertial navigation system.

[0176] Step 6: Construct the velocity measurement equation. The velocity information that the navigation system can obtain during operation includes the auxiliary velocity from the main inertial navigation system. The speed of the inertial navigation system The result of subtracting the two can be used as a speed measurement, therefore the speed measurement is selected as:

[0177]

[0178] in, This represents the estimated value of the rotation matrix from the sub-inertial navigation coordinate system to the navigation coordinate system; This represents the rotation matrix from the main inertial navigation coordinate system to the sub-inertial navigation coordinate system; The value is an estimated value of the lever arm vector at time k-1.

[0179] Step 7: Select state variables and construct state-space equations. For the measurement error of the sub-inertial gyroscope in equation (8) Measurement error of neutron inertial accelerometer in equation (19) We can model this using constant zero bias and white noise, as shown in the following formula:

[0180]

[0181] Where w g and w a Zero-mean Gaussian white noise is used for the gyroscope's zero bias ε and the accelerometer's zero bias ε. Modeling it as constant and zero bias can be expressed as:

[0182]

[0183] The lever arm and installation error angle are modeled using constant values:

[0184]

[0185] ε, ▽, Extending μ to the left-invariant error state, the left-invariant state x of the filtering model can be defined as follows:

[0186]

[0187] Where φ is the misalignment angle of the sub-inertial navigation system, dv L Here, ε represents the velocity error, and ε represents the gyroscope's zero bias. To achieve zero bias in the accelerometer, The lever arm between the master and sub-inertial navigation systems, μ is the installation error angle between the master and sub-inertial navigation systems.

[0188] Noise is defined as:

[0189] w = [w g w a ] T (27)

[0190] Based on the definitions of state and noise, and combining equations (8) and (19), the state transition matrix F can be determined as follows:

[0191]

[0192] The noise-driven array G can be determined as follows:

[0193]

[0194] Based on attitude measurement equation (21), the attitude measurement generating function h(x) is determined as follows:

[0195] h(x)=g(φ,μ)(30)

[0196] Based on the velocity measurement equation (22), the velocity measurement matrix H is determined as follows:

[0197]

[0198] Combining the attitude measurement function h(x) from equation (30) and the velocity measurement matrix H from equation (31), and finally combining the noise level of the actual system, the state-space equation of the system is obtained:

[0199]

[0200] Among them, v obφ For attitude measurement noise, v obv For speed measurement noise;

[0201] Step 7.4: Discretize the state-space equations to obtain:

[0202]

[0203] Where Δt is the discrete time; Φ k,k-1 Γ is the discrete transition matrix; k-1 The discrete noise matrix; v obφ,k For attitude discrete measurement noise, v obv,k This is noise in discrete velocity measurements.

[0204] Step 8: Based on the state-space model described above, since the measurement model involves nonlinear transformations, using UKF can more accurately capture the changes in the mean and variance of the state distribution, providing more precise estimates even in nonlinear systems. First, construct the σ point for time updates:

[0205]

[0206] The weights of each σ point are calculated as follows:

[0207]

[0208] Where, ω (m) ω is the weight used when calculating the mean. (c) The weights are used to calculate the covariance; the parameter β is used to introduce prior information about the non-Gaussian distribution, and the parameter λ = α. 2 (n+K)-n is the proportionality coefficient, λ∈[0,1], K=0, n=18, and the distribution of σ points near the mean can be determined by parameters α and K;

[0209] Next, we introduce point σ into the state propagation:

[0210]

[0211] Calculate the predicted mean and prediction P k|k-1 :

[0212]

[0213] Then, a new σ point is constructed and the measurement is updated:

[0214]

[0215] Substituting the new σ point into the measurement equation, we get:

[0216] z i,k|k-1 =h(χ i,k|k-1 )i=0,1,2…,2n(40)

[0217] Computational quantity measurement prediction mean and predicted covariance and the cross-covariance between state and measurement

[0218]

[0219] Calculate the filter gain K k Combined measurement z k Obtain the filtered state estimate and variance P k|k :

[0220]

[0221] Example 1

[0222] A transfer alignment method based on a Lie group left-invariant error model includes:

[0223] Step 1: Define the coordinate system.

[0224] Step 2: In this embodiment, the placement of the main and sub-inertial navigation systems is as follows: Figure 2 As shown. The initial geographical location is 126.6877°E, 45.7817°N; the sensor output frequency is 100Hz; the gyroscope drift is a constant bias of 10° / h, with random walk. Accelerometer drift: constant bias of 1 mg, random walk The installation error angle between the primary and secondary inertial navigation systems is set to [1°1°1°]; the lever arm between the primary and secondary inertial navigation systems is set to [10m 10m 10m]. The simulation trajectory settings in this embodiment are as follows:

[0225]

[0226] Where θ is the pitch angle, γ is the roll angle, and θ is the sway amplitude. m γ m Both are 15°, with an initial angle θ. I γ I All are 0°, shaking period T θ T γAll times are 5s, the discrete time T is 0.01s, and the entire simulation lasts for 60s.

[0227] Step 3: Based on Lie group theory, establish the state definition based on left-invariant error:

[0228] The state definition of a Lie group is: According to Lie group theory, the left-invariant error state is defined as follows: This represents the rotation matrix from the navigation coordinate system to the sub-inertial navigation coordinate system. To calculate the rotation matrix from the sub-inertial navigation coordinate system to the navigation coordinate system, The auxiliary velocity vector is represented by the symbol (·×). This is an estimate of the auxiliary velocity vector.

[0229] Step 4: Construct the attitude differential equation:

[0230]

[0231] Where φ is the misalignment angle of the sub-inertial navigation system. These are sampled values ​​of the sub-inertial gyroscope data. This refers to the error amount of the gyroscope data from the inertial navigation system.

[0232] Step 5: Construct the velocity differential equation:

[0233]

[0234] in, The velocity error of the sub-inertial navigation system. Here, φ represents the sampled value of the sub-inertial navigation gyroscope data, and φ is the misalignment angle of the sub-inertial navigation system. These are sampled values ​​from the sub-inertial accelerometer data; This refers to the error quantity of the sub-inertial accelerometer data.

[0235] Step 6: Construct attitude measurement equations. The attitude information that the navigation system can acquire during operation includes the attitude matrix of the main inertial navigation system. attitude matrix of sub-inertial navigation The result of multiplying the two can be used as an attitude measurement; therefore, the attitude measurement under the Lie group is selected as follows:

[0236] Z obφ =[exp(φ×)][exp(-μ×)](46)

[0237] but:

[0238] z obφ =eul(Z obφ )=g(φ,μ)(47)

[0239] Here, eul(·) represents the Euler angle corresponding to the direction cosine matrix, and g(φ,μ) is a nonlinear function of the platform misalignment angle φ and the installation error angle μ.

[0240] Step 7: Construct the velocity measurement equation:

[0241]

[0242] in, This represents the estimated value of the rotation matrix from the navigation coordinate system to the sub-inertial navigation coordinate system; This represents the rotation matrix from the navigation coordinate system to the master inertial navigation coordinate system; The value is an estimated value of the lever arm vector at time k-1.

[0243] Step 8: Construct the state-space equations under the definition of right-invariant error.

[0244] The 18-dimensional state variables are selected as follows:

[0245]

[0246] Where φ is the misalignment angle of the sub-inertial navigation system, dv L Here, ε represents the velocity error, and ε represents the gyroscope's zero bias. To achieve zero bias in the accelerometer, The lever arm between the master and sub-inertial navigation systems, μ is the installation error angle between the master and sub-inertial navigation systems.

[0247] The 6-dimensional state noise is selected as follows:

[0248] w = [w g w a ] T (50)

[0249] Among them, w g w represents the noise level of the gyroscope. a This indicates the noise level of the accelerometer.

[0250] Based on the definitions of state and noise, and combined with the state propagation equation, the state transition matrix F and the noise driving matrix G can be determined as follows:

[0251]

[0252] Secondly, based on the attitude measurement equation in step 6 and the velocity measurement equation in step 7, the measurement generation function h(x) can be obtained. Finally, the measurement noise v is matched with the actual system. k After simplification, the state-space equations are obtained as follows:

[0253]

[0254] Among them, vobφ For attitude measurement noise, v obv For speed measurement noise;

[0255] Step 9: Perform state estimation using UKF. The algorithm flow used in this embodiment is as follows: Figure 3 As shown, the specific steps are as follows:

[0256] First, construct point σ for time updating:

[0257]

[0258] The weights of each σ point are calculated as follows:

[0259]

[0260] Where λ=α 2 (n+K)-n, λ∈[0,1], K=0, n=18, α=0.01, β=2. Next, the σ point is introduced into the state propagation:

[0261]

[0262] Calculate the predicted mean and prediction P k|k-1 :

[0263]

[0264] Then, a new σ point is constructed and the measurement is updated:

[0265]

[0266] Substituting the new σ point into the measurement equation, we get:

[0267] z i,k|k-1 =h(χ i,k|k-1 )i=0,1,2…,2n(59)

[0268] Calculate the measurement prediction mean and covariance matrix:

[0269]

[0270] Finally, the Kalman gain is calculated and the measurements are updated:

[0271]

[0272] Step 10: Output the simulation results of Monte Carlo simulation 20 times:

[0273] Using the root mean square error (RMSE) as a metric, the simulation results are as follows: Figure 4 , Figure 5As shown in the figure, the estimated results of the installation error angle and the lever arm stabilize and converge around 10s, and the mean maximum RMSE values ​​of the two states in the last 5s on the X, Y, and Z axes are 0.00896° and 0.0844m, respectively. The simulation results demonstrate that this invention can quickly and effectively complete the transfer alignment of the main and sub-inertial navigation systems even with only two-axis motion.

[0274] In summary, the method of this invention first establishes a left-invariant error state model between the master and sub-inertial navigation systems based on Lie group theory. Second, it constructs attitude and velocity error differential equations that simultaneously consider the effects of lever arm effects and installation error angles. Leveraging the structural advantages of Lie group theory in attitude representation, error propagation is defined in Lie algebra space, thus effectively decoupling the inertial navigation calculation parameters from the system modeling. Then, attitude and velocity measurements defined by Lie groups are selected, and corresponding measurement equations are established. Finally, an unscented Kalman filter is designed for the nonlinear state equations and measurement equations to jointly estimate parameters such as misalignment angle, velocity error, lever arm vector, installation error angle, and inertial navigation zero bias. The transfer alignment method proposed in this invention not only fully considers the actual lever arm errors between the master and sub-inertial navigation systems, improving the completeness of system error modeling, but also uses the local navigation coordinate system as a reference coordinate system, which is closer to actual engineering needs and has good versatility and engineering application prospects.

[0275] In particular, in some preferred embodiments of the present invention, a computer device is also provided, including a memory and a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the transfer alignment method based on the Lie group left-invariant error model described in any of the above embodiments.

[0276] In some other preferred embodiments of the present invention, a computer-readable storage medium is also provided, on which a computer program / instruction is stored, wherein when the computer program is executed by a processor, the steps of the transfer alignment method based on the Lie group left-invariant error model described in any of the above embodiments are implemented.

[0277] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above embodiments of the transfer alignment method based on the Lie group left-invariant error model, which will not be repeated here.

[0278] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0279] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "N" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0280] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more N executable instructions for implementing custom logic functions or processes, and the scope of preferred embodiments of the invention includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of the invention pertain.

[0281] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0282] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0283] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0284] Furthermore, the functional units in the various embodiments of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0285] The storage medium mentioned above can be a read-only memory, a disk, or an optical disk, etc. Although embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. A transfer alignment method based on a Lie group left-invariant error model, characterized in that, include: Step 1: Define the coordinate system; Step 2: Based on the left-invariant error theory under Lie groups, an auxiliary velocity vector is introduced to establish an error state model between the master and sub-inertial navigation systems; Step 3: Based on the exponential mapping relationship between attitude error and misalignment angle in the left-invariant error state, construct the differential equation of attitude error of the sub-inertial navigation system; Step 4: Considering the lever arm error caused by the misalignment of the main and sub-inertial navigation systems, construct the velocity error differential equation of the sub-inertial navigation system; Step 4.1: Based on the auxiliary velocity vector and the main and sub-inertial navigation positions of the linkage, the relationship between the auxiliary velocities of the main and sub-inertial navigation systems and the linkage is obtained as follows: （1） in, For auxiliary velocity vector; The auxiliary velocity of the main inertial navigation system; This is the rotation matrix from the navigation coordinate system to the master inertial navigation coordinate system. The projection of the angular velocity of the main inertial navigation coordinate system relative to the inertial coordinate system onto the main inertial navigation coordinate system; The projection of the lever arm between the main inertial navigation system and the sub-inertial navigation system in the coordinate system of the main inertial navigation system; Step 4.2: The differential equation for the auxiliary velocity is: （2） （3） in, This is the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system onto the navigation frame. This is the projection of gravitational acceleration onto the real navigation frame; This is the rotation matrix from the navigation coordinate system to the sub-inertial navigation coordinate system; This is the projection of gravitational acceleration onto the actual navigation frame. The projection of the position of the main inertial navigation coordinate system relative to the Earth coordinate system onto the actual navigation coordinate system; Step 4.3: The differential equation for the velocity error of the sub-inertial navigation system is calculated as follows: （4） in, These are sampled values ​​of the sub-inertial gyroscope data; The misalignment angle of the sub-inertial navigation system; These are sampled values ​​from the sub-inertial accelerometer data; The error amount of the sub-inertial accelerometer data; For speed error; Step 5: Based on the relationship between the attitude matrices obtained from the main and sub-inertial navigation systems, construct the attitude measurement equations of the sub-inertial navigation system that take into account the installation error angle; Step 6: Based on the relationship between the auxiliary velocities of the main and sub-inertial navigation systems, construct the velocity measurement equation for the sub-inertial navigation system; Step 7: First, construct the system's error state vector based on attitude error, velocity error, gyroscope bias, accelerometer bias, lever arm error, and installation angle error. Simultaneously, construct the system noise vector based on the measurement errors of the sub-inertial navigation gyroscope and accelerometer. Next, based on the differential equations of each error state, and combining the system state vector and noise vector, construct the state differential equations of the sub-inertial navigation system. Based on the attitude and velocity measurement equations of the sub-inertial navigation system, construct the error state space equations of the sub-inertial navigation system. Finally, organize and discretize the system to obtain a complete discrete-time error state space model. Step 8: For the discrete-time error state-space model, the unscented Kalman filter algorithm is used to accurately estimate the error state, thereby completing the master and sub-inertial navigation transfer alignment.

2. The transfer alignment method based on the left-invariant error model of a Lie group according to claim 1, characterized in that, In step 1, the coordinate system is defined as follows: System: Geocentric inertial coordinate system, or simply inertial coordinate system; System: Earth-centered, Earth-fixed coordinate system, or simply Earth coordinate system; The "East-North-Sky" geographic coordinate system is used as the navigation coordinate system; the navigation coordinate system is the reference coordinate system used by the inertial navigation system when solving for navigation parameters. System: The main inertial navigation coordinate system, with the geometric center of the main inertial navigation as the origin, and the three axes x, y, and z pointing to the right, front, and top directions of the carrier, respectively; System: Sub-inertial navigation coordinate system, with the geometric center of the sub-inertial navigation as the origin, and the three axes x, y, and z pointing to the right, front, and top directions of the carrier, respectively.

3. The transfer alignment method based on the left-invariant error model of a Lie group according to claim 2, characterized in that, In step 2, establishing the left-invariant error state model between the master and sub-inertial navigation systems specifically involves: Step 2.1: Construct the auxiliary velocity vector : （5） Among them, symbols for ; This is the projection of the Earth's rotational angular velocity onto the navigation coordinate system. The projection of the velocity of the sub-inertial navigation system relative to the Earth coordinate system onto the navigation coordinate system; The projection of the sub-inertial navigation system's position relative to the Earth coordinate system onto the navigation coordinate system; Step 2.2: State in Lie group space for: （6） in, This is the rotation matrix from the navigation coordinate system to the sub-inertial navigation coordinate system, i.e., the sub-inertial navigation attitude; Step 2.3: Based on the calculation status Compared to the actual state The deviation between them is defined as follows: The left-invariant error is defined as: （7） in, To calculate the rotation matrix from the sub-inertial navigation coordinate system to the navigation coordinate system; This is an estimate of the auxiliary speed; The attitude error is the left-invariant error. The velocity error is a left-invariant error.

4. The transfer alignment method based on the left-invariant error model of a Lie group according to claim 3, characterized in that, In step 3, the attitude error differential equation of the sub-inertial navigation system is specifically as follows: Step 3.1: Based on the relationship between the equivalent rotation vector and the rotation matrix, the attitude error is defined as: （8） in, For the calculated sub-inertial navigation coordinate system to the sub-inertial navigation coordinate system The equivalent rotation vector of the system, i.e., the misalignment angle error; It is a 3x3 identity matrix; Step 3.2: Attitude differential equations of the sub-inertial navigation system: （9） in, For the gyroscope data of the sub-inertial navigation system, This is the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system onto the navigation frame. Step 3.3: The differential equation for the attitude error of the sub-inertial navigation system is: （10） in, These are sampled values ​​of the sub-inertial gyroscope data; This refers to the error amount of the gyroscope data from the inertial navigation system.

5. The transfer alignment method based on the left-invariant error model of a Lie group according to claim 4, characterized in that, In step 4, the speed error is: （11） in, To assist in speed error; The influence of the lever arms between the main and sub-inertial navigation systems is considered, and the positions of the main and sub-inertial navigation systems are modeled as follows: （12） in, The projection of the lever arm between the main inertial navigation system and the sub-inertial navigation system in the coordinate system of the main inertial navigation system, i.e., the lever arm; The projection of the position of the sub-inertial navigation coordinate system relative to the Earth coordinate system onto the Earth coordinate system; The projection of the position of the main inertial navigation coordinate system relative to the Earth coordinate system onto the Earth coordinate system; The projection of the position of the sub-inertial navigation coordinate system relative to the main inertial navigation coordinate system onto the Earth coordinate system; This is the rotation matrix from the Earth coordinate system to the principal inertial navigation coordinate system.

6. The transfer alignment method based on the left-invariant error model of a Lie group according to claim 5, characterized in that, In step 5, the attitude matrix of the main inertial navigation system is obtained from the operation of the navigation system. attitude matrix of sub-inertial navigation The equations for calculating the attitude measurement of the sub-inertial navigation system are as follows: （13） （14） in, This indicates the calculation of the Euler angles corresponding to the direction cosine matrix. It concerns the misalignment angle of the sub-inertial navigation system. and installation error angle A nonlinear function.

7. The transfer alignment method based on the left-invariant error model of a Lie group according to claim 6, characterized in that, In step 6, the auxiliary velocity of the main inertial navigation system is calculated based on the operation of the navigation system. auxiliary velocity estimates of the inertial navigation system The equation for calculating the velocity measurement of the sub-inertial navigation system is as follows: （15） in, This represents the estimated value of the rotation matrix from the navigation coordinate system to the sub-inertial navigation coordinate system; This represents the rotation matrix from the navigation coordinate system to the master inertial navigation coordinate system; For known The estimated value of the lever arm vector at time t.

8. The transfer alignment method based on the left-invariant error model of a Lie group according to claim 7, characterized in that, Step 7 specifically involves: Step 7.1: Construct the state vector of the sub-inertial navigation system; Step 7.1.1: Construct the lever arm vector and installation error angle using constant value modeling; （16） Step 7.1.2: Transfer the state variables , , and Extending to the left-invariant error state, we construct the 18-dimensional state variables of the filtering model. for: （17） in, For the misalignment angle of the sub-inertial navigation system, For speed error, To achieve zero bias in the gyroscope, To achieve zero bias in the accelerometer, The lever arm between the master and sub-inertial navigation systems, The installation error angle between the main and sub-inertial navigation systems; Step 7.2: Construct the noise vector of the sub-inertial navigation system; Step 7.2.1: Construct the measurement error of the sub-inertial gyroscope in equation (10) using constant zero bias and white noise modeling. Measurement error of neutron inertial accelerometer in equation (4) ; （18） in, and Gaussian white noise with zero mean; for gyroscope zero bias and accelerometer zero bias Modeled as constant with zero bias, , ; Step 7.2.2: 6-dimensional state noise of the filtered model for: （19） Step 7.3: Construct the state-space equations of the sub-inertial navigation system; Step 7.3.1: Determine the transfer matrix based on the attitude error differential equation (10) and the velocity error differential equation (4). and noise matrix for: （20） （21） Step 7.3.2: Determine the attitude measurement generation function according to attitude measurement equation (13). for: （22） Step 7.3.3: Determine the velocity measurement matrix according to the velocity measurement equation (15). for: （23） Step 7.3.4: Combine the attitude measurement function (22) and the velocity measurement matrix of equation (23) The state-space equations of the sub-inertial navigation system are obtained as follows: （24） in, For attitude measurement noise, For speed measurement noise; Step 7.4: Discretize the state-space equations to obtain: （25） （26） in, It is discrete time; It is a discrete transition matrix; It is a discrete noise matrix; For attitude discrete measurement noise, This is noise in discrete velocity measurements.

9. The transfer alignment method based on the left-invariant error model of a Lie group according to claim 8, characterized in that, In step 8, state estimation is achieved using an unscented Kalman filter, specifically as follows: Step 8.1: Construction indivual Click to update the time: （27） Step 8.2: Each Point weights are calculated as follows: （28） in, The weights used when calculating the mean. Weights used to calculate covariance; parameters Used to introduce prior information about non-Gaussian distributions, parameters This is the proportionality coefficient. ; Step 8.3: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require Point-in state propagation: （29） Step 8.4: Calculate the predicted mean based on steps 8.1 to 8.

3. and the mean square error of prediction ; （30） Step 8.5: For the attitude measurement equation and velocity measurement equation, update the measurements using a sequential update method. or ; Reconstruct indivual Click and update the measurement: （31） Step 8.6: The newly constructed Substituting the points into the measurement equations of the sub-inertial navigation system, we get: （32） Step 8.7: Calculate the predicted mean. and predicted covariance and the cross-covariance between state and measurement : （33） Step 8.8: Calculate the filter gain Combined measurement Obtain the filtered state estimate and variance : （34）。 10. A computer device, comprising a memory, a processor, and a computer program stored in the memory, characterized in that, The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 9.

Images