SINS positioning method and device assisted by additional state space model

Abstract

The application discloses a strapdown inertial navigation positioning method and device assisted by an additional state space model. In a pure inertial system, a state space model is established by offline fusion of a carrier kinematic model and an inertial navigation differential equation, and online Kalman filtering is performed to directly obtain optimal state estimation to update the attitude, velocity and position; after each update, zero operation is performed on the corresponding elements in the state vector, and the covariance matrix is transformed, and then recursive iteration is performed. The application solves the problems of fast error accumulation and incapability of providing precision information in the traditional pure inertial navigation solution method, realizes real-time estimation and update of navigation parameters by state space modeling and recursive processing of inertial sensor data through Kalman filtering without depending on any specific carrier. Compared with the traditional inertial navigation scheme based on the same sensor, the scheme can smooth random noise and inhibit rapid divergence of errors, and can obtain higher solution precision and more stable output.

Description

Technical Field

[0001] This invention relates to the field of navigation and guidance technology, and in particular to a strapdown inertial navigation and positioning method and apparatus with the assistance of an additional state-space model. Background Technology

[0002] Inertial navigation systems (INS) employ inertial components such as gyroscopes and accelerometers, using integral calculations to achieve navigation and positioning of a vehicle. INS operate completely autonomously, independent of any external conditions, weather conditions, or work location. They do not rely on external signals such as radio waves, nor do they transmit any physical signals. Therefore, INS possess advantages such as complete autonomy and covert operation. Furthermore, INS can provide all navigation elements, including three-dimensional position, three-dimensional velocity, and three-dimensional attitude. Consequently, INS plays an irreplaceable role in many crucial situations.

[0003] Strapdown inertial navigation systems (SINS) are a type of mechanical orchestration method for inertial navigation systems. Compared to platform-based SINS, they offer advantages such as simpler structure, easier installation and debugging, and higher reliability. They are currently the dominant mechanical orchestration method for inertial navigation systems. Except for a very few special mission platforms requiring ultra-high precision, almost all inertial navigation systems employ strapdown mechanical orchestration. The operation of a strapdown SINS is essentially an integration process. It performs numerical integration calculations based on the outputs of gyroscopes and accelerometers to obtain attitude, velocity, and position increments. These increments are then used to update the attitude, velocity, and position obtained at the previous moment. This process involves three integration calculations: attitude calculation, velocity calculation, and position calculation.

[0004] Current methods for solving strapdown inertial navigation systems directly substitute the outputs of the gyroscope and accelerometer into the fundamental equations of the inertial navigation system (a system of six differential equations), and then use a numerical integration method to integrate the system of differential equations. In order to reduce non-commutative errors such as rowing error and conic error during the integration process, a method of jointly processing the outputs at multiple time points is generally adopted, which is the so-called multi-sample method.

[0005] The derivation of the multiple sampling method is essentially a process of modeling the output of components, or more precisely, a process of establishing a kinematic model of the carrier's acceleration and angular velocity. This modeling process can employ polynomial motion models or pure conical motion models. Current traditional navigation calculation problems often involve combining strapdown inertial navigation systems with other equipment or data to correct measurement errors and obtain navigation data. This sacrifices the autonomy of inertial navigation and fails to achieve navigation results directly from pure inertial navigation calculations, independent of external observation. Summary of the Invention

[0006] Therefore, the purpose of this invention is to provide a strapdown inertial navigation and positioning method and device with the assistance of an additional state-space model, to perform pure inertial calculations and realize the conceptual shift from "error correction" to "direct calculation".

[0007] To achieve the above objectives, the present invention provides a strapdown inertial navigation and positioning method assisted by an additional state-space model, comprising the following steps:

[0008] S1. Based on the characteristics of the inertial elements and the expected motion characteristics of the carrier, establish a state-space model offline;

[0009] In the state-space model, the kinematic model for the carrier's acceleration and angular velocity is a constant second derivative model, and the third derivative is used as process noise; the state-space model includes:

[0010] State transition equation ;

[0011] Observation equations ;

[0012] in, Let be the state vector at time k+1; Let k be the state vector at time k; Let k be the process noise vector at time k; Let k be the state transition matrix at time k; Let be the observation noise vector at time k; Let y be the observation matrix at time k; k The observation vector at time k includes the angular velocity vector measured by the gyroscope and the specific force vector measured by the accelerometer.

