An invariant filtering-based static base fine alignment method and system, a terminal and a storage medium

Abstract

The application provides a static base fine alignment method and system based on invariant filtering, a terminal and a storage medium. The method comprises the following steps: obtaining initial state information and an IMU data set of a static base carrier; dividing the IMU data set into a plurality of IMU data subsets based on a preset time interval; sequentially performing a plurality of times of iterative updating on the initial state information by using the IMU data subsets based on a time sequence, and obtaining updated state information, wherein, in each iterative updating process, the state information of the current iteration is sequentially subjected to mechanical arrangement, state updating and Kalman filtering measurement updating according to the current IMU data subset, and updated state information is obtained, wherein, invariant filtering is introduced in the process of state updating and Kalman filtering measurement updating; and outputting the updated state information as a fine alignment result, thereby improving the accuracy of static base fine alignment.

Description

Technical Field

[0001] This application relates to the field of inertial navigation technology, and in particular to a method, system, terminal and storage medium for precise alignment of a static base based on invariant filtering. Background Technology

[0002] Static base alignment is usually divided into two steps. The first step is coarse alignment of the static base, which uses analytical coarse alignment and utilizes two non-coplanar vectors—angular velocity measurement and acceleration measurement—to solve for the initial rotation array. Essentially, it determines the inertial navigation orientation by sensing the Earth's rotation and gravity vector. The second step is fine alignment of the static base, which uses the rotation array obtained from coarse alignment as the initial attitude for fine alignment. Based on the characteristic that the velocity of the static base carrier is basically zero, the attitude angle error is deduced from the velocity error.

[0003] Existing static base alignment typically approximates the specific force as gravity directly in the update equations, thus introducing attitude-related approximation errors. Replacing the specific force with the local gravity vector also reduces the observability of the attitude, resulting in unsatisfactory effects on further refining the initial attitude. If the specific force approximation is not handled in the update equations, vibrations are usually unavoidable when the carrier is stationary. This effect introduces some erroneous additional observability, such as the observation of previously unobservable states like zero bias, which also leads to unsatisfactory effects on further refining the initial attitude. Summary of the Invention

[0004] To address the aforementioned technical problems, this application provides a method, system, terminal, and storage medium for static base precision alignment based on invariant filtering, thereby improving the accuracy of static base precision alignment.

[0005] In a first aspect, embodiments of this application provide a method for precise alignment of a static base based on invariant filtering, comprising:

[0006] Obtain the initial state information and IMU dataset of the static base carrier;

[0007] The IMU dataset is divided into several IMU data subsets based on a preset time interval;

[0008] Based on the time sequence, the initial state information is iteratively updated several times using each subset of IMU data to obtain updated state information. In each iteration, the state information of the current iteration is mechanically arranged, updated, and Kalman filtered measurement is updated according to the current subset of IMU data to obtain updated state information. Invariant filtering is introduced in the process of state update and Kalman filtered measurement update.

[0009] The updated status information is output as the alignment result.

[0010] This application provides a static base precision alignment method based on invariant filtering. First, IMU data is divided according to a preset time interval, allowing for multiple rounds of mechanical arrangement, state updates, and measurement updates during the static base precision alignment process. This enables iterative updates to the static base's state information. Compared to directly using all IMU data for precision alignment, this application effectively utilizes IMU data, allowing it to more accurately reflect the object's motion state and improving the accuracy of static base precision alignment. Furthermore, this application introduces invariant filtering for state and measurement updates during each iteration, mathematically avoiding the introduction of comparison forces. This preserves the unobservable characteristics of the originally unobservable state while achieving accurate attitude estimation, further improving the accuracy of static base precision alignment.

[0011] Furthermore, the step of sequentially performing mechanical orchestration, state update, and Kalman filter measurement update on the current iteration's state information based on the current IMU data subset to obtain the updated state information includes:

[0012] Based on the current IMU data subset and the current iteration state information, the inertial navigation state is recursively calculated, and the current estimated state information is updated to obtain the estimated state information, which includes the estimated attitude matrix, the estimated n-system velocity, the estimated gyroscope zero bias, and the estimated accelerometer zero bias.

[0013] Based on the estimated attitude matrix and the estimated n-system velocity, the current estimated system error state variables and state variance matrix are obtained by updating based on invariant filtering.

[0014] Based on the estimated system error state quantity and the state variance matrix, the estimated state information is updated by Kalman filtering to obtain the updated state information.

[0015] This application provides a method for updating state information. In mechanical orchestration, the current IMU data subset is used to perform inertial navigation state recursion on the state information of the current iteration, and the current estimated state information is initially calculated. Then, invariant filtering is introduced, and the current estimated system error state quantity and state variance matrix are updated based on the estimated state information. The estimated system error state quantity is used for subsequent compensation and correction of the estimated state information. However, the estimated system error state quantity is still not accurate enough at this time, and further calculation of the estimated system error state quantity is required through Kalman filter measurement update. The state variance matrix is ​​a necessary matrix in the Kalman filter measurement update process. The current estimated system error state quantity and state variance matrix are obtained through attitude estimation and n-system velocity estimation updates, preparing data for subsequent Kalman filter measurement updates.

