Static base fine alignment method and system based on invariant filtering, terminal and storage medium

Abstract

The invention provides a static base fine alignment method and system based on invariant filtering, a terminal and a storage medium. The method comprises the steps of 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; on the basis of a time sequence, the IMU data subsets are sequentially used for conducting iteration updating on the initial state information for a plurality of times, updated state information is obtained, in each iteration updating process, mechanical arrangement, state updating and Kalman filtering measurement updating are sequentially conducted on the state information of current iteration according to the current IMU data subset, and the state information of the current iteration is obtained. The updated state information is obtained, and 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. The accuracy of fine alignment of the static base is improved.

Description

Technical Field

[0001] The present application relates to the field of inertial navigation technology, and in particular to a static base precision alignment method, system, terminal and storage medium based on invariant filtering. Background Art

[0002] The static base alignment is usually divided into two steps. The first step is the rough alignment of the static base. The analytical rough alignment is used to solve the initial rotation array using two non-coplanar vectors - angular velocity measurement and acceleration measurement. Its essence is to determine the inertial navigation orientation by sensing the earth's rotation and gravity vector. The second step is the fine alignment of the static base. The rotation array obtained by the rough alignment is used as the initial attitude for the fine alignment. Based on the characteristic that the static base carrier speed is basically zero, the attitude angle error is inferred from the velocity error.

[0003] In the existing static base precision alignment, the specific force is usually directly approximated as gravity in the update equation, thus introducing an approximation error related to the attitude. Replacing the specific force with the local gravity vector also reduces the observability of the attitude, and the effect of further refining the initial attitude is not ideal. If the specific force is not approximated in the update equation, since vibration is usually inevitable when the carrier is stationary, this influence will introduce some erroneous additional observability, such as introducing the observation of originally unobservable states such as zero bias, which will also lead to unsatisfactory effect of further refining the initial attitude. Summary of the invention

[0004] In response to the above technical problems, the present application provides a static base precision alignment method, system, terminal and storage medium based on invariant filtering to improve the accuracy of static base precision alignment.

[0005] In a first aspect, an embodiment of the present application provides a static base fine alignment method based on invariant filtering, comprising:

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

[0007] Dividing the IMU data set into a plurality of 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 the respective IMU data subsets in sequence to obtain updated state information, wherein in each iterative update process, the state information of the current iteration is mechanically arranged, state updated, and Kalman filter measurement updated in sequence according to the current IMU data subset to obtain updated state information, wherein invariant filtering is introduced in the process of the state update and Kalman filter measurement update;

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

[0010] The embodiment of the present application provides a static base precision alignment method based on invariant filtering. First, the IMU data is divided according to a preset time interval, so that multiple rounds of mechanical arrangement, state update and measurement update can be performed during the static base precision alignment process, and then the state information of the static base can be gradually updated multiple times in an iterative update manner. Compared with directly using all IMU data for precision alignment, the embodiment of the present application can effectively utilize the IMU data, so that the IMU data can more accurately reflect the motion state of the object and improve the accuracy of the static base precision alignment. At the same time, the embodiment of the present application introduces the introduction of invariant filtering for state update and measurement update in the process of each iterative update, which can reasonably avoid the introduction of specific force from a mathematical perspective, ensure the unobservable characteristics of the originally unobservable state, and realize the correct estimation of the posture, further improving the accuracy of the static base precision alignment.

[0011] Furthermore, the state information of the current iteration is mechanically arranged, updated, and Kalman filtered in sequence according to the current IMU data subset to obtain updated state information, including:

[0012] Performing inertial navigation state recursion according to the current IMU data subset and the current iteration state information, updating and obtaining current estimated state information, wherein the estimated state information includes an estimated attitude matrix, an estimated n-frame velocity, an estimated gyro bias, and an estimated accelerometer bias;

[0013] According to the estimated attitude matrix and the estimated n-system velocity, obtaining the current estimated system error state quantity and state variance matrix based on invariant filtering update;

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