[0013] The state transition equation is obtained by simultaneously discretizing the kinematic model of the carrier's acceleration and angular velocity and the basic equations of the strapdown inertial navigation system; wherein, the kinematic model of the carrier's acceleration and angular velocity includes differential equations of angular velocity and angular acceleration, as well as differential equations of acceleration and jerk; the basic equations of the strapdown inertial navigation system include differential equations of rotation vector and differential equations of velocity increment;

[0014] S2. Model initialization, including obtaining the initialization parameters required for filtering, the initial output data of the inertial navigation system, and the covariance matrix;

[0015] S3. After the navigation task begins, the output data of the inertial element is used as the observation value, Kalman filtering is performed, and the state estimate is output. The attitude, velocity, and position navigation parameters of the strapdown inertial navigation system are updated based on the obtained state estimate.

[0016] S4. After each update of the navigation parameters, set the elements in the state vector estimate to zero; perform a transformation operation on the corresponding state vector covariance matrix.

[0017] S5. Save the estimated state vector and the transformed covariance matrix after setting the elements to zero. Return to S3. When acquiring new gyroscope and accelerometer outputs, perform Kalman filtering and navigation parameter updates at the next sampling time until the navigation task ends.

[0018] Furthermore, the basic equations of the strapdown inertial navigation system include:

[0019] Rotational vector differential equation:

[0020] Formula (1)

[0021] The differential equation for the velocity increment:

[0022] Formula (2)

[0023] In formula (1), This represents the rotation vector within the sampling interval at time t. Let be the angular velocity vector within the sampling interval at time t. This represents the derivative of the rotating vector with respect to time.

[0024] In formula (2), This represents the derivative of the velocity increment caused by the specific force with respect to time. Let B be the force vector at time t. t Let be the coordinate transformation matrix from the vehicle coordinate system to the navigation coordinate system at time t.

[0025] Furthermore, the kinematic model of the carrier's acceleration and angular velocity includes:

[0026] Differential equations for angular velocity and angular acceleration: Formula (3)

[0027] The differential equations for acceleration and jerk are: Formula (4)

[0028] In formula (3), η t μ t e t and ε t All are zero-mean Gaussian white noise that is independent at time t; in formula (3) For the differential of angular velocity, Angular acceleration, The differential of angular acceleration; in formula (4) The differential of the force; g tTo accelerate, The derivative of the accelerometer.

[0029] Furthermore, the four sets of differential equations are combined and then expressed in a unified manner according to the following formula (5):

[0030] Formula (5)

[0031] Formula (6)

[0032] in, Let represent the differential of the state transition equation at time t. It represents the state vector matrix at time t, and I3 represents the 3D identity matrix. Indicates in t k-0.5 The comparison of time; express The cross product matrix, Indicates in t k-0.5 angular velocity at time t; express The cross product matrix; For vectors The cross product matrix is ; ;in, This represents the coordinates of the angular velocity of the n-system relative to the inertial coordinate system i at time k-1 in the navigation coordinate system n. This represents the coordinates of the angular velocity of the n-frame relative to the inertial coordinate system i at time k-2 in the navigation coordinate system n. Indicates the time interval between adjacent moments; in, η t , μ t , e t and ε t The stochastic differential equations for angular velocity, angular acceleration, acceleration, and jerk are independent zero-mean Gaussian white noise at time t.

[0033] Furthermore, after discretizing the unified representation of the equation, we obtain the state transition equation as shown in formula (7):

[0034] Formula (7)

[0035] Using the acceleration output by the gyroscope and the acceleration output by the accelerometer as observations in the state-space model, the observation equation shown in formula (10) is obtained:

[0036] Formula (10)

[0037] In a further preferred embodiment, the obtained state-space model is subjected to Kalman filtering, and the direction cosine matrix between the carrier coordinate system b at two adjacent time points is constructed based on the rotation vector estimation obtained from the Kalman filtering.

[0038] Formula (13)

[0039] in, It is a 3D identity matrix; This represents the rotation vector estimate obtained at time k in the Kalman filter. Represents rotation vector estimation The cross product matrix.

[0040] Further preferably, based on the constructed direction cosine matrix, the direction cosine at the current moment is calculated to complete the update of attitude parameters, velocity parameters, and position. The covariance matrix of the state vector estimation in Kalman filtering is then subjected to the following linear transformation.

[0041] Formula (19)

[0042] in, Let k be the covariance matrix at time k. , Represents a 15x15 identity matrix; The direction cosine matrix from the carrier coordinate system at time k-1 to the carrier coordinate system at time k.