[0016] In one possible implementation, the step of performing inertial navigation state recursion based on the current IMU data subset and the current iteration's state information to update and obtain the current estimated state information includes:

[0017] Calculate the current estimated gyroscope zero bias and estimated accelerometer zero bias based on the current subset of IMU data;

[0018] The estimated attitude matrix is ​​updated by recursively performing attitude inference based on the angular velocity measurement values ​​in the current IMU data subset and the first attitude matrix in the current iteration state information;

[0019] The estimated n-system velocity is updated by recursively calculating the velocity based on the acceleration increment in the current IMU data subset and the first n-system velocity in the current iteration state information.

[0020] In this embodiment, gyroscope bias estimation, accelerometer bias estimation, attitude array estimation, and n-system velocity estimation are performed using the current IMU data subset, completing attitude recursion and velocity recursion, thus preparing data for subsequent static base precision alignment.

[0021] In one possible implementation, the step of updating the current estimated system error state variables and state variance matrix based on the estimated attitude matrix and the estimated n-system velocity using invariant filtering includes:

[0022] Construct a deterministic matrix and a noise-driven matrix based on the estimated attitude matrix and the estimated n-system velocity;

[0023] Construct a one-step matrix based on the deterministic matrix;

[0024] The first system error state quantity is updated according to the one-step matrix to obtain the estimated system error state quantity, wherein the first system error state quantity is constructed based on the first misalignment angle, first velocity error, first gyroscope zero bias error and first accelerometer zero bias error obtained after the last Kalman filter measurement update;

[0025] The first state variance matrix is ​​updated based on the noise driving matrix and the one-step matrix to obtain the state variance matrix, wherein the first state variance matrix is ​​the state variance matrix used in the previous Kalman filter measurement update.

[0026] In this embodiment, invariant filtering is introduced by constructing a deterministic matrix, a noise-driven matrix, and a one-step matrix, thereby updating the system error state variables and the state variance matrix. Since the first system error state variable includes Lie group states and non-Lie group states, this embodiment combines the system error state variable obtained after the previous round of Kalman filter measurement update with the current estimated attitude matrix and estimated n-system velocity for error recursion. The error recursion conforms to Lie group states and can be regarded as introducing invariant filtering. This makes the static base fine alignment process of this embodiment conform to the mathematical variance matrix update process without sacrificing or increasing observability, and can more accurately reflect the motion state of the object, better utilize the characteristics of the static base to complete fine alignment, and improve the accuracy of static base fine alignment.

[0027] Furthermore, the specific formula for constructing a deterministic matrix and a noise-driven matrix based on the estimated attitude matrix and the estimated n-system velocity is as follows:

[0028]

[0029] Among them, F k+1 Let g be the deterministic matrix at time k+1, skew is the notation for finding the antisymmetric matrix of a three-dimensional vector, and g is the deterministic matrix at time k+1. n Let I be the weight vector in the n-system, τ be the preset correlation time, and I be the weight vector in the n-system. 3×3 It is a 3x3 identity matrix. Let v be the estimated attitude matrix at time k+1. n(k+1) G is the estimated velocity of the n-system at time k+1. k+1 The noise-driven array at time k+1.

[0030] The first state variance matrix is ​​updated based on the noise driving matrix and the one-step matrix to obtain the state variance matrix, and the specific formula is as follows:

[0031] x k+1 =Φ k+1 ·x k

[0032] The step of updating the first system error state quantity based on the one-step matrix to obtain the estimated system error state quantity is specifically formulated as follows:

[0033]

[0034] Where, x k+1 Let x be the estimated system error state variable at time k+1. k Let Φ be the first systematic error state quantity at time k. k+1 Let D be the matrix at time k+1. k+1 Let D be the state variance matrix at time k+1. k Let G be the state variance matrix at time k.k+1 Let Q be the noise driving array at time k+1, and let Q be the preset noise array.

[0035] In one possible implementation, updating the estimated state information using Kalman filtering measurements based on the estimated system error state quantity and the state variance matrix to obtain the updated state information includes:

[0036] Based on the state variance matrix, the estimated system error state quantity is updated by Kalman filtering to obtain the second system error state quantity. The second system error state quantity includes the second misalignment angle, the second velocity error, the second gyroscope zero bias error, and the second accelerometer zero bias error.

[0037] The second attitude matrix is ​​calculated based on the second misalignment angle and the estimated attitude matrix;

[0038] The second n-system velocity is calculated based on the second misalignment angle, the second velocity error, and the estimated n-system velocity.

[0039] The second gyroscope zero bias and the second accelerometer zero bias are calculated based on the second gyroscope zero bias error, the second accelerometer zero bias, the estimated gyroscope zero bias, and the estimated accelerometer zero bias.

[0040] The updated state information is obtained by combining the second attitude array, the second n-system velocity, the second gyroscope zero bias, and the second accelerometer zero bias.