[0015] The embodiment of the present application provides a method for updating state information. In mechanical arrangement, 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 preliminarily calculated. Then, an invariant filter is introduced, and the current estimated system error state quantity and state variance matrix are obtained according to the estimated state information update, wherein the estimated system error state quantity is used for subsequent compensation and correction of the estimated state information, but the estimated system error state quantity at this time is still not accurate enough, and the estimated system error state quantity needs to be further calculated through Kalman filter measurement update. The state variance matrix is ​​a necessary matrix in the Kalman filter measurement update process, and the current estimated system error state quantity and state variance matrix are obtained through the estimated attitude matrix and the estimated n-system velocity update, so as to prepare data for the subsequent Kalman filter measurement update.

[0016] In a possible implementation, performing inertial navigation state recursion according to the current IMU data subset and the current iteration state information to update and obtain the current estimated state information includes:

[0017] Calculating a current estimated gyro bias and an estimated accelerometer bias based on the current IMU data subset;

[0018] Performing attitude recursion according to the angular velocity measurement value in the current IMU data subset and the first attitude matrix in the state information of the current iteration, and updating to obtain the estimated attitude matrix;

[0019] Velocity recursion is performed according to the acceleration increment in the current IMU data subset and the first n-system velocity in the state information of the current iteration, so as to update and obtain the estimated n-system velocity.

[0020] In an embodiment of the present application, the current IMU data subset is used to perform gyro bias estimation, accelerometer bias estimation, attitude array estimation and n-system velocity estimation, respectively, completing attitude recursion and velocity recursion, and preparing data for subsequent static base precision alignment.

[0021] In a possible implementation, the obtaining of the current estimated system error state quantity and state variance matrix based on the estimated attitude matrix and the estimated n-system velocity based on invariant filtering update includes:

[0022] Constructing a deterministic matrix and a noise driving matrix according to the estimated attitude matrix and the estimated n-system velocity;

[0023] constructing a one-step matrix according to 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, the first velocity error, the first gyro bias error quantity and the first accelerometer bias error quantity obtained after the last Kalman filter measurement update;

[0025] The first state variance matrix is ​​updated according to 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 last Kalman filter measurement update.

[0026] In the embodiment of the present application, an invariant filter is introduced by constructing a deterministic matrix, a noise driving matrix, and a one-step matrix, thereby realizing the update of the system error state quantity and the state variance matrix. Since the first system error state quantity contains Lie group states and non-Lie group states, the embodiment of the present application combines the system error state quantity obtained after the last round of Kalman filter measurement update and the current estimated attitude matrix and the estimated n-system velocity for error recursion. The error recursion conforms to the Lie group state and can be regarded as introducing an invariant filter, so that the static base precision alignment process of the embodiment of the present application conforms 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 the precision alignment, and improve the accuracy of the static base precision alignment.

[0027] Furthermore, 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:

[0028]

[0029] Among them, F k+1 is the deterministic matrix at time k+1, skew is the symbol of the antisymmetric matrix of the three-dimensional vector, g n is the weight vector under the n system, τ is the preset correlation time, I 3×3 is the 3 by 3 identity matrix, is the estimated attitude matrix at time k+1, v n(k+1) is the estimated n-system velocity at time k+1, G k+1 is the noise driving array at time k+1.

[0030] The first state variance matrix is ​​updated according to the noise driving matrix and the one-step matrix to obtain the state variance matrix. The specific formula is:

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

[0032] 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:

[0033]

[0034] Among them, x k+1 is the estimated system error state at time k+1, x k is the first system error state quantity at time k, Φ k+1 is the one-step matrix at time k+1, D k+1 is the state variance matrix at time k+1, D k is the state variance matrix at time k, Gk+1 is the noise driving matrix at time k+1, and Q is the preset noise matrix.

[0035] In a possible implementation, performing Kalman filter measurement update on the estimated state information according to 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, performing Kalman filter measurement update on the estimated system error state quantity to obtain a second system error state quantity, wherein the second system error state quantity includes a second misalignment angle, a second velocity error, a second gyro bias error quantity, and a second accelerometer bias error quantity;

[0037] Obtaining a second attitude matrix by calculation according to the second misalignment angle and the estimated attitude matrix;

[0038] Obtain a second n-frame velocity by calculation according to the second misalignment angle, the second velocity error and the estimated n-frame velocity;

[0039] Calculate and obtain a second gyro bias and a second accelerometer bias according to the second gyro bias error, the second accelerometer bias error, the estimated gyro bias, and the estimated accelerometer bias;

[0040] The updated state information is constructed by combining the second attitude array, the second n-frame velocity, the second gyro bias and the second accelerometer bias.

[0041] The embodiment of the present application provides a Kalman filter measurement update method. Since the basic idea of ​​the conventional Kalman filter algorithm is to use the state estimate value at the previous moment and the observation value at the current moment to obtain the optimal estimate of the state variable of the dynamic system at the current moment, and produce an estimate of the unknown variable that is more accurate than that based on a single measurement value alone, it is particularly suitable for solving the measurement update problem in the embodiment of the present application and improving the accuracy of the static base alignment. In the embodiment of the present application, based on the state variance matrix, the estimated system error state quantity is measured and updated according to the standard Kalman filter to obtain an updated second system error state quantity. After determining the second system error state quantity, the current estimated state information can be compensated according to the second system error state quantity to obtain the updated state information, thereby realizing the iterative update of the state information.

[0042] In a second aspect, accordingly, an embodiment of the present application provides a static base precision alignment system based on invariant filtering, including an acquisition module, a data division module, an iterative update module, and an output module;

[0043] Wherein, the acquisition module is used to obtain the initial state information and IMU data set of the static base carrier;

[0044] The data partitioning module is used to divide the IMU data set into a plurality of 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 the time sequence, using the respective IMU data subsets in sequence, to obtain updated state information, wherein in each iterative update process, mechanical arrangement, state update and Kalman filter measurement update are performed on the state information of the current iteration in sequence according to the current IMU data subset, to obtain updated state information, wherein invariant filtering is introduced in the process of the state update and Kalman filter measurement update;

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

[0047] In a third aspect, an embodiment of the present application provides a terminal, comprising a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor implements the static base precise alignment method based on invariant filtering when executing the computer program.

[0048] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium includes a stored computer program, wherein when the computer program is running, the device where the computer-readable storage medium is located is controlled to execute the static base precision alignment method based on invariant filtering. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 : A flow chart of a static base precision alignment method based on invariant filtering provided in an embodiment of the present application.

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

[0051] Figure 3 : A schematic diagram of the process of updating the status information of the current iteration in a static base precision alignment method based on invariant filtering provided in an embodiment of the present application.

[0052] Figure 4 : A structural schematic diagram of a static base precision alignment system based on invariant filtering provided in an embodiment of the present application. DETAILED DESCRIPTION

[0053] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0054] It should be noted that the step numbers in the text are only for the convenience of explanation of the specific embodiments and do not serve to limit the order in which the steps are executed. In the description of this application, the terms "first" and "second" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features.

[0055] Throughout the specification, the static base described in this specification is a carrier that carries an inertial navigation device and is in a stationary state. The n-system described in this specification is the full name of the n-system coordinate system, also known as the local coordinate system. In the n-system coordinate system, the y-axis direction points to the north along the tangent of the meridian circle, the x-axis direction points to the east along the tangent of the latitude circle, and the z-axis direction is obtained by the right-hand rule of x and y. The precise alignment described in this specification is a relatively accurate process of determining the initial attitude by the inertial navigation, that is, the process of rotating the local coordinate system to the inertial navigation coordinate system.

[0056] Embodiment 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, obtaining the initial state information and IMU data set of the stationary base carrier;

[0059] Step S2, dividing the IMU data set into a plurality of IMU data subsets based on a preset time interval;

[0060] Step S3, based on the time sequence, sequentially use the respective IMU data subsets to perform several iteration updates on the initial state information to obtain updated state information, wherein in each iteration update process, mechanical arrangement, state update and Kalman filter measurement update are sequentially performed on the state information of the current iteration according to the current IMU data subset to obtain updated state information, wherein invariant filtering is introduced in the process of state update and Kalman filter measurement update;

[0061] Step S4: output the update status information as a precise alignment result.

[0062] The embodiment of the present application provides a static base precision alignment method based on invariant filtering. First, the IMU data is divided according to a preset time interval, so that multiple rounds of mechanical arrangement, state update and measurement update can be performed during the static base precision alignment process, and then the state information of the static base can be gradually updated multiple times in an iterative update manner. Compared with directly using all IMU data for precision alignment, the embodiment of the present application can effectively utilize the IMU data, so that the IMU data can more accurately reflect the motion state of the object and improve the accuracy of the static base precision alignment. At the same time, the embodiment of the present application introduces the introduction of invariant filtering for state update and measurement update in the process of each iterative update, which can reasonably avoid the introduction of specific force from a mathematical perspective, ensure the unobservable characteristics of the originally unobservable state, and realize the correct estimation of the posture, further improving the accuracy of the static base precision alignment.

[0063] In a preferred embodiment, the system error state quantity in the alignment process has 12 dimensions, including 3 dimensions of attitude error (roll, pitch and heading), 3 dimensions of velocity error (east velocity, north velocity and zenith velocity), 3 dimensions of gyro bias error and 3 dimensions of additive bias error. The initial attitude (i.e., the initial state information) is provided by the outside world, for example, by obtaining the initial attitude through rough alignment of a static base; in the initial attitude, the initial velocity, gyro bias and additive bias are set to 0. The entire fine alignment algorithm process is as follows: Figure 2 As shown, first obtain the initial posture; then perform mechanical arrangement and status update of pure IMU data for 5 seconds; perform a measurement update; after the measurement update, determine whether all IMU data are used for alignment, if so, end the alignment, otherwise enter the next round of mechanical arrangement, status update and measurement update process.

[0064] Further, in step S3, the state information of the current iteration is mechanically arranged, updated, and Kalman filtered in sequence according to the current IMU data subset to obtain updated state information, such as Figure 3 As shown, steps S301-S303 are included:

[0065] Step S301, performing inertial navigation state recursion according to the current IMU data subset and the current iteration state information, updating and obtaining current estimated state information, wherein the estimated state information includes an estimated attitude matrix, an estimated n-frame velocity, an estimated gyro bias, and an estimated accelerometer bias;

[0066] Step S302, obtaining the current estimated system error state quantity and state variance matrix based on the estimated attitude matrix and the estimated n-system velocity based on invariant filtering update;

[0067] Step S303: performing Kalman filter measurement update on the estimated state information according to the estimated system error state quantity and the state variance matrix to obtain the updated state information.

[0068] The embodiment of the present application provides a method for updating state information. In mechanical arrangement, 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 preliminarily calculated. Then, an invariant filter is introduced, and the current estimated system error state quantity and state variance matrix are obtained according to the estimated state information update, wherein the estimated system error state quantity is used for subsequent compensation and correction of the estimated state information, but the estimated system error state quantity at this time is still not accurate enough, and the estimated system error state quantity needs to be further calculated through Kalman filter measurement update. The state variance matrix is ​​a necessary matrix in the Kalman filter measurement update process, and the current estimated system error state quantity and state variance matrix are obtained through the estimated attitude matrix and the estimated n-system velocity update, so as to prepare data for the subsequent Kalman filter measurement update.

[0069] In a possible implementation, in step S301, performing inertial navigation state recursion according to the current IMU data subset and the current iteration state information to update and obtain the current estimated state information includes:

[0070] Calculating a current estimated gyro bias and an estimated accelerometer bias based on the current IMU data subset;

[0071] Performing attitude recursion according to the angular velocity measurement value in the current IMU data subset and the first attitude matrix in the state information of the current iteration, and updating to obtain the estimated attitude matrix;

[0072] Velocity recursion is performed according to the acceleration increment in the current IMU data subset and the first n-system velocity in the state information of the current iteration, so as to update and obtain the estimated n-system velocity.

[0073] In an embodiment of the present application, the current IMU data subset is used to perform gyro bias estimation, accelerometer bias estimation, attitude array estimation and n-system velocity estimation, respectively, completing attitude recursion and velocity recursion, and preparing data for subsequent static base precision alignment.

[0074] In a preferred embodiment, the specific formula of the posture recursion is:

[0075]

[0076] Where q represents the attitude quaternion, n represents the local coordinate system n, b represents the carrier system b, k+1 represents the k+1 moment, and k represents the previous moment k. and They are the attitude quaternion at time k+1 and the attitude quaternion at time k, It is the rotation quaternion from system n at time k+1 to system n at time k. is the rotation quaternion from system b at time k to system b at time k+1, and The calculation formula is as follows:

[0077]

[0078] Among them, RotvecToQuat is the conversion of rotation vector to quaternion. is the value of the rotation speed of system n relative to the inertial system i in system n, 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 the attitude quaternion is calculated, the attitude quaternion is further converted into the corresponding estimated attitude matrix.

[0080] The specific formula for the speed recursion is:

[0081]

[0082] Among them, v n(k+1) is the velocity of the n-system at time k+1, v n(k) is the speed of the n-system at time k, is the specific velocity increment at time k+1, is the harmful velocity increment at time k+1.

[0083] In a possible implementation, in step S302, obtaining the current estimated system error state and state variance matrix based on the estimated attitude matrix and the estimated n-system velocity based on invariant filtering update includes:

[0084] Constructing a deterministic matrix and a noise driving matrix according to the estimated attitude matrix and the estimated n-system velocity;

[0085] constructing a one-step matrix according to 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, the first velocity error, the first gyro bias error quantity and the first accelerometer bias error quantity obtained after the last Kalman filter measurement update;

[0087] The first state variance matrix is ​​updated according to 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 last Kalman filter measurement update.

[0088] In the embodiment of the present application, an invariant filter is introduced by constructing a deterministic matrix, a noise driving matrix, and a one-step matrix, thereby realizing the update of the system error state quantity and the state variance matrix. Since the first system error state quantity contains Lie group states and non-Lie group states, the embodiment of the present application combines the system error state quantity obtained after the last round of Kalman filter measurement update and the current estimated attitude matrix and the estimated n-system velocity for error recursion. The error recursion conforms to the Lie group state and can be regarded as introducing an invariant filter, so that the static base precision alignment process of the embodiment of the present application conforms 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 the precision alignment, and improve the accuracy of the static base precision alignment.

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

[0090]

[0091] in, represents the misalignment angle, and the subscripts e, n, and u represent the three directions. δv is the velocity error under the constant filter, and the subscripts e, n, and u represent the northeast sky direction. δb g Denotes the gyro bias error, δb a represents the accelerometer zero bias, and the subscripts x, y, and z represent the x, y, and z axes of the load system. Further, the calculation formula of δv is as follows:

[0092]

[0093] in, represents the estimated n-system velocity, To estimate the attitude matrix, is the transposed matrix of the true attitude matrix, v n The real speed.

[0094] In the embodiment of the present application, the system error state quantity is not directly calculated, but the first system error state quantity obtained after the last Kalman filter measurement update is updated based on the invariant filter to obtain the current estimated system error state quantity. When updating the system error state quantity and the state variance matrix based on the right invariant filter, it is necessary to construct a deterministic matrix, a noise drive matrix and a one-step matrix, wherein the deterministic matrix and the noise drive matrix are constructed according to the estimated attitude matrix and the estimated n-system velocity, and the specific formula is:

[0095]

[0096]

[0097] Among them, F k+1 is the deterministic matrix at time k+1, skew is the symbol of the antisymmetric matrix of the three-dimensional vector, g n is the weight vector under the n system, τ is the preset correlation time, I 3×3 is the 3 by 3 identity matrix, is the estimated attitude matrix at time k+1, v n(k+1) is the estimated n-system velocity at time k+1, G k+1 is the noise driving 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] Among them, Φ k+1 is a one-step matrix, I 12×12 It is a 12 by 12 unit matrix.

[0101] The first state variance matrix is ​​updated according to the noise driving matrix and the one-step matrix to obtain the state variance matrix. The specific formula is:

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

[0103] 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:

[0104]

[0105] Among them, x k+1 is the estimated system error state at time k+1, x k is the first system error state quantity at time k, Φ k+1 is the one-step matrix at time k+1, D k+1 is the state variance matrix at time k+1, D k is the state variance matrix at time k, G k+1 is the noise driving matrix at time k+1, Q is the preset noise matrix, and the value is set by calibrating the IMU in advance.

[0106] In a possible implementation, in step S303, performing Kalman filter measurement update on the estimated state information according to 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, performing Kalman filter measurement update on the estimated system error state quantity to obtain a second system error state quantity, wherein the second system error state quantity includes a second misalignment angle, a second velocity error, a second gyro bias error quantity, and a second accelerometer bias error quantity;

[0108] Obtaining a second attitude matrix by calculation according to the second misalignment angle and the estimated attitude matrix;

[0109] Obtain a second n-frame velocity by calculation according to the second misalignment angle, the second velocity error and the estimated n-frame velocity;

[0110] Calculate and obtain a second gyro bias and a second accelerometer bias according to the second gyro bias error, the second accelerometer bias error, the estimated gyro bias, and the estimated accelerometer bias;

[0111] The updated state information is constructed by combining the second attitude array, the second n-frame velocity, the second gyro bias and the second accelerometer bias.

[0112] The embodiment of the present application provides a Kalman filter measurement update method. Since the basic idea of ​​the conventional Kalman filter algorithm is to use the state estimate value at the previous moment and the observation value at the current moment to obtain the optimal estimate of the state variable of the dynamic system at the current moment, and produce an estimate of the unknown variable that is more accurate than that based on a single measurement value alone, it is particularly suitable for solving the measurement update problem in the embodiment of the present application and improving the accuracy of the static base alignment. In the embodiment of the present application, based on the state variance matrix, the estimated system error state quantity is measured and updated according to the standard Kalman filter to obtain an updated second system error state quantity. After determining the second system error state quantity, the current estimated state information can be compensated according to the second system error state quantity to obtain the updated state information, thereby realizing the iterative update of the state information.

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

[0114] The zero speed is used as the speed measurement value for measurement update, and the measurement equation and measurement residual formula Z n The construction of is as follows:

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

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

[0117] The updating process is performed according to the standard Kalman filter. After obtaining the updated second system error state quantity, the state information of the current iteration is updated according to the second system error state quantity to obtain the updated state information, including the second attitude array, the second n-system velocity, the second gyro bias and the second accelerometer bias. The specific formula is as follows:

[0118]

[0119] in, Represents the second attitude matrix after measurement update, represents the transposed matrix of the attitude matrix obtained from the misalignment angle, is the estimated attitude matrix at time k+1 obtained by mechanical arrangement, Indicates the second n-series speed after measurement update, is the estimated n-system velocity at time k+1 obtained by mechanical arrangement, δv is the second velocity error obtained by measurement update, Indicates the measurement of the updated second gyro bias and second accelerometer bias. are the estimated gyro bias and the estimated accelerometer bias, δb g ,δb a The second gyro bias error and the second accelerometer bias error are measured and updated.

[0120] Embodiment 2:

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

[0122] Wherein, the acquisition module 10 is used to acquire the initial state information and IMU data set of the static base carrier;

[0123] The data partitioning module 20 is used to partition the IMU data set into a plurality of IMU data subsets based on a preset time interval;

[0124] The iterative update module 30 is used to perform several iterative updates on the initial state information based on the time sequence, using the respective IMU data subsets in sequence, to obtain updated state information, wherein in each iterative update process, mechanical arrangement, state update and Kalman filter measurement update are performed on the state information of the current iteration in sequence according to the current IMU data subset, to obtain updated state information, wherein invariant filtering is introduced in the process of the state update and Kalman filter measurement update;

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

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

[0127] Performing inertial navigation state recursion according to the current IMU data subset and the current iteration state information, updating and obtaining current estimated state information, wherein the estimated state information includes an estimated attitude matrix, an estimated n-frame velocity, an estimated gyro bias, and an estimated accelerometer bias;

[0128] According to the estimated attitude matrix and the estimated n-system velocity, obtaining the current estimated system error state quantity and state variance matrix based on invariant filtering update;

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

[0130] In a possible implementation, performing inertial navigation state recursion according to the current IMU data subset and the current iteration state information to update and obtain the current estimated state information includes:

[0131] Calculating a current estimated gyro bias and an estimated accelerometer bias based on the current IMU data subset;

[0132] Performing attitude recursion according to the angular velocity measurement value in the current IMU data subset and the first attitude matrix in the state information of the current iteration, and updating to obtain the estimated attitude matrix;

[0133] Velocity recursion is performed according to the acceleration increment in the current IMU data subset and the first n-system velocity in the state information of the current iteration, so as to update and obtain the estimated n-system velocity.

[0134] In a possible implementation, the obtaining of the current estimated system error state quantity and state variance matrix based on the estimated attitude matrix and the estimated n-system velocity based on invariant filtering update includes:

[0135] Constructing a deterministic matrix and a noise driving matrix according to the estimated attitude matrix and the estimated n-system velocity;

[0136] constructing a one-step matrix according to 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, the first velocity error, the first gyro bias error quantity and the first accelerometer bias error quantity obtained after the last Kalman filter measurement update;

[0138] The first state variance matrix is ​​updated according to 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 last Kalman filter measurement update.

[0139] In a possible implementation, performing Kalman filter measurement update on the estimated state information according to 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, performing Kalman filter measurement update on the estimated system error state quantity to obtain a second system error state quantity, wherein the second system error state quantity includes a second misalignment angle, a second velocity error, a second gyro bias error quantity, and a second accelerometer bias error quantity;

[0141] Obtaining a second attitude matrix by calculation according to the second misalignment angle and the estimated attitude matrix;

[0142] Obtain a second n-frame velocity by calculation according to the second misalignment angle, the second velocity error and the estimated n-frame velocity;

[0143] Calculate and obtain a second gyro bias and a second accelerometer bias according to the second gyro bias error, the second accelerometer bias error, the estimated gyro bias, and the estimated accelerometer bias;

[0144] The updated state information is constructed by combining the second attitude array, the second n-frame velocity, the second gyro bias and the second accelerometer bias.

[0145] The embodiment of the present application provides a static base precision alignment system based on invariant filtering. First, the IMU data is divided according to a preset time interval, so that multiple rounds of mechanical arrangement, state update and measurement update can be performed during the static base precision alignment process, and then the state information of the static base can be gradually updated multiple times in an iterative update manner. Compared with directly using all IMU data for precision alignment, the embodiment of the present application can effectively utilize the IMU data, so that the IMU data can more accurately reflect the motion state of the object and improve the accuracy of the static base precision alignment. At the same time, the embodiment of the present application introduces the introduction of invariant filtering for state update and measurement update in the process of each iterative update, which can reasonably avoid the introduction of specific force from a mathematical perspective, ensure the unobservable characteristics of the originally unobservable state, and realize the correct estimation of the posture, further improving the accuracy of the static base precision alignment.

[0146] The more detailed working principle and step flow of this embodiment can refer to, but are not limited to, the relevant records of Embodiment 1.

[0147] Embodiment three:

[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, wherein the processor implements the static base precise alignment method based on invariant filtering when executing the computer program.

[0149] The processor may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc. The processor is the control center of the terminal, and various interfaces and lines are used to connect various parts of the entire terminal.

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

[0151] Embodiment 4:

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

[0153] Wherein, if the module integrated by the positioning method based on RIS reflection is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on such an understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. Wherein, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form, etc. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electric carrier signal, telecommunication signal and software distribution medium, etc.

[0154] The specific embodiments described above further describe the purpose, technical solutions and beneficial effects of the present application in detail. It should be understood that the above description is only a specific embodiment of the present application and is not intended to limit the scope of protection of the present application. It is particularly pointed out that for those skilled in the art, any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of protection of the present application.

Claims

1. A static base precision alignment method based on invariant filtering, characterized in that: include: Obtain the initial state information and IMU data set of the static base carrier; Dividing the IMU data set into a plurality of IMU data subsets based on a preset time interval; Based on the time sequence, the initial state information is iteratively updated several times using the respective IMU data subsets in sequence to obtain updated state information, wherein in each iterative update process, the state information of the current iteration is mechanically arranged, state updated, and Kalman filter measurement updated in sequence according to the current IMU data subset to obtain updated state information, wherein invariant filtering is introduced in the process of the state update and Kalman filter measurement update; The updated status information is output as a precise alignment result.

2. A static base fine alignment method based on invariant filtering as claimed in claim 1, characterized in that: The mechanical arrangement, state update and Kalman filter measurement update of the current iteration state information are performed in sequence according to the current IMU data subset to obtain the updated state information, including: Performing inertial navigation state recursion according to the current IMU data subset and the current iteration state information, updating and obtaining current estimated state information, wherein the estimated state information includes an estimated attitude matrix, an estimated n-frame velocity, an estimated gyro bias, and an estimated accelerometer bias; According to the estimated attitude matrix and the estimated n-system velocity, obtaining the current estimated system error state quantity and state variance matrix based on invariant filtering update; According to the estimated system error state quantity and the state variance matrix, the estimated state information is updated by Kalman filtering measurement to obtain the updated state information.

3. A static base precise alignment method based on invariant filtering as claimed in claim 2, characterized in that: The performing inertial navigation state recursion according to the current IMU data subset and the current iterative state information to update and obtain the current estimated state information includes: Calculating a current estimated gyro bias and an estimated accelerometer bias based on the current IMU data subset; Performing attitude recursion according to the angular velocity measurement value in the current IMU data subset and the first attitude matrix in the state information of the current iteration, and updating to obtain the estimated attitude matrix; Velocity recursion is performed according to the acceleration increment in the current IMU data subset and the first n-system velocity in the state information of the current iteration, so as to update and obtain the estimated n-system velocity.

4. A static base precise alignment method based on invariant filtering as claimed in claim 2, characterized in that: The method of obtaining the current estimated system error state quantity and state variance matrix based on the estimated attitude matrix and the estimated n-system velocity and updating based on the invariant filtering comprises: Constructing a deterministic matrix and a noise driving matrix according to the estimated attitude matrix and the estimated n-system velocity; constructing a one-step matrix according to the deterministic matrix; 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, the first velocity error, the first gyro bias error quantity and the first accelerometer bias error quantity obtained after the last Kalman filter measurement update; The first state variance matrix is ​​updated according to 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 last Kalman filter measurement update.

5. A static base precise alignment method based on invariant filtering as claimed in claim 4, characterized in that: The deterministic matrix and the noise driving matrix are constructed according to the estimated attitude matrix and the estimated n-system velocity. The specific formula is: Among them, F k+1 is the deterministic matrix at time k+1, skew is the symbol of the antisymmetric matrix of the three-dimensional vector, g n is the weight vector under the n system, τ is the preset correlation time, I 3×3 is the 3 by 3 identity matrix, is the estimated attitude matrix at time k+1, v n(k+1) is the estimated n-system velocity at time k+1, G k+1 is the noise driving array at time k+1.

6. A static base fine alignment method based on invariant filtering as claimed in claim 4, characterized in that: The first state variance matrix is ​​updated according to the noise driving matrix and the one-step matrix to obtain the state variance matrix. The specific formula is: 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: Among them, x k+1 is the estimated system error state at time k+1, x k is the first system error state quantity at time k, Φ k+1 is the one-step matrix at time k+1, D k+1 is the state variance matrix at time k+1, D k is the state variance matrix at time k, G k+1 is the noise driving matrix at time k+1, and Q is the preset noise matrix.

7. A static base precise alignment method based on invariant filtering as claimed in claim 2, characterized in that: The step of performing Kalman filter measurement update on the estimated state information according to the estimated system error state quantity and the state variance matrix to obtain the updated state information includes: Based on the state variance matrix, performing Kalman filter measurement update on the estimated system error state quantity to obtain a second system error state quantity, wherein the second system error state quantity includes a second misalignment angle, a second velocity error, a second gyro bias error quantity, and a second accelerometer bias error quantity; Obtaining a second attitude matrix by calculation according to the second misalignment angle and the estimated attitude matrix; Obtain a second n-frame velocity by calculation according to the second misalignment angle, the second velocity error and the estimated n-frame velocity; Calculate and obtain a second gyro bias and a second accelerometer bias according to the second gyro bias error, the second accelerometer bias error, the estimated gyro bias, and the estimated accelerometer bias; The updated state information is constructed by combining the second attitude array, the second n-frame velocity, the second gyro bias and the second accelerometer bias.

8. A static base precision alignment system based on invariant filtering, characterized in that: It includes an acquisition module, a data partitioning module, an iterative update module and an output module; Wherein, the acquisition module is used to obtain the initial state information and IMU data set of the static base carrier; The data partitioning 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 perform several iterative updates on the initial state information based on the time sequence, using the respective IMU data subsets in sequence, to obtain updated state information, wherein in each iterative update process, mechanical arrangement, state update and Kalman filter measurement update are performed on the state information of the current iteration in sequence according to the current IMU data subset, to obtain updated state information, wherein invariant filtering is introduced in the process of the state update and Kalman filter measurement update; The output module is used to output the update status information as a precise alignment result.

9. A terminal, characterized in that: The method comprises a processor, a memory and a computer program stored in the memory and configured to be executed by the processor, wherein when the processor executes the computer program, a static base precise alignment method based on invariant filtering as described in any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored computer program, wherein when the computer program is executed, the device where the computer-readable storage medium is located is controlled to execute a static base precise alignment method based on invariant filtering as described in any one of claims 1 to 7.

Images