SINS (Strapdown Inertial Navigation System) dynamic initial alignment method based on state-related Bayesian-e group filtering

Abstract

The invention discloses a motion alignment method based on state-dependent Bayesian group filtering, which is used for solving the problems of rapid attitude change and state-dependent noise in high-dynamic initial alignment of a strapdown inertial navigation system. The method has two creative points. The method comprises the following steps: firstly, establishing an accurate initial alignment model containing an inertial sensor error by utilizing Lie group representation; an observation probability model is established by adopting a shaft angle model which is more in line with physical definition, and the rotation of the carrier is described and tracked more accurately. Secondly, in the process of designing a state correlation Lie group Bayesian filtering algorithm, exact composition of state correlation noise in the alignment model is deduced and analyzed, and observation noise and the state are decoupled through vector dot product and cross product; experimental results show that the method is obviously superior to the existing method in alignment precision and time.

Description

Technical Field

[0001] The present invention is a SINS dynamic initial alignment method based on state - related Bayesian Lie group filtering, and this method belongs to the technical field of navigation methods and applications. Background Technique

[0002] The strap - down inertial navigation system (SINS) directly mounts inertial devices on the vehicle, and has the advantages of high precision, strong anti - interference ability, smaller volume and weight, etc. Since the physical platform is omitted, the SINS must determine the rotation matrix between the vehicle coordinate system and the navigation coordinate system at the initial moment, which is the initial alignment of the SINS. The initial alignment provides the initial conditions for subsequent strap - down integral calculation and directly affects the accuracy and rapidity of the SINS. Therefore, studying the initial alignment of the SINS has very important practical significance.

[0003] According to the motion state of the vehicle during alignment, the initial alignment can be divided into static base alignment, sway alignment, and motion alignment. The latter two are often collectively referred to as dynamic alignment. The static base alignment is less interfered and relatively simple. With the efforts of many researchers, the research on static base alignment has been relatively perfect. However, in reality, the motion trajectory of the vehicle is usually complex and variable, and it is unrealistic to keep the vehicle stationary for a long time to complete the alignment task.

[0004] Currently, the mainstream initial alignment algorithms use unit quaternions to describe the model. However, in addition to the non - uniqueness problem, the quaternion alignment model also has a more difficult - to - solve model nonlinearity problem. If the nonlinear filtering method is directly used, it will consume a large amount of navigation computer resources, not only not improving the alignment accuracy, but also significantly increasing the alignment time. Therefore, two - step alignment is usually required. In the coarse alignment stage, the attitude error angle is quickly reduced to make the alignment model meet the linear approximation condition; in the fine alignment stage, the Kalman filter is mainly used to improve the alignment accuracy. There are many problems in the two - step alignment in practical applications. On the one hand, in the large misalignment angle scenario, the quaternion two - step alignment algorithm cannot meet the linearization approximation condition, resulting in a significant reduction in the alignment effect. In practical applications, the large misalignment angle scenario is widely present, which is an unavoidable problem. On the other hand, the noise types and statistical characteristics in dynamic alignment are more complex, resulting in a reduction in the alignment accuracy and an extension of the convergence time of the quaternion two - step alignment algorithm.

[0005] The attitude estimation method based on Lie group description shows good performance, but its core still relies on the Kalman filter framework, which has inherent limitations in local parameterization and linearization. The Kalman filter is based on a linear system model and Gaussian noise assumptions. Its core lies in estimating the state of the system by minimizing the variance of the prediction error. However, when linearizing the motion equation, the Kalman filter usually only considers the first-order approximation term of the attitude error, which may lead to algorithm divergence in the face of large attitude errors. In addition, the Gaussian distribution model relied on by the Kalman filter cannot accurately represent the attitude uncertainty on the manifold. When there are large errors in attitude estimation, the true attitude may be distributed in multiple regions on the manifold, and at this time, the attitude uncertainty exhibits significant dispersion. Due to the unique wrapping property of the manifold, the traditional Gaussian model fails because it relies on linear measurement and cannot accurately describe this dispersion.