[0041] This application provides a Kalman filter measurement update method. The basic idea of ​​a conventional Kalman filter algorithm is to use the state estimate from the previous moment and the observation from the current moment to obtain the optimal estimate of the state variables of a dynamic system at the current moment, generating a more accurate estimate of unknown variables than based solely on a single measurement. Therefore, it is particularly suitable for solving the measurement update problem in this application embodiment, improving the accuracy of static base alignment. In this application embodiment, based on the state variance matrix, the estimated system error state quantity is updated using standard Kalman filtering to obtain the updated second system error state quantity. After determining the second system error state quantity, the current estimated state information can be compensated based on the second system error state quantity to obtain the updated state information, thus realizing iterative updating of the state information.

[0042] Secondly, correspondingly, embodiments of this application provide a static base precision alignment system based on invariant filtering, including an acquisition module, a data partitioning module, an iterative update module, and an output module;

[0043] The acquisition module is used to acquire the initial state information and IMU dataset of the static base carrier;

[0044] The data partitioning module is used to divide the IMU dataset into several IMU data subsets based on a preset time interval;

[0045] The iterative update module is used to perform several iterative updates on the initial state information based on time order using each subset of IMU data to obtain updated state information. In each iterative update process, the state information of the current iteration is mechanically arranged, updated, and Kalman filtered measurement is sequentially performed according to the current subset of IMU data to obtain updated state information. Invariant filtering is introduced in the process of state update and Kalman filtered measurement update.

[0046] The output module is used to output the updated status information as a fine alignment result.

[0047] Thirdly, embodiments of this application provide a terminal including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor executes the computer program to implement the described static base fine alignment method based on invariant filtering.

[0048] Fourthly, embodiments of this application provide a computer-readable storage medium, the computer-readable storage medium including a stored computer program, wherein, when the computer program is executed, it controls the device where the computer-readable storage medium is located to perform the static base fine alignment method based on invariant filtering. Attached Figure Description

[0049] Figure 1 : A flowchart illustrating a static base alignment method based on invariant filtering provided in an embodiment of this application.

[0050] Figure 2 : A schematic diagram of the algorithm flow of a static base fine alignment method based on invariant filtering provided in an embodiment of this application.

[0051] Figure 3 This is a schematic diagram illustrating the process of updating the state information of the current iteration in a static base alignment method based on invariant filtering provided in an embodiment of this application.

[0052] Figure 4 : A schematic diagram of a static base precision alignment system based on invariant filtering provided in an embodiment of this application. Detailed Implementation

[0053] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0054] It should be noted that the step numbers in this document are only for the convenience of explaining the specific embodiments and are not intended to limit the order in which the steps are performed. In the description of this application, 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 technical features indicated. Therefore, a feature specified as "first" or "second" may explicitly or implicitly include one or more of that feature.

[0055] Throughout this manual, the static base described is a carrier that houses the inertial navigation system (INS) and remains stationary. The "n-system" referred to here is the n-coordinate system, also known as the local coordinate system. In the n-coordinate system, the y-axis points north along the tangent to the meridian, the x-axis points east along the tangent to the parallel, and the z-axis is determined by the right-hand rule from x and y. The precise alignment described here refers to a relatively accurate process of determining the initial attitude using the INS, i.e., a rotation from the local coordinate system to the INS coordinate system.

[0056] Example 1:

[0057] like Figure 1 As shown, Embodiment 1 provides a static base fine alignment method based on invariant filtering, including steps S1-S4:

[0058] Step S1: Obtain the initial state information and IMU dataset of the static base carrier;

[0059] Step S2: Divide the IMU dataset into several IMU data subsets based on a preset time interval;

[0060] Step S3: Based on the time sequence, the initial state information is iteratively updated several times using each subset of IMU data to obtain updated state information. In each iteration update process, the state information of the current iteration is mechanically arranged, updated, and Kalman filtered measurement is updated according to the current subset of IMU data to obtain updated state information. In the process of state update and Kalman filtered measurement update, invariant filtering is introduced.

[0061] Step S4: Output the updated status information as the alignment result.

[0062] This application provides a static base precision alignment method based on invariant filtering. First, IMU data is divided according to a preset time interval, allowing for multiple rounds of mechanical arrangement, state updates, and measurement updates during the static base precision alignment process. This enables iterative updates to the static base's state information. Compared to directly using all IMU data for precision alignment, this application effectively utilizes IMU data, allowing it to more accurately reflect the object's motion state and improving the accuracy of static base precision alignment. Furthermore, this application introduces invariant filtering for state and measurement updates during each iteration, mathematically avoiding the introduction of comparison forces. This preserves the unobservable characteristics of the originally unobservable state while achieving accurate attitude estimation, further improving the accuracy of static base precision alignment.