[0043] The present invention also provides a navigation and positioning device, characterized in that it includes: one or more processors, a memory, and one or more computer programs, wherein the one or more computer programs are stored in the memory and configured to be executed by the one or more processors, and the computer programs include methods for performing the above-described strapdown inertial navigation and positioning method assisted by an additional state-space model.

[0044] This application discloses a strapdown inertial navigation and positioning method and apparatus assisted by an additional state-space model. It achieves pure inertial computation, rather than combining the strapdown inertial navigation system with other devices or data. Compared to existing technologies based on navigation devices, this application is entirely based on internal data from inertial sensors, without relying on any specific carrier. It directly estimates and updates navigation parameters (attitude, velocity, and position) in real time, using state-space modeling and recursive processing of inertial sensor data via Kalman filtering. Through an "infinite memory" filtering process, random noise is smoothed, and rapid error divergence is suppressed. Compared to equivalent sensors, this scheme achieves higher computational accuracy and more stable output.

[0045] Traditional navigation and positioning algorithms rely on the raw sampling frequency of the vehicle and sensors. Traditional methods require caching data from multiple sampling points to calculate higher-order error compensations (such as conical compensation and rowing compensation), resulting in a navigation parameter update rate lower than the sensor's raw sampling rate. In this application, however, navigation parameter updates are synchronized with filter updates, and the update rate equals the sensor's sampling rate. This is crucial for highly dynamic vehicles, enabling the capture of more subtle motion changes.

[0046] Traditional methods can only output numerical values ​​for attitude, velocity, and position, but cannot inform the user how reliable these results are. This application, however, automatically updates and outputs the covariance matrix of the state estimate through Kalman filtering. This directly reflects the estimation error and confidence level of each navigation parameter at the current moment. It enables integrity monitoring and fault diagnosis of the navigation system. The system can recognize that "the positioning is now inaccurate" and alert the upper-level system—a qualitative leap that traditional inertial navigation cannot achieve.

[0047] This application can provide navigation parameters such as attitude, angular velocity, position, velocity, and acceleration, which are provided by traditional inertial navigation system calculation methods, as well as other redundant parameters such as angular acceleration and jerk. Furthermore, this invention is applicable not only to rate gyroscopes and accelerometers that output angular velocity and acceleration, but also to incremental gyroscopes and accelerometers that output angular increments and velocity increments. Attached Figure Description

[0048] Figure 1 This is a flowchart illustrating the strapdown inertial navigation and positioning method for which an additional state-space model is used as an aid in this invention.

[0049] Figure 2 This is a schematic diagram of the additional state space model used in the strapdown inertial navigation and positioning method of the present invention. Detailed Implementation

[0050] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0051] like Figure 1 As shown, one embodiment of the present invention provides a strapdown inertial navigation and positioning method assisted by an additional state-space model, comprising the following steps:

[0052] S1. Based on the characteristics of the inertial elements and the expected motion characteristics of the carrier, establish a state-space model offline;

[0053] S2. Model initialization, including obtaining the initialization parameters required for filtering, the initial output data of the inertial elements, and the covariance matrix; the initial output data of the inertial elements are used as observations to form the observation equations.

[0054] More preferably, in the state-space model, the kinematic model of the carrier's acceleration and angular velocity is a constant second derivative model, and the third derivative is used as process noise;

[0055] The state transition equations are formed by combining the kinematic model of the carrier's acceleration and angular velocity with the basic equations of the strapdown inertial navigation system and then discretizing them.

[0056] The basic equations of the strapdown inertial navigation system include:

[0057] Rotational vector differential equation: Formula (1)

[0058] The differential equation of the rotational vector is the differential equation of the velocity increment caused by the specific force:

[0059] Formula (2)

[0060] The expression for matrix B is: , Represents a 3D identity matrix; In this context, i, b, and n in the subscripts represent the inertial coordinate system, the vehicle coordinate system, and the navigation coordinate system, respectively, and m in the subscript indicates the m-th time. Representing vectors The cross product matrix. Let be the direction cosine matrix from the vehicle coordinate system b to the navigation coordinate system n at time m-1; It is represented as the direction cosine matrix from the navigation coordinate system n to the vehicle coordinate system b at time m-1; express The coordinates of the angular velocity of the n-frame relative to the inertial coordinate system i at time n in the navigation coordinate system n.

[0061] The kinematic model of the carrier's acceleration and angular velocity includes:

[0062] Differential equations for angular velocity and angular acceleration: Formula (3)