[0006] To solve the problems existing in the above method and further improve the accuracy and time of initial alignment, the present invention draws on the properties of Lie groups, the axis-angle model, and the idea of Bayesian filtering, and proposes a new Bayesian Lie group filtering method. First, an initial alignment model is established using the Lie group probability distribution. Then, by making full use of the properties of vector dot product and cross product, and adopting an axis-angle model that is more in line with physical meaning, the state-related noise is decoupled from the state quantity attitude matrix and compensated to obtain an axis-angle probability distribution model. Finally, based on Bayesian estimation theory, a state-related Bayesian Lie group filter is designed using a probability distribution model that is more in line with the Lie group model. Actual road and turntable experiments show that the algorithm proposed by the present invention has better alignment accuracy and speed when the carrier faces complex motion trajectories, especially in high-dynamic alignment tasks. Summary of the Invention

[0007] The motion alignment of the carrier is ubiquitous, so studying the initial alignment algorithm in the dynamic state has great research significance and application. Since the carrier is more likely to be affected by various external interference factors during the initial alignment process in the dynamic state, the requirement for the effectiveness of the algorithm is also higher. The purpose of the present invention is to address the problems existing in the existing dynamic alignment methods: (1) The present invention describes the attitude matrix based on Lie groups, avoiding the singular value problem of the Euler angle method and the non-linearity and non-uniqueness problems of the quaternion method, and can be used in the case of rapid angle changes. (2) The present invention defines the probability distribution model of initial alignment through an axis-angle model that is more in line with physical meaning, transforms the observation model into an equivalent axis-angle rotation model, and decouples the state-related noise using vector dot product and cross product operations to obtain the probability distribution model under axis-angle observation. (3) The present invention uses Bayesian estimation theory to complete the algorithm design and realizes the initial alignment in the motion state. By this method, the problem of accuracy degradation caused by linear approximation in the traditional Kalman method is avoided.

[0008] A SINS dynamic initial alignment method based on state-dependent Bayesian Lie group filtering, which is realized through the following steps:

[0009] Step (1): The SINS strapdown inertial navigation system performs system warm-up preparation, starts the system, and obtains the longitude λ, latitude L of the carrier's location, and the projection g of the local gravitational acceleration in the navigation system n and other basic information, and collects the projection of the rotation angular rate information of the carrier system relative to the inertial system output by the gyroscope in the carrier system in the inertial measurement unit IMU and the acceleration information f of the carrier system output by the accelerometer b .

[0010] Step (2): Preprocess the data collected by the gyroscope and accelerometer, and establish a linear alignment system model described based on Lie groups based on Lie group differential equations

[0011] The coordinate system definitions in the detailed description of this method are as follows:

[0012] The Earth coordinate system e: Select the center of the Earth as the origin, the X-axis is in the equatorial plane, pointing from the center of the Earth to the prime meridian, the Z-axis points from the center of the Earth to the geographic north pole, and the X-axis, Y-axis, and Z-axis form a right-handed coordinate system and rotate with the Earth's rotation;

[0013] The geocentric inertial coordinate system i: Select the center of the Earth as the origin, the X-axis is in the equatorial plane, pointing from the center of the Earth to the vernal equinox point, the Z-axis points from the center of the Earth to the geographic north pole, and the X-axis, Y-axis, and Z-axis form a right-handed coordinate system;

[0014] The navigation coordinate system n: In this method, the navigation coordinate system is selected as the geographic coordinate system, with the center of gravity of the carrier as the origin, aligned with the east-north-up coordinate axes, the X-axis coincides with the east direction (E), the Y-axis coincides with the north direction (N), and the Z-axis coincides with the up direction (U);

[0015] The carrier coordinate system b: Represents the coordinate system where the outputs of the inertial sensors in the strapdown inertial navigation system are located, with the center of gravity of the carrier as the origin, and the X-axis, Y-axis, and Z-axis point to the right along the transverse axis of the carrier, forward along the longitudinal axis, and upward along the vertical axis respectively;

[0016] The initial navigation coordinate system n(0): Represents the navigation coordinate system when the strapdown inertial navigation system is powered on and operates, and remains stationary relative to the inertial space during the entire alignment process;

[0017] The initial carrier coordinate system b(0): Represents the carrier coordinate system when the strapdown inertial navigation system is powered on and operates, and remains stationary relative to the inertial space during the entire alignment process;

[0018] The error navigation coordinate system n': The navigation coordinate system obtained by the attitude estimation algorithm