[0063] In a preferred embodiment, the system error state quantities during the alignment process comprise 12 dimensions, including 3-dimensional attitude error (roll, pitch, and yaw), 3-dimensional velocity error (eastward velocity, northward velocity, and zenith velocity), 3-dimensional gyro bias error, and 3-dimensional accelerator bias error. The initial attitude (i.e., the initial state information) is provided externally, for example, by obtaining the initial attitude through coarse alignment on a static base; in the initial attitude, the initial velocity, gyro bias, and accelerator bias are set to 0. The entire fine alignment algorithm flow is as follows: Figure 2 As shown, the initial attitude is first acquired; then, mechanical arrangement and state update of pure IMU data are performed for 5 seconds; a measurement update is performed; after the measurement update is completed, it is determined whether all the IMU data is used for alignment. If so, the alignment ends; otherwise, the next round of mechanical arrangement, state update and measurement update process begins.

[0064] Furthermore, in step S3, the state information of the current iteration is sequentially subjected to mechanical orchestration, state update, and Kalman filter measurement update based on the current IMU data subset to obtain the updated state information, such as... Figure 3 As shown, steps S301-S303 are included:

[0065] Step S301: Perform inertial navigation state recursion based on the current IMU data subset and the current iteration state information, and update to obtain the current estimated state information. The estimated state information includes the estimated attitude array, the estimated n-system velocity, the estimated gyroscope zero bias, and the estimated accelerometer zero bias.

[0066] Step S302: Based on the estimated attitude matrix and the estimated n-system velocity, update the current estimated system error state variables and state variance matrix using invariant filtering;

[0067] Step S303: Based on the estimated system error state quantity and the state variance matrix, perform Kalman filtering measurement update on the estimated state information to obtain the updated state information.

[0068] This application provides a method for updating state information. In mechanical orchestration, the current IMU data subset is used to perform inertial navigation state recursion on the state information of the current iteration, and the current estimated state information is initially calculated. Then, invariant filtering is introduced, and the current estimated system error state quantity and state variance matrix are updated based on the estimated state information. The estimated system error state quantity is used for subsequent compensation and correction of the estimated state information. However, the estimated system error state quantity is still not accurate enough at this time, and further calculation of the estimated system error state quantity is required through Kalman filter measurement update. The state variance matrix is ​​a necessary matrix in the Kalman filter measurement update process. The current estimated system error state quantity and state variance matrix are obtained through attitude estimation and n-system velocity estimation updates, preparing data for subsequent Kalman filter measurement updates.

[0069] In one possible implementation, step S301, which involves performing inertial navigation state recursion based on the current IMU data subset and the current iteration's state information to update and obtain the current estimated state information, includes:

[0070] Calculate the current estimated gyroscope zero bias and estimated accelerometer zero bias based on the current subset of IMU data;

[0071] The estimated attitude matrix is ​​updated by recursively performing attitude inference based on the angular velocity measurement values ​​in the current IMU data subset and the first attitude matrix in the current iteration state information;

[0072] The estimated n-system velocity is updated by recursively calculating the velocity based on the acceleration increment in the current IMU data subset and the first n-system velocity in the current iteration state information.

[0073] In this embodiment, gyroscope bias estimation, accelerometer bias estimation, attitude array estimation, and n-system velocity estimation are performed using the current IMU data subset, completing attitude recursion and velocity recursion, thus preparing data for subsequent static base precision alignment.

[0074] In a preferred embodiment, the specific formula for attitude recursion is as follows:

[0075]

[0076] In the formula, q represents the attitude quaternion, n represents the local coordinate system n, b represents the carrier system b, k+1 represents time k+1, and k represents the previous time k. and Let be the attitude quaternion at time k+1 and the attitude quaternion at time k, respectively. It is the rotation quaternion of the n-system at time k+1 to the n-system at time k. It is the rotation quaternion of system b at time k to system b at time k+1. and The calculation formula is as follows:

[0077]

[0078] Where RotvecToQuat is the rotation vector to quaternion. Let n be the rotational velocity of the n-frame relative to the inertial i-frame in the n-frame. dt is the IMU angular velocity measurement value at time k+1, and dt is the time interval between time k and time k+1.

[0079] After calculating the attitude quaternions, the attitude quaternions are further converted into the corresponding estimated attitude matrix.

[0080] The specific formula for the velocity recursion is as follows:

[0081]

[0082] Among them, v n(k+1) Let v be the velocity of the n-system at time k+1. n(k) Let n be the velocity of the system at time k. The specific velocity increment at time k+1 This represents the harmful velocity increment at time k+1.

[0083] In one possible implementation, step S302, which involves updating the current estimated system error state variables and state variance matrix based on the estimated attitude matrix and the estimated n-system velocity using an invariant filter, includes:

[0084] Construct a deterministic matrix and a noise-driven matrix based on the estimated attitude matrix and the estimated n-system velocity;

[0085] Construct a one-step matrix based on the deterministic matrix;

[0086] The first system error state quantity is updated according to the one-step matrix to obtain the estimated system error state quantity, wherein the first system error state quantity is constructed based on the first misalignment angle, first velocity error, first gyroscope zero bias error and first accelerometer zero bias error obtained after the last Kalman filter measurement update;

[0087] The first state variance matrix is ​​updated based on the noise driving matrix and the one-step matrix to obtain the state variance matrix, wherein the first state variance matrix is ​​the state variance matrix used in the previous Kalman filter measurement update.

[0088] In this embodiment, invariant filtering is introduced by constructing a deterministic matrix, a noise-driven matrix, and a one-step matrix, thereby updating the system error state variables and the state variance matrix. Since the first system error state variable includes Lie group states and non-Lie group states, this embodiment combines the system error state variable obtained after the previous round of Kalman filter measurement update with the current estimated attitude matrix and estimated n-system velocity for error recursion. The error recursion conforms to Lie group states and can be regarded as introducing invariant filtering. This makes the static base fine alignment process of this embodiment conform to the mathematical variance matrix update process without sacrificing or increasing observability, and can more accurately reflect the motion state of the object, better utilize the characteristics of the static base to complete fine alignment, and improve the accuracy of static base fine alignment.

[0089] In a preferred embodiment, the expression for the system error state quantity is:

[0090]

[0091] in, The value represents the misalignment angle, with subscripts e, n, and u representing the three directions. δv represents the velocity error under constant filtering, with subscripts e, n, and u representing the northeast-sky direction. δb g δb represents the zero bias error of the gyroscope. a This indicates zero bias of the accelerometer, with subscripts x, y, and z representing the x, y, and z axes of the load system. Furthermore, the formula for calculating δv is as follows:

[0092]

[0093] in, This represents the estimated velocity of the n-series. To estimate the attitude array, Let v be the transpose of the true attitude matrix. n This represents the actual speed.

[0094] In this embodiment, the system error state quantity is not directly calculated. Instead, it is updated based on the first system error state quantity obtained after the previous Kalman filter measurement update using right-invariant filtering to obtain the current estimated system error state quantity. When updating the system error state quantity and state variance matrix based on right-invariant filtering, a deterministic matrix, a noise-driven matrix, and a one-step matrix need to be constructed. Specifically, the deterministic matrix and noise-driven matrix are constructed based on the estimated attitude matrix and the estimated n-system velocity, using the following formula:

[0095]

[0096]

[0097] Among them, F k+1 Let g be the deterministic matrix at time k+1, skew is the notation for finding the antisymmetric matrix of a three-dimensional vector, and g is the deterministic matrix at time k+1. n Let I be the weight vector in the n-system, τ be the preset correlation time, and I be the weight vector in the n-system. 3×3 It is a 3x3 identity matrix. Let v be the estimated attitude matrix at time k+1. n(k+1) G is the estimated velocity of the n-system at time k+1. k+1 The noise-driven array at time k+1.

[0098] The specific formula for constructing a one-step matrix is:

[0099] Φ k+1 =I 12×12 +F k+1

[0100] Where, Φ k+1 For a one-step matrix, I 12×12 It is a 12x12 identity matrix.

[0101] The first state variance matrix is ​​updated based on the noise driving matrix and the one-step matrix to obtain the state variance matrix, and the specific formula is as follows:

[0102] x k+1 =Φ k+1 ·x k

[0103] The step of updating the first system error state quantity based on the one-step matrix to obtain the estimated system error state quantity is specifically formulated as follows:

[0104]

[0105] Where, x k+1 Let x be the estimated system error state variable at time k+1. k Let Φ be the first systematic error state quantity at time k. k+1 Let D be the matrix at time k+1. k+1 Let D be the state variance matrix at time k+1. k Let G be the state variance matrix at time k. k+1 Q is the noise-driven array at time k+1, and Q is the preset noise array, which is set by calibrating the IMU in advance.

[0106] In one possible implementation, in step S303, updating the estimated state information using Kalman filtering based on the estimated system error state quantity and the state variance matrix to obtain the updated state information includes:

[0107] Based on the state variance matrix, the estimated system error state quantity is updated by Kalman filtering to obtain the second system error state quantity. The second system error state quantity includes the second misalignment angle, the second velocity error, the second gyroscope zero bias error, and the second accelerometer zero bias error.

[0108] The second attitude matrix is ​​calculated based on the second misalignment angle and the estimated attitude matrix;

[0109] The second n-system velocity is calculated based on the second misalignment angle, the second velocity error, and the estimated n-system velocity.

[0110] The second gyroscope zero bias and the second accelerometer zero bias are calculated based on the second gyroscope zero bias error, the second accelerometer zero bias, the estimated gyroscope zero bias, and the estimated accelerometer zero bias.

[0111] The updated state information is obtained by combining the second attitude array, the second n-system velocity, the second gyroscope zero bias, and the second accelerometer zero bias.

[0112] This application provides a Kalman filter measurement update method. The basic idea of ​​a conventional Kalman filter algorithm is to use the state estimate from the previous moment and the observation from the current moment to obtain the optimal estimate of the state variables of a dynamic system at the current moment, generating a more accurate estimate of unknown variables than based solely on a single measurement. Therefore, it is particularly suitable for solving the measurement update problem in this application embodiment, improving the accuracy of static base alignment. In this application embodiment, based on the state variance matrix, the estimated system error state quantity is updated using standard Kalman filtering to obtain the updated second system error state quantity. After determining the second system error state quantity, the current estimated state information can be compensated based on the second system error state quantity to obtain the updated state information, thus realizing iterative updating of the state information.

[0113] In a preferred embodiment, the measurement update process is as follows:

[0114] Using zero velocity as the velocity measurement value for measurement updates, the measurement equation and measurement residual equation Z... n The structure is as follows:

[0115] Z n =[0I 3×3 00]·x

[0116] Z n =0 3×1 -v n(k+1)

[0117] The update process follows standard Kalman filtering. After obtaining the updated second systematic error state quantity, the state information of the current iteration is updated based on the second systematic error state quantity to obtain the updated state information, including the second attitude matrix, the second n-system velocity, the second gyroscope zero bias, and the second accelerometer zero bias. The specific formula is as follows:

[0118]

[0119] in, This represents the second attitude array after measurement update. Let represent the transpose of the attitude matrix obtained from the misalignment angle. This is the estimated attitude matrix at time k+1 obtained from mechanical arrangement. This indicates the updated second n-series velocity. Let δv be the estimated n-system velocity at time k+1 obtained from the mechanical arrangement, and let δv be the second velocity error obtained from the measurement update. This indicates the zero bias of the second gyroscope and the zero bias of the second accelerometer after the measurement update. These are the estimation of gyroscope bias and accelerometer bias, respectively, δb g δb a The measurement update is used to obtain the second gyroscope zero bias error and the second accelerometer zero bias error.

[0120] Example 2:

[0121] like Figure 4 As shown, correspondingly, Embodiment 2 provides a static base precision alignment system based on invariant filtering, including an acquisition module 10, a data partitioning module 20, an iterative update module 30, and an output module 40;

[0122] The acquisition module 10 is used to acquire the initial state information and IMU dataset of the static base carrier;

[0123] The data partitioning module 20 is used to divide the IMU dataset into several IMU data subsets based on a preset time interval;

[0124] The iterative update module 30 is used to iteratively update the initial state information several times based on the time sequence using each subset of IMU data to obtain updated state information. In each iterative update process, the state information of the current iteration is mechanically arranged, updated, and Kalman filtered measurement is sequentially performed according to the current subset of IMU data to obtain updated state information. Invariant filtering is introduced in the process of state update and Kalman filtered measurement update.

[0125] The output module 40 is used to output the updated status information as a fine alignment result.

[0126] Furthermore, the iterative update module 30 sequentially performs mechanical orchestration, state update, and Kalman filter measurement update on the current iteration's state information based on the current IMU data subset to obtain the updated state information, including:

[0127] Based on the current IMU data subset and the current iteration state information, the inertial navigation state is recursively calculated, and the current estimated state information is updated to obtain the estimated state information, which includes the estimated attitude matrix, the estimated n-system velocity, the estimated gyroscope zero bias, and the estimated accelerometer zero bias.

[0128] Based on the estimated attitude matrix and the estimated n-system velocity, the current estimated system error state variables and state variance matrix are obtained by updating based on invariant filtering.

[0129] Based on the estimated system error state quantity and the state variance matrix, the estimated state information is updated by Kalman filtering to obtain the updated state information.

[0130] In one possible implementation, the step of performing inertial navigation state recursion based on the current IMU data subset and the current iteration's state information to update and obtain the current estimated state information includes:

[0131] Calculate the current estimated gyroscope zero bias and estimated accelerometer zero bias based on the current subset of IMU data;

[0132] The estimated attitude matrix is ​​updated by recursively performing attitude inference based on the angular velocity measurement values ​​in the current IMU data subset and the first attitude matrix in the current iteration state information;

[0133] The estimated n-system velocity is updated by recursively calculating the velocity based on the acceleration increment in the current IMU data subset and the first n-system velocity in the current iteration state information.

[0134] In one possible implementation, the step of updating the current estimated system error state variables and state variance matrix based on the estimated attitude matrix and the estimated n-system velocity using invariant filtering includes:

[0135] Construct a deterministic matrix and a noise-driven matrix based on the estimated attitude matrix and the estimated n-system velocity;

[0136] Construct a one-step matrix based on the deterministic matrix;

[0137] The first system error state quantity is updated according to the one-step matrix to obtain the estimated system error state quantity, wherein the first system error state quantity is constructed based on the first misalignment angle, first velocity error, first gyroscope zero bias error and first accelerometer zero bias error obtained after the last Kalman filter measurement update;

[0138] The first state variance matrix is ​​updated based on the noise driving matrix and the one-step matrix to obtain the state variance matrix, wherein the first state variance matrix is ​​the state variance matrix used in the previous Kalman filter measurement update.

[0139] In one possible implementation, updating the estimated state information using Kalman filtering measurements based on the estimated system error state quantity and the state variance matrix to obtain the updated state information includes:

[0140] Based on the state variance matrix, the estimated system error state quantity is updated by Kalman filtering to obtain the second system error state quantity. The second system error state quantity includes the second misalignment angle, the second velocity error, the second gyroscope zero bias error, and the second accelerometer zero bias error.

[0141] The second attitude matrix is ​​calculated based on the second misalignment angle and the estimated attitude matrix;

[0142] The second n-system velocity is calculated based on the second misalignment angle, the second velocity error, and the estimated n-system velocity.

[0143] The second gyroscope zero bias and the second accelerometer zero bias are calculated based on the second gyroscope zero bias error, the second accelerometer zero bias, the estimated gyroscope zero bias, and the estimated accelerometer zero bias.

[0144] The updated state information is obtained by combining the second attitude array, the second n-system velocity, the second gyroscope zero bias, and the second accelerometer zero bias.

[0145] This application provides a static base precision alignment system based on invariant filtering. First, IMU data is divided according to a preset time interval, allowing for multiple rounds of mechanical arrangement, state updates, and measurement updates during static base precision alignment. This enables iterative updates to the static base's state information. Compared to directly using all IMU data for precision alignment, this application effectively utilizes IMU data, allowing it to more accurately reflect the object's motion state and improving the accuracy of static base precision alignment. Furthermore, this application introduces invariant filtering for state and measurement updates during each iteration, mathematically avoiding the introduction of comparison forces. This preserves the unobservable characteristics of the originally unobservable state while achieving accurate attitude estimation, further improving the accuracy of static base precision alignment.

[0146] For a more detailed explanation of the working principle and procedures of this embodiment, please refer to the relevant description in Embodiment 1.

[0147] Example 3:

[0148] Embodiment 3 provides a terminal including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements the static base fine alignment method based on invariant filtering.

[0149] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. The processor is the control center of the terminal, connecting various parts of the terminal via various interfaces and lines.

[0150] The memory can be used to store the computer program. The processor implements various functions of the terminal by running or executing the computer program stored in the memory and calling data stored in the memory. The memory may mainly include a program storage area and a data storage area. The program storage area may store the operating system, at least one application program required for a function (such as sound playback function, image playback function, etc.), etc.; the data storage area may store data created according to the use of the mobile phone (such as audio data, phonebook, etc.). In addition, the memory may include high-speed random access memory, and may also include non-volatile memory, such as hard disk, memory, plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, at least one disk storage device, flash memory device, or other volatile solid-state storage device.

[0151] Example 4:

[0152] Example 4 provides a computer-readable storage medium including a stored computer program, wherein the computer program, when running, controls the device where the computer-readable storage medium is located to execute the static base fine alignment method based on invariant filtering.

[0153] The module integrated in the RIS reflection-based positioning method, if implemented as a software functional unit and sold or used as an independent product, can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the above embodiments of the present invention can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc.

[0154] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of this application. It should be understood that the above descriptions are merely specific embodiments of this application and are not intended to limit the scope of protection of this application. In particular, it should be noted that any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application for those skilled in the art.

Claims

1. A method for inertial filter based stationary base fine alignment, the method comprising: The method comprises the following steps: obtaining initial state information and an IMU data set of a static base carrier; dividing the IMU data set into a plurality of IMU data subsets based on a preset time interval; sequentially performing a plurality of times of iterative updating on the initial state information based on the plurality of IMU data subsets in a time sequence, and obtaining updated state information, wherein, in each time of iterative updating, sequentially performing mechanical arrangement, state updating and Kalman filter measurement updating on state information of a current iteration based on a current IMU data subset, and obtaining updated state information, wherein, in the process of state updating and Kalman filter measurement updating, introducing invariant filtering; the sequentially performing mechanical arrangement, state updating and Kalman filter measurement updating on the state information of the current iteration based on the current IMU data subset, and obtaining the updated state information, comprises: performing inertial navigation state recursion on the current IMU data subset and the state information of the current iteration, and obtaining current estimated state information, wherein the estimated state information comprises an estimated attitude matrix, an estimated n-system velocity, an estimated gyro zero bias and an estimated accelerometer zero bias; based on the invariant filtering, updating the estimated attitude matrix and the estimated n-system velocity to obtain current estimated system error state quantity and state variance matrix; performing Kalman filter measurement updating on the estimated state information based on the estimated system error state quantity and the state variance matrix, and obtaining the updated state information; the performing inertial navigation state recursion on the current IMU data subset and the state information of the current iteration, and obtaining the current estimated state information, comprises: calculating the current estimated gyro zero bias and the estimated accelerometer zero bias based on the current IMU data subset; performing attitude recursion on an angular velocity measurement value in the current IMU data subset and a first attitude matrix in the state information of the current iteration, and updating the estimated attitude matrix; performing velocity recursion on an acceleration increment in the current IMU data subset and a first n-system velocity in the state information of the current iteration, and updating the estimated n-system velocity; outputting the updated state information as a fine alignment result.

2. The invariant filtering based stationary base fine alignment method of claim 1, wherein, the updating the estimated attitude matrix and the estimated n-system velocity based on the invariant filtering to obtain the current estimated system error state quantity and the state variance matrix, comprises: constructing a deterministic matrix and a noise driving matrix based on the estimated attitude matrix and the estimated n-system velocity; constructing a one-step matrix based on the deterministic matrix; updating a first system error state quantity based on the one-step matrix to obtain the estimated system error state quantity, wherein the first system error state quantity is constructed based on a first misalignment angle, a first velocity error, a first gyro zero bias error quantity and a first accelerometer zero bias error quantity obtained after a previous Kalman filter measurement updating; updating a first state variance matrix based on the noise driving matrix and the one-step matrix to obtain the state variance matrix, wherein the first state variance matrix is a state variance matrix used in the previous Kalman filter measurement updating.

3. A stationary base fine alignment method based on invariant filtering as claimed in claim 2, wherein, The deterministic matrix and the noise driving matrix are constructed according to the estimated attitude matrix and the estimated n-system velocity, and the specific formula is as follows: where F k+1 is the deterministic matrix at time k+1, skew is the symbol for the skew-symmetric matrix of three-dimensional vectors, g n is the weight vector under n, τ is a predetermined correlation time, I 3×3 is a 3 by 3 identity matrix, is the estimated attitude matrix at time k+1, v n(k+1) is the estimated n velocity at time k+1, G k+1 is the noise driven matrix at time k+1.

4. The invariant filtering based stationary base fine alignment method of claim 2, wherein, The first state variance matrix is updated according to the noise driving matrix and the one-step matrix to obtain the state variance matrix, and the specific formula is as follows: x k+1 = Φ k+1 • x k The first system error state quantity is updated according to the one-step matrix to obtain the estimated system error state quantity, and the specific formula is as follows: wherein x k+1 is the estimated system error state quantity at k+1 time, x k is the first system error state quantity at k time, Φ k+1 is a one-step matrix at k+1 time, D k+1 is the state variance matrix at k+1 time, D k is the state variance matrix at k time, G k+1 is the noise driving matrix at k+1 time, and Q is a preset noise matrix.

5. The invariant filtering based stationary base fine alignment method of claim 1, wherein, The estimated state information is Kalman filter measurement updated according to the estimated system error state quantity and the state variance matrix to obtain the updated state information, including: The estimated system error state quantity is Kalman filter measurement updated based on the state variance matrix to obtain a second system error state quantity, and the second system error state quantity includes a second misalignment angle, a second velocity error, a second gyro zero bias error quantity and a second accelerometer zero bias error quantity; A second attitude matrix is calculated according to the second misalignment angle and the estimated attitude matrix; A second n-system velocity is calculated according to the second misalignment angle, the second velocity error and the estimated n-system velocity; A second gyro zero bias and a second accelerometer zero bias are calculated according to the second gyro zero bias error quantity, the second accelerometer zero bias error quantity, the estimated gyro zero bias and the estimated accelerometer zero bias; The updated state information is constructed according to the second attitude matrix, the second n-system velocity, the second gyro zero bias and the second accelerometer zero bias.

6. An invariant filtering based stationary base fine alignment system, characterized by, It includes an acquisition module, a data division module, an iterative update module and an output module. The acquisition module is used to acquire the initial state information and the IMU data set of the static base carrier. The data division module is used to divide the IMU data set into a plurality of IMU data subsets based on a preset time interval. The iterative update module is used to sequentially update the initial state information a plurality of times based on the time sequence using the plurality of IMU data subsets to obtain updated state information, wherein in each iterative update process, the current iteration state information is sequentially subjected to mechanical arrangement, state update and Kalman filter measurement update according to the current IMU data subset to obtain updated state information, wherein the invariant filter is introduced in the process of state update and Kalman filter measurement update; The current iteration state information is sequentially subjected to mechanical arrangement, state update and Kalman filter measurement update according to the current IMU data subset to obtain updated state information, including: the current estimated state information is updated according to the current IMU data subset and the current iteration state information, and the estimated state information includes an estimated attitude matrix, an estimated n-system velocity, an estimated gyro zero bias and an estimated accelerometer zero bias; the current estimated system error state quantity and the state variance matrix are updated based on the invariant filter update according to the estimated attitude matrix and the estimated n-system velocity; the estimated state information is Kalman filter measurement updated according to the estimated system error state quantity and the state variance matrix to obtain the updated state information; The inertial navigation state recursion according to the current IMU data subset and the state information of the current iteration comprises: calculating a current estimated gyroscope zero bias and an estimated accelerometer zero bias according to the current IMU data subset; performing attitude recursion according to an angular velocity measurement in the current IMU data subset and a first attitude matrix in the state information of the current iteration to update the estimated attitude matrix; and performing velocity recursion according to an acceleration increment in the current IMU data subset and a first n-system velocity in the state information of the current iteration to update the estimated n-system velocity. The output module is configured to output the updated state information as a fine alignment result.

7. A terminal, characterized by comprising: A computer program product comprising a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the computer program, when executed by the processor, implements the inertial filter-based fine alignment method according to any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium comprises a stored computer program, wherein the computer readable storage medium controls a device in which the computer readable storage medium is located to execute the inertial filter-based fine alignment method according to any one of claims 1 to 5 when the computer program is executed.

Images