[0063] The differential equations for acceleration and jerk are: Formula (4)

[0064] In formula (3), ε t η t μ t e t and ε tThese are independent zero-mean Gaussian white noises at time t, representing the stochastic differential equations of angular velocity, angular acceleration, acceleration, and jerk. In the above formulas, these noises only indicate their existence; what is used subsequently are the covariance matrices of these noises, which are set by the user based on the performance of the inertial element. In formula (3)... For the differential of angular velocity, Angular acceleration, The differential of angular acceleration; in formula (4) The differential of the force; g t To accelerate, The derivative of the accelerometer.

[0065] A further preferred approach is to combine the four sets of differential equations and express them uniformly according to the following formula (5):

[0066] Formula (5)

[0067] in

[0068] Formula (6)

[0069] in, Let represent the differential of the state transition equation at time t. It represents the state vector matrix at time t, and I3 represents the 3D identity matrix. Indicates in t k-0.5 The comparison of time; express The cross product matrix, Indicates in t k-0.5 angular velocity at time t; express The cross product matrix; For vectors The cross product matrix is ; ;in, This represents the coordinates of the angular velocity of the n-system relative to the inertial coordinate system i at time k-1 in the navigation coordinate system n. Let represent the coordinates of the angular velocity of the n-system relative to the inertial coordinate system i at time k-2 in the navigation coordinate system n; similarly, it can be calculated; Where, η t μ t e t and ε t The stochastic differential equations for angular velocity, angular acceleration, acceleration, and jerk are independent zero-mean Gaussian white noise at time t.

[0070] Discretizing the unified equation yields the state transition equation shown in formula (7):

[0071] Formula (7)

[0072] Before discretization, let t represent the current time; after discretization, let k represent the current time. The following relationship exists between t and k:

[0073] Formula (8)

[0074] Let the process noise w in formula (7) k The covariance matrix is ​​represented as Q k Q k The derivation of the expression is as follows:

[0075] Formula (9)

[0076] , , , These represent the covariance matrices of angular velocity, angular acceleration, acceleration, and jerk, respectively, using zero-mean Gaussian white noise. They are typically set as constant * 3 * 3 identity matrices, with the constant set by the user.

[0077] Using the acceleration output by the gyroscope and the acceleration output by the accelerometer as observations in the state-space model, the observation equation shown in formula (10) is obtained:

[0078] Formula (10)

[0079] in The measured acceleration value output by the gyroscope. The measured value of the specific force vector obtained from the acceleration output by the accelerometer;

[0080] Formula (11)

[0081] Represents a 6-dimensional identity matrix;

[0082] The resulting state-space model includes:

[0083] State transition equation ;

[0084] Observation equations ;

[0085] in, Let be the state vector at time k+1; Let k be the state vector at time k; Let k be the process noise vector at time k; Let k be the state transition matrix at time k; Let be the observation noise vector at time k; Let be the observation matrix at time k; y k The observation vector at time k includes the angular velocity vector measured by the gyroscope and the specific force vector measured by the accelerometer.

[0086] S3. After the navigation task begins, the obtained inertial element output data is used as the observation value, Kalman filtering is performed, and the output is the state estimate. The attitude, velocity and position of the strapdown inertial navigation system are updated based on the obtained state estimate.

[0087] S4. After each navigation parameter update, set the elements in the state vector estimate to zero; and perform a transformation operation on the corresponding state vector covariance matrix.

[0088] Kalman filtering is performed based on the above state-space model. The Kalman filtering formula is shown in formula (12).

[0089] Formula (12)

[0090] Indicates to The estimated value; This represents the true value of the variable at time k, given that the observations at time k-1 and before (and including) time k-1 are known. express The covariance matrix of the estimation error; R represents the state transition matrix at time k-1; k Q is the observation noise vector matrix at time k; k For process noise w k The covariance matrix.

[0091] In the formula, the subscript k|k-1 represents the estimate of the variable at time k given that the observations before (and including) time k-1 are known.

[0092] S5. Save the estimated state vector and the transformed covariance matrix after setting the elements to zero. Return to S3. When obtaining the new gyroscope and accelerometer outputs, perform Kalman filtering and navigation parameter updates at the next sampling time until the navigation task ends.

[0093] Based on the rotation vector estimate obtained from the aforementioned Kalman filtering, without loss of generality, it is expressed here as follows: The obtained state-space model is subjected to Kalman filtering, and the direction cosine matrix is ​​constructed based on the rotation vector estimate obtained from the Kalman filtering.

[0094] Formula (13)

[0095] in, The cosine matrix in the carrier coordinate system; It is a 3D identity matrix; This represents the rotation vector estimate at time k obtained from the Kalman filter; express The cross product matrix.

[0096] Based on the constructed direction cosine matrix, the direction cosine at the current moment is calculated, and the attitude parameters, velocity parameters, and position are updated.

[0097] The extrapolation formula can be expressed as:

[0098] Formula (14)

[0099] In the formula, , , Then, the direction cosine matrix in the following formula is calculated. This represents the coordinates of the angular velocity of the n-frame relative to the inertial coordinate system i in the navigation coordinate system n. The coordinates of the angular velocity of the n-frame relative to the inertial coordinate system e-frame in the navigation coordinate system n-frame; l represents latitude, and h represents altitude. Indicates eastward speed; Indicates northward velocity; r M Earth's radius of curvature, r N It is the radius of curvature of the meridian; This represents the coordinates of the angular velocity of the n-system relative to the inertial coordinate system i at time k-1 in the navigation coordinate system n. This represents the coordinates of the angular velocity of the n-system relative to the inertial coordinate system i at time k-2 in the navigation coordinate system n.

[0100] Formula (15)

[0101] The direction cosine matrix from time k-1 to time k in the navigation coordinate system n.

[0102] According to formulas (13) and (15), the direction cosine matrix at the current time can be obtained by formula (16).

[0103] Formula (16)

[0104] in, This represents the direction cosine matrix at time k-1, transformed from the vehicle coordinate system b to the navigation coordinate system n. The direction cosine matrix from the carrier coordinate system at time k-1 to the carrier coordinate system b at time k completes the update of the attitude parameters.

[0105] The speed update formula is described below, and the following calculations are performed first.

[0106] Formula (17)

[0107] g represents gravitational acceleration; Let n be the gravitational acceleration in the navigation coordinate system n. This represents the gravitational acceleration value at time k-0.5 in the n-system; This represents the coordinates of the angular velocity of the e-frame relative to the i-frame in the navigation coordinate system n. This represents the coordinates of the angular velocity in the n-frame relative to the e-frame in the navigation coordinate system n. Represents the velocity matrix in the n-system; Let represent the velocity matrix at time k-0.5 in the n-system; This represents the coordinates of the angular velocity of the n-frame relative to the e-frame at time k-0.5 in the navigation coordinate system n.

[0108] The value at k-0.5 in the above formula is obtained by extrapolation as shown in formula (14). Based on the above formula and the u obtained from the aforementioned Kalman filtering... k The speed update formula is as shown in formula (18).

[0109] Formula (18)

[0110] The location update method is the same as that of traditional strapdown inertial navigation systems, so it will not be repeated here.

[0111] Parameters in Kalman filter estimation and Perform a zeroing operation; apply the following linear transformation to the covariance matrix of the state vector estimation in the Kalman filter.

[0112] Formula (19)

[0113] in, Let k be the covariance matrix at time k. , Represents a 15x15 identity matrix; The direction cosine matrix from the carrier coordinate system at time k-1 to the carrier coordinate system at time k.

[0114] In this application, multiple parameters only differ in their superscript and subscript indices. The specification provides an exemplary explanation for each type of parameter. For parameters whose superscript and subscript indices change compared to the example, the explanation rules of the example shall be followed.

[0115] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. An inertial navigation positioning method assisted by an augmented state space model, characterized in that, The method comprises the following steps: S1, establishing a state space model offline according to characteristics of an inertial element and expected motion characteristics of a carrier; in the state space model, a kinematic model of motion acceleration and angular velocity of the carrier is a constant second derivative model, and a third derivative is taken as process noise; the state space model comprises: State transition equation Observation equation ; wherein, is the state vector at time k + 1 ; is the state vector at time k ; is the process noise vector at time k ; is the state transition matrix at time k ; is the observation noise vector at time k ; is the observation matrix at time k ; y k denotes the observation vector at time k, including the angular velocity vector measured by the gyroscope and the specific force vector measured by the accelerometer; the state transition equation is obtained by simultaneously solving a kinematic model of motion acceleration and angular velocity of the carrier and basic equations of a strapdown inertial navigation system and discretizing the equations; the kinematic model of motion acceleration and angular velocity of the carrier comprises differential equations of angular velocity and angular acceleration, and differential equations of acceleration and jerk; the basic equations of the strapdown inertial navigation system comprise a differential equation of a rotation vector and a differential equation of a velocity increment; the kinematic model of motion acceleration and angular velocity of the carrier comprises: Differential equations of angular velocity and angular acceleration: Equation (3) Differential equation of acceleration and jerk: Equation (4) In formula (3), η t , μ t , e t and ε t are mutually independent zero-mean Gaussian white noises at time t; in formula (3), is an angular velocity differential, is an angular acceleration, is an angular acceleration differential; in formula (4), is a specific force differential; g t is a jerk, is a jerk differential; S2, model initialization, comprising obtaining initialization parameters required for filtering, initial output data of an inertial navigation system, and a covariance matrix; S3, after a navigation task starts, performing Kalman filtering with output data of the inertial element as observation values, outputting state estimation, and updating attitude, velocity and position navigation parameters of the strapdown inertial navigation system according to the obtained state estimation; S4, after updating the navigation parameters each time, performing a zero operation on elements in the state vector estimation, and performing a transformation operation on a corresponding state vector covariance matrix; S5, saving the state vector estimation after the elements are zeroed and the transformed covariance matrix, returning to S3, and performing Kalman filtering and navigation parameter updating of a next sampling time when new gyroscope and accelerometer output is obtained, until the navigation task ends.

2. The additional state space model aided SINS positioning method according to claim 1, characterized in that, the basic equations of the strapdown inertial navigation system comprise: a differential equation of a rotation vector: Formula (1) a differential equation of a velocity increment: Equation (2) In equation (1), denotes the rotation vector in the sampling interval at time t, is the angular velocity vector in the sampling interval at time t, denotes the differential of the rotation vector with respect to time; In equation (2), denotes the differential of the velocity increment caused by the specific force with respect to time, is the specific force vector at time t, B t is the coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system at time t.

3. The additional state space model aided SINS positioning method according to claim 2, characterized in that, after the four sets of differential equations are simultaneously solved, the equations are uniformly represented according to the following formula (5): Equation (5) Formula (6) wherein denotes the differential of the state transition equation at time t, denotes the state vector matrix at time t, I 3 denotes the 3-dimensional identity matrix, denotes the angular velocity at time t k-0.5 ; denotes the cross product matrix of , denotes the angular acceleration at time t k-0.5 ; denotes the cross product matrix of ; is the cross product matrix of the vector ; ; ; wherein denotes the coordinates of the angular velocity of the n-frame with respect to the inertial i-frame in the navigation n-frame at time k-1; denotes the coordinates of the angular velocity of the n-frame with respect to the inertial i-frame in the navigation n-frame at time k-2; denotes the time interval between adjacent time instants; wherein η t , μ t , e t and ε t are mutually independent zero-mean Gaussian white noises at time t of the stochastic differential equations of the angular velocity, the angular acceleration, the acceleration and the jerk, respectively.

4. The additional state space model aided SINS positioning method according to claim 3, characterized in that, after the uniformly represented equations are discretized, a state transition equation as shown in formula (7) is obtained: Equation (7) the acceleration output by the gyroscope and the acceleration output by the accelerometer are taken as observation values in the state space model to obtain an observation equation as shown in formula (10): 。 5. The additional state space model aided SINS positioning method according to claim 2, characterized in that, the obtained state space model is subjected to Kalman filtering, a direction cosine matrix between carrier coordinate systems b at adjacent two times is constructed according to rotation vector estimation obtained in the Kalman filtering; ； wherein is a 3-dimensional identity matrix; denotes the k-th rotation vector estimate obtained in the Kalman filter denotes the rotation vector estimate is the cross product matrix of the rotation vector estimate 6. The additional state space model aided SINS positioning method according to claim 5, characterized in that, according to the constructed direction cosine matrix, a direction cosine at a current time is calculated, attitude parameter updating, velocity parameter updating and position updating are completed, and a covariance matrix of state vector estimation in the Kalman filtering is linearly transformed as follows ； wherein, is the covariance matrix at the current k time instant; , denotes a 15*15 identity matrix; direction cosine matrix from the k-1 time instant body coordinate system to the k time instant body coordinate system.

7. A navigation positioning device, characterized by comprise: one or more processors, a memory and one or more computer programs, wherein the one or more computer programs are stored in the memory and configured to be executed by the one or more processors, and the computer programs comprise a method for performing additional state space model assisted strapdown inertial navigation positioning of any one of claims 1-6.

Images