[0019] Combined with the properties of Lie groups and the output of a low-precision SINS, an initial alignment model described by Lie groups is established:

[0020] According to the characteristics of the strapdown inertial navigation system, the alignment problem in the motion state can be transformed into the attitude estimation problem of the carrier. The attitude transformation matrix represents the rotation between the navigation coordinate system n and the carrier coordinate system b. This matrix is a 3×3 orthogonal matrix with a determinant equal to 1, which exactly conforms to the properties of the three-dimensional special orthogonal group SO(3) in Lie groups, thus forming the three-dimensional rotation group SO(3):

[0021]

[0022] Among them, R∈SO(3) represents the element in the three-dimensional rotation group SO(3) that represents the attitude transformation matrix, represents the 3×3 vector space, the superscript T represents the transpose of the matrix, I represents the three-dimensional identity matrix, and det(R) represents the determinant of matrix R;

[0023] The alignment problem in the jitter state is transformed into the problem of estimating the attitude transformation matrix R of the carrier described by Lie groups; according to the chain rule of real-time attitude matrix decomposition described by Lie groups, the attitude matrix to be solved is decomposed into the product form of three matrices in the time domain:

[0024]

[0025] Among them, t represents time, represents the attitude matrix of the initial navigation coordinate system relative to the navigation coordinate system at time t, and the initial attitude matrix represents the attitude matrix of the initial carrier coordinate system relative to the initial navigation coordinate system, represents the attitude matrix of the carrier coordinate system at time t relative to the initial carrier coordinate system;

[0026] According to the kinematic characteristics and Lie group differential equations, the update differential equations for the attitude matrices and changing with time are:

[0027]

[0028] Among them, represents the attitude matrix of the initial carrier coordinate system relative to the current carrier coordinate system, represents the projection of the rotation angular rate of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, It represents the projection of the rotation angular rate of the vehicle coordinate system relative to the inertial coordinate system output by the gyroscope onto the vehicle coordinate system. The symbol (·×) represents the operation of converting a three-dimensional vector into an anti-symmetric matrix, and the operation rules are as follows:

[0029]

[0030] By discretizing formulas (3) and (4), the attitude matrix and can be obtained for the iterative update equations:

[0031]

[0032] From formulas (2)-(7), it can be obtained that and can be calculated in real time from the IMU sensor data. To solve only the value of is required. And represents the attitude matrix at the initial moment, which does not change with time and is a constant attitude matrix; therefore, in the alignment process under the motion state, the problem of solving the attitude matrix is transformed into the problem of solving the initial attitude matrix described based on the Lie group;

[0033] According to the outputs of the IMU and GPS in the strapdown inertial navigation, two velocity vectors can be constructed, and their relationship with the initial attitude matrix described based on the Lie group can be written in the following form:

[0034]

[0035] where β and α respectively represent the velocity vectors constructed by the vehicle in the n system and the velocity vectors constructed by the vehicle in the b system, and they can be obtained through the following formula:

[0036]

[0037] where v n represents the vehicle velocity of the GPS in the n system, and v n (0) represents the vehicle velocity of the GPS in the n system at the initial moment.

[0038] Considering the influence of the sensor output error, the relationship between the true values and the measured values of the gyroscope and the accelerometer can be expressed as:

[0039]

[0040] where, represents the output value of the gyroscope, represents the output value of the accelerometer, δω represents the measurement noise of the gyroscope, and δf represents the measurement noise of the accelerometer.

[0041] After considering the noise of the sensors, rewrite Equation (10) as:

[0042]

[0043] where

[0044]

[0045]

[0046] Δθ1 and Δθ2 are the measurement increments of the gyroscope in two adjacent sampling periods. Δv1 and Δv2 are the measurement increments of the accelerometer in two adjacent sampling periods.

[0047] Considering the initial attitude matrix as a constant attitude matrix and Equations (8)-(16), the alignment model based on Lie group description in the motion state is:

[0048]

[0049] Step (3): To solve the error problem faced in the SINS motion alignment process, especially the state-dependent noise in Model (17), a state-dependent Bayesian Lie group filtering method is proposed.

[0050] The probability density function Matrix Fisher (MF) distribution on the Lie group space can be expressed as:

[0051]

[0052] where the operator tr(·) represents the trace of the matrix, and the F parameter can describe the average attitude and uncertainty information under this probability distribution through SVD decomposition. c(F) is the normalization coefficient, and its value is:

[0053] c(F) = ∫ SO(3) exp(tr(F T R))dR (18)

[0054] Performing SVD decomposition on the F parameter gives:

[0055] F = U'S'V' T (19)

[0056] where U, V are orthogonal matrices, and S is a diagonal matrix. To make this decomposition more consistent with the properties of the Lie group space and the probability distribution, the following transformations are made to U', S', V'.

[0057] U = diag[1, 1, det[U']] (20)

[0058] S = diag[s1, s2, s3] = diag[s′1, s′2, det[U′V′]s′3] (21)

[0059] V = V'diag[1, 1, det[V']] (22)

[0060] Among them, s1, s2, s3 are the diagonal elements of S, and the operator det(·) represents the determinant of the matrix. At this time, U, V ∈ SO(3), and S is the uncertainty on the MF distribution.

[0061] F = USV T (23)

[0062] According to the mapping relationship between Lie groups and Lie algebras:

[0063] R = exp(φ×) (24)

[0064] Among them, φ represents the Lie algebra corresponding to the rotation matrix R, and according to Rodrigues' formula, φ is the rotation vector corresponding to this rotation relationship. Therefore, to determine the rotation matrix R k only need to determine the rotation vector φ k That's it. According to the physical meaning of the rotation vector, the rotation matrix R in the initial alignment model k is equivalent to the rotation relationship between β and which is expressed as:

[0065] φ k = θa (25)

[0066] Among them, θ represents the rotation angle corresponding to R k and a represents the rotation axis corresponding to R k as shown Figure 2 in the figure.

[0067] According to the definition of vector cross product and the physical definition of rotation, the rotation angle is equal to the angle between two vectors, and the cross product between the rotation axis and the vector is in the same direction. Specifically expressed as:

[0068]

[0069] Therefore, the estimated value of the attitude matrix can be expressed as:

[0070]

[0071] Due to the deviation between the estimated rotation matrix and the actual rotation matrix, similarly according to the definition of the rotation vector, as shown in the figure, the error vector ε is expressed as:

[0072]

[0073] Then the estimated error covariance matrix of the attitude is:

[0074] E k = E[ε T ε] (32)

[0075] The measurement error covariance matrix of the sensor is:

[0076]

[0077] Therefore, the error covariance matrix of the observation model is:

[0078] Q k = E k + Q v (34)

[0079] According to the properties of the MF distribution, the following properties of the covariance matrix can be obtained:

[0080] Q k = diag[s2 + s3, s1 + s3, s1 + s2]

[0081] where s i,i ∈(1, 2, 3) is the S matrix parameter corresponding to the MF distribution. Therefore, the S matrix parameter is specifically expressed as:

[0082]

[0083] Thus, the parameter information required for the MF distribution in the axis-angle model is obtained, and the observation probability model is obtained:

[0084]

[0085] Therefore, the probability distribution on the Lie group is modeled as:

[0086]

[0087] According to the Bayesian posterior probability formula, we get:

[0088]

[0089] According to the properties of the MF distribution, perform SVD decomposition on the parameters in the posterior probability distribution:

[0090]

[0091] The optimal attitude estimate of the current Bayesian estimation filter is:

[0092] E[R] = UV T (40)

[0093] In summary, the process of the state - related Bayesian Lie group filtering algorithm is as follows:

[0094]

[0095] In each step of filtering, is used as the estimated value.

[0096] Step (4): Solve the attitude matrix required by the navigation system Thus, the alignment process in the motion state is completed.

[0097] According to the attitude change matrix obtained by solving in the previous steps and information, the navigation attitude matrix can be solved through formula (2), and the alignment of SINS in the motion state is completed.

[0098] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0099] (1) The present invention uses the Lie group description method to represent the attitude matrix, avoiding the singular value problem of the Euler angle representation method and the non - linearity and non - uniqueness problems of the quaternion representation method. A probability model is directly constructed on the Lie group, and a Bayesian filter is developed to update the attitude information. This method overcomes the limitations of the local parameterization and linear approximation of the traditional Kalman filter framework.

[0100] (2) The present invention constructs a probability distribution model under the axis - angle model through the definition of the Lie group error rotation matrix and the axis - angle rotation model. And by using the dot - product and cross - product operations of vectors, the sensor noise is decoupled from the state variables, and the noise is compensated during the filtering process, improving the alignment accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0101] Figure 1 Flowchart of the strapdown inertial navigation system.

[0102] Figure 2 Schematic diagram of the axis - angle model.

[0103] Figure 3 Flowchart of the state - related Lie group Bayesian filtering.

[0104] Figure 4 Alignment result diagram. DETAILED DESCRIPTION OF THE INVENTION

[0105] The present invention is designed for the state - related Bayesian Lie group filtering motion alignment method of SINS. The following describes the specific implementation steps of the present invention in detail with reference to the system flowchart of the present invention:

[0106] The beneficial effects of the present invention are as follows:

[0107] Under the following experimental conditions, experiments were conducted on the state-related Bayesian Lie group filtering motion alignment method:

[0108] In step (1), dynamic alignment during the erection process of the simulated vehicle was performed. The change in the pitch angle in the first 150 seconds was At 150 seconds, the pitch angle began to change at a speed of 10° / s to 80°. The roll angle and the heading angle were affected by the impact of the waves, and the attitude changes were and At the same time, the attitude angles of the vehicle were randomly and violently changed to make the output of the sensor more interfered.

[0109] In step (1), a three-axis turntable was used as the experimental platform, and the SINS was installed on the platform, and the system was preheated. Angular velocity data of the gyroscope and the accelerometer were collected for 15 minutes.

[0110] In step (1), the initial geographical location: 118° east longitude, 40° north latitude;

[0111] In step (1), the sensor output frequency was 100 Hz;

[0112] In step (3), the earth's angular rotation rate 7.2921158e -5 rad / s;

[0113] In step (3), the time interval T was 0.02 s;

[0114] In step (4), the initial value of the Lie group filtering algorithm

[0115] The experimental results of the method are as follows:

[0116] An experiment of 600 s was conducted, and the estimation error of the attitude angle was used as the measurement index. The experimental results are as Figure 4 shown. It can be seen from the figure that the pitch attitude completed alignment at about 165 s and converged to 4.45'; the roll attitude completed alignment at about 180 s and converged to 1.85'; the heading attitude completed alignment at about 323 s and converged to 7.45'. From the experimental results, it can be known that the present method can quickly and effectively complete the initial alignment task of the SINS in the motion state.

[0117] The present invention proposes and implements a state - related Bayesian Lie group filtering SINS motion alignment method. By using the matrix Fisher distribution and the more physically - defined axis - angle model, a probability distribution model is established under the Lie group initial alignment model framework. Then, according to the output models of the gyroscope and accelerometer, they are substituted into the initial alignment model under a moving base. By using the dot - product and cross - product operations of vectors, the observation noise decoupled from the state variables is deduced and analyzed, and the errors existing in the model are compensated. The SINS initial alignment method proposed by the present invention is applicable to the initial alignment under a moving state and effectively improves the stability and reliability of the navigation system.

[0118] The above - mentioned is only the preferred embodiment of the present invention and is not used to limit the present invention. It should be pointed out that for those of ordinary skill in the art in this technical field, without departing from the principle of the present invention, several improvements and modifications can be made, and these improvements and modifications should also be regarded as the protection scope of the present invention.

Claims

1. A SINS dynamic initial alignment method based on state-dependent Bayesian Lie group filtering, characterized in that, This method is implemented through the following steps: Step (1): The SINS strapdown inertial navigation system performs system warm-up preparation, starts the system, and obtains the longitude λ, latitude L of the carrier's location, and the projection g of the local gravitational acceleration in the navigation system. n Collect the projection of the rotation angular rate information of the carrier system relative to the inertial system output by the gyroscope in the inertial measurement unit (IMU) in the carrier system. And the acceleration information f of the carrier system output by the accelerometer. b ; Step (2): Preprocess the data collected by the gyroscope and accelerometer, and establish a linear alignment system model described by Lie group based on Lie group differential equations; The coordinate systems are defined as follows: The Earth coordinate system e: Select the center of the Earth as the origin, the X-axis lies in the equatorial plane, points from the center of the Earth to the prime meridian, the Z-axis points from the center of the Earth to the geographic north pole, and the X-axis, Y-axis, and Z-axis form a right-handed coordinate system and rotate with the Earth's rotation; The geocentric inertial coordinate system i: Select the center of the Earth as the origin, the X-axis lies in the equatorial plane, points from the center of the Earth to the vernal equinox, the Z-axis points from the center of the Earth to the geographic north pole, and the X-axis, Y-axis, and Z-axis form a right-handed coordinate system; The navigation coordinate system n: In this method, the navigation coordinate system is selected as the geographic coordinate system, with the center of gravity of the vehicle as the origin, aligned with the east-north-up coordinate axes, the X-axis coincides with the east direction (E), the Y-axis coincides with the north direction (N), and the Z-axis coincides with the up direction (U); The vehicle coordinate system b: Represents the coordinate system where the outputs of the inertial sensors in the strapdown inertial navigation system are located, with the center of gravity of the vehicle as the origin, the X-axis, Y-axis, and Z-axis point to the right along the transverse axis of the vehicle, forward along the longitudinal axis, and upward along the vertical axis respectively; The initial navigation coordinate system n(0): Represents the navigation coordinate system when the strapdown inertial navigation system is powered on and operates, and remains stationary relative to the inertial space throughout the alignment process; The initial vehicle coordinate system b(0): Represents the vehicle coordinate system when the strapdown inertial navigation system is powered on and operates, and remains stationary relative to the inertial space throughout the alignment process; The error navigation coordinate system n': The navigation coordinate system obtained by the attitude estimation algorithm; Combining the properties of Lie group and the output of the low-precision SINS, establish an initial alignment model described by Lie group: According to the characteristics of the strapdown inertial navigation system, transform the alignment problem in the motion state into the attitude estimation problem of the vehicle. The attitude transformation matrix represents the rotation between the navigation coordinate system n and the vehicle coordinate system b. This attitude transformation matrix is a 3×3 orthogonal matrix and its determinant is equal to 1, which exactly conforms to the properties of the three-dimensional special orthogonal group SO(3) in Lie group, forming the three-dimensional rotation group SO(3): where \(R\in SO(3)\) represents an element in the three-dimensional rotation group \(SO(3)\) that is used to represent the attitude transformation matrix, denotes a \(3\times3\) vector space, the superscript \(T\) represents the transpose of a matrix, \(I\) represents the three-dimensional identity matrix, and \(\det(R)\) represents the determinant of matrix \(R\); The alignment problem in the shaking state is transformed into the problem of estimating the attitude transformation matrix R of the vehicle body described by Lie group; according to the real-time attitude matrix decomposition chain rule based on Lie group description, the attitude matrix to be solved is decomposed into the product form of three matrices in the time domain: where t represents time, represents the attitude matrix of the initial navigation coordinate system relative to the navigation coordinate system at time t, and the initial attitude matrix represents the attitude matrix of the initial vehicle coordinate system relative to the initial navigation coordinate system, represents the attitude matrix of the vehicle coordinate system at time t relative to the initial vehicle coordinate system; According to the kinematic characteristics and Lie group differential equations, the attitude matrices and The updated differential equations that vary with time are as follows: Among them, represents the attitude matrix of the initial vehicle coordinate system relative to the current vehicle coordinate system, represents the projection of the rotational angular rate of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, represents the projection of the rotational angular rate of the vehicle coordinate system output by the gyroscope relative to the inertial coordinate system in the vehicle coordinate system. The symbol (·×) represents the operation of converting a three-dimensional vector into an anti-symmetric matrix, and the operation rules are as follows: Discretize equations (3) and (4) to obtain the attitude matrix and Iterative update equations for It can be obtained from formulas (2)-(7) that and are calculated in real time from IMU sensor data. To solve only need to the value of; represents the attitude matrix at the initial moment and is a constant attitude matrix; during the alignment process in the motion state, the problem of solving the attitude matrix is transformed into the problem of solving the initial attitude matrix described based on Lie groups; According to the outputs of the IMU and GPS in strapdown inertial navigation, two velocity vectors are constructed, and their relationship with the initial attitude matrix described based on Lie groups is written in the following form: is written as follows: Among them, β and α respectively represent the velocity vectors constructed by the vehicle in the n coordinate system and the velocity vectors constructed in the b coordinate system, and are obtained through the following formula: where v n represents the carrier velocity of GPS in the n - frame, and v n (0) represents the carrier velocity of GPS in the n - frame at the initial moment; Considering the influence of the sensor output error, the relationship between the true values and the measured values of the gyroscope and accelerometer is expressed as: Among them, represents the output value of the gyroscope, represents the output value of the accelerometer, δω represents the measurement noise of the gyroscope, and δf represents the measurement noise of the accelerometer; After considering the noise of the sensor, rewrite formula (10) as: Where δα = K f δf b +K w δω b #(14) Δθ1 and Δθ2 are the measurement increments of the gyroscope in two adjacent sampling periods; Δv1 and Δv2 are the measurement increments of the accelerometer in two adjacent sampling periods; Consider the initial attitude matrix For the constant attitude matrix and formulas (8)-(16), the alignment model described based on Lie groups in the motion state is as follows: Step (3): Propose a state-dependent Bayesian Lie group filtering method; The probability density function MF distribution on the Lie group space is expressed as: Among them, the operator tr(·) represents the trace of the matrix, the F parameter describes the average attitude and uncertainty information under this probability distribution through SVD decomposition; c(F) is the normalization coefficient, which is: c(F) = ∫ SO(3) exp(tr(F T R)) dR (18) Perform SVD decomposition on the F parameter to obtain: F = U'S'V' T (19) where U', S', U, V are orthogonal matrices, S is a diagonal matrix. To make this decomposition more consistent with the properties of the Lie group space and probability distribution, the following transformations are made to U', S', and V'; U = diag[1, 1, det[U′]] (20) S = diag[s1, s2, s3] = diag[s'1, s'2, det[U′V′]s'3] (21) V = V'diag[1, 1, det[V']] (22) where s1, s2, s3 are the diagonal elements of S, and the operator det(·) represents the determinant of a matrix; at this time, U, V ∈ SO(3), and S is the uncertainty on the MF distribution; F = USV T (23) According to the mapping relationship between Lie groups and Lie algebras: R = exp(φ×) (24) where, φ represents the Lie algebra corresponding to the rotation matrix R, and according to Rodrigues' formula, φ is the rotation vector corresponding to this rotation relationship; therefore, to determine the rotation matrix R k it is only necessary to determine the rotation vector φ k That's it; according to the physical meaning of the rotation vector, the rotation matrix R in the initial alignment model k is equivalent to the rotation relationship between β and expressed as; φ k = θa (25) where θ represents the rotation angle corresponding to R k and a represents the rotation axis corresponding to R k ; According to the definition of vector cross product and the physical definition of rotation, the angle of rotation is equal to the angle between two vectors, and the cross product between the axis of rotation and the vector is in the same direction; specifically expressed as: Therefore, the estimated value of the attitude matrix is expressed as: Due to the deviation between the estimated rotation matrix and the actual rotation matrix, according to the definition of the rotation vector, the error vector ε is obtained and expressed as: Then the estimated error covariance matrix of the attitude is: E k = E[ε T ε] (32) The measurement error covariance matrix of the sensor is: Therefore, the error covariance matrix of the observation model is: Q k = E k + Q v (34) According to the properties of the MF distribution, the following properties regarding the covariance matrix are obtained: Q k = diag[s2 + s3, s1 + s3, s1 + s2] where s i,i∈(1,2,3) is the S-matrix parameter corresponding to the MF distribution; therefore, the S-matrix parameter is specifically expressed as: Thus, the parameter information required for the MF distribution in the axis-angle model is obtained, and the observation probability model is obtained as: Therefore, the probability distribution on the Lie group is modeled as: According to the Bayesian posterior probability formula, it is obtained that: According to the properties of the MF distribution, perform SVD decomposition on the parameters in the posterior probability distribution: The optimal attitude estimation of the current Bayesian estimation filter is: E[R] = UV T (40) To sum up, the process of the state-dependent Bayesian Lie group filtering algorithm is: At each step of the filtering, is used as the estimated value; Step (4): Solve the attitude matrix required by the navigation system Thus, the alignment process under the motion state is completed; According to the attitude change matrix obtained by solving in the previous steps and information, the navigation attitude matrix can be solved through formula (2) to complete the alignment of SINS in the motion state.

Images