Kalman filtering attitude estimation method based on SigKAN network assistance

Abstract

The invention belongs to the field of attitude estimation, and discloses a SigKAN network assistance-based Kalman filtering attitude estimation method, which comprises the following steps of: establishing a discrete time state equation and a measurement equation, and modeling a disturbance component and a reference component; a SigKAN model is trained; a SigKAN model is used to predict the dynamic characteristics of the noise; removing a disturbance component by using a Kalman filtering algorithm, and initializing state variable priori estimation and a covariance matrix thereof; calculating a residual error and a covariance thereof by using the measurement information, calculating a Kalman gain, and obtaining a state variable estimation value and a covariance matrix thereof; the gravitational acceleration and the geomagnetic field serve as observation vectors, a reference vector is obtained, attitude calculation is conducted through a QUEST algorithm, and an attitude estimation quaternion is obtained; and obtaining attitude estimation at all moments. According to the method, the dynamic characteristics of disturbance are learned by adopting the SigKAN network, disturbance estimation is carried out by assisting the filter in a mode of dynamically adjusting process noise covariance, and the anti-interference capability of a traditional Kalman filter is enhanced.

Description

Technical Field

[0001] The present invention relates to the field of attitude estimation, and specifically to a Kalman filter attitude estimation method assisted by a SigKAN network. Background Technique

[0002] In the field of attitude estimation, inertial measurement units have been widely studied and applied due to their advantages of miniaturized design and low cost. Due to the problem of integral cumulative error in gyroscopes, it is difficult to maintain high attitude estimation accuracy for a long time. Generally, accelerometers and magnetometers are used to correct the attitude cumulative error. Specifically, the gravitational acceleration measured by the accelerometer can assist in calculating the roll angle and pitch angle, and the geomagnetic field is used to calculate the yaw angle. However, accelerometers and magnetometers are easily affected by motion acceleration and ferromagnetic disturbances. To enhance the accuracy of attitude estimation, adaptive Kalman filtering methods have been applied to attitude estimation based on inertial measurement units, considering motion acceleration and ferromagnetic disturbances as external noises and adjusting the noise covariance to respond in a timely manner to changes in the disturbance intensity. Threshold-based adaptive filtering is a commonly used method, which has the advantages of easy implementation and small computational complexity. However, the selection of the threshold requires experience and is not applicable to situations where external disturbances change violently. With the rapid development of deep learning, neural network algorithms have shown great potential in processing multi-dimensional time series. Neural networks can learn the internal relationships between inertial measurement unit data, extract relevant spatio-temporal features, and predict the dynamic changes of disturbances. Therefore, the present invention proposes a filtering attitude estimation method assisted by a SigKAN network. First, a Kalman filter is established based on a disturbance separation model. Secondly, the SigKAN network is used to learn the dynamic characteristics of disturbances, and the filter is assisted in disturbance estimation by dynamically adjusting the process noise covariance to respond to the dynamic changes in the disturbance intensity. Finally, a quaternion estimator (QUEST) is used to perform attitude calculation on the gravitational acceleration and geomagnetic field intensity after removing the disturbances. Summary of the Invention

[0003] The purpose of the present invention is to provide a Kalman filter attitude estimation method assisted by a SigKAN network to solve the problems raised in the above background technique.

[0004] To achieve the above purpose, the present invention provides the following technical solutions:

[0005] A Kalman filter attitude estimation method assisted by a SigKAN network includes the following steps:

[0006] Step 1, establish the discrete-time state equation and measurement equation of the attitude estimation system, and model the disturbance component and reference component;

[0007] Step 2: Train the SigKAN model. The input of the SigKAN model is the measurement information of the IMU. Substitute the process noise covariance output by the network into the Kalman filter. The label value of the network uses the attitude information captured by the OptiTrack system, calculate the loss function, calculate the gradient through backpropagation, and optimize the network parameters;

[0008] Step 3: Use the trained SigKAN model to predict the dynamic characteristics of the noise and obtain the process noise covariance Q at time k k ;

[0009] Step 4: Use the Kalman filter algorithm to remove the disturbance component. Initialize the prior estimate x0 of the state variable and its covariance matrix P0. According to the discrete-time model of the attitude estimation system, update the predicted value of the attitude information filtered at time k and its covariance matrix P k|k-1 ;

[0010] Step 5: Use the measurement information z collected by the IMU k , calculate the residual and its covariance, calculate the Kalman gain K k , and obtain the estimated value of the state variable at time k and its covariance matrix P k|k ;

[0011] Step 6: Use the gravitational acceleration and geomagnetic field in the Kalman filter estimate x k|k as the observation vector, and obtain the reference vector in the stationary state. Perform attitude solution through the QUEST algorithm to obtain the attitude estimation quaternion at time k;

[0012] Step 7: Repeat Steps 3 - 6 to obtain the attitude estimations at all times.

[0013] Furthermore, the said Step 1 includes: modeling the disturbance component and the reference component

[0014]

[0015] where the subscript k in the lower right represents the time, and the superscript s in the upper right represents that the signal is based on the sensor coordinate frame, f s is the reference component, d is the disturbance component, I3 is the 3×3 identity matrix, T s is the discrete-time sampling period, g y k-1 is the gyroscope signal, g v k-1 is the gyroscope error, [·×] represents the skew-symmetric matrix operation, is a constant value, w d,k-1 is the zero-mean Gaussian white noise of the disturbance at time k;

[0016] The disturbance component and the reference component are combined to obtain the state variable:

[0017]

[0018] where and are the gravitational acceleration vector and the geomagnetic field vector respectively, a d k-1 and m d k-1 are the motion acceleration and the ferromagnetic disturbance respectively;

[0019] The discrete-time state equation and the measurement equation of the attitude estimation system are established:

[0020] x k = F k-1 x k-1 + G k-1 w k-1

[0021] z k = Hx k + v k (2)

[0022] where w k-1 = g v k-1 g v k-1 a w d,k-1 m w d,k-1 T ;

[0023] In the above formula, I3 and 03 are the 3×3 identity matrix and the zero matrix respectively, c1 and c2 are constant values, z k is the measurement value at time k, -T s [·×] g v k-1 is the offset error caused by the error of the gyroscope, g v k-1 is the zero-mean Gaussian white noise of the gyroscope at time k-1, F k-1 is the state matrix, H is the measurement matrix, G k-1 is the process noise matrix, v k is the measurement noise vector, w k-1 is the Gaussian white noise of the gyroscope g v k-1 and the Gaussian white noise of the motion acceleration a w d,k-1 ​and the Gaussian white noise of ferromagnetic disturbance m w d,k-1 which forms a process noise vector.

[0024] Furthermore, the measurement information of the IMU in step 2 includes the gyroscope signal, accelerometer signal, and magnetometer signal in the sensor coordinate system, namely angular velocity, acceleration, and magnetic field.

[0025] Furthermore, the SigKAN model in step 2 consists of a SigKAN layer, a flattening layer, and a linear layer. Among them, the SigKAN layer is composed of a learnable path signature layer, a KAN layer, a dropout layer, a gated unit linear layer, and a normalization layer, and the loss function is the mean square error function.

[0026] Furthermore, the state variable prediction of the attitude estimation system at time k in step 4 and its covariance matrix P k|k-1 are calculated by the following formula:[[]]

[0027]

[0028] where is the prior estimate, P k-1|k-1 is the covariance matrix of the prior estimate, Q k-1 is the process noise covariance predicted by the neural network, is the transpose matrix of F k-1 , is the transpose matrix of G k-1 .

[0029] Furthermore, the residuals and their covariance in step 5 are used to calculate the posterior estimate and the specific process is given by the following formula:[[]]

[0030] K k = P k|k-1 H T (HP k|k-1 H T + R) -1 (5)

[0031]

[0032] P k|k = (I 12 - K k H)P k|k-1 (7)

[0033] where R is the measurement noise covariance, K k is the Kalman gain, I 12 is the 12×12 identity matrix, and H Tis the transpose matrix of H, is the state estimation value, P k|k is the covariance matrix of the state estimation.

[0034] Further, in the step 6, the observation vector is composed of the estimated gravitational acceleration and the geomagnetic field, and the reference vector is composed of the gravitational acceleration and the geomagnetic field in the same coordinate system; the QUEST algorithm will find an optimal quaternion that maximizes the gain function to realize the attitude solution of the reference vector and the observation vector, and its specific formula is as follows:

[0035]

[0036] where h(·) is the gain function, is the attitude quaternion, a i , i = 1, …, n is a set of non - negative weights, σ is the variance of the measurement error, γ is the sum - of - squares matrix of the difference between the observation vector and the reference vector, β is the weighted sum - of - squares matrix of the difference between the observation vector and the reference vector, K is the weight matrix, and the optimal quaternion is found when the matrix K corresponds to the eigenvector of the maximum eigenvalue λ max , that is:

[0037]

[0038] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention establishes a Kalman filter based on the perturbation separation model, and uses the SigKAN network to learn the dynamic characteristics of the perturbation. By dynamically adjusting the process noise covariance to assist the filter in perturbation estimation, the anti - interference ability of the traditional Kalman filter is enhanced, the estimation error problem caused by inaccurate noise covariance is overcome, and the accuracy of attitude estimation is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 is the flow chart of the present invention;

[0040] Figure 2 is the schematic diagram of the SigKAN neural network model structure in the present invention;

[0041] Figure 3 is the block diagram of attitude estimation of the Kalman filter and SigKAN network fusion in the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0042] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0043] Referring to Figure 1 、 Figure 2 and Figure 3 , a Kalman filter attitude estimation method based on SigKAN network assistance includes the following steps:

[0044] Step 1: Establish the discrete-time state equation and measurement equation of the attitude estimation system, and model the disturbance component and reference component.

[0045] In the following model, the subscript k in the lower right represents the moment, and the superscript s in the upper right represents that the signal is based on the sensor coordinate frame. f s is the reference component, d is the disturbance component, g y k-1 is the gyroscope signal, g v k-1 is the gyroscope error, [·×] represents the skew-symmetric matrix operation, is a constant value, w d,k-1 is the zero-mean Gaussian white noise of the disturbance at time k.

[0046]

[0047] Augment the disturbance component and reference component to obtain the state variable:

[0048]

[0049] where and are the gravitational acceleration vector and the geomagnetic field vector respectively, a d k-1 and m d k-1 are the motion acceleration and ferromagnetic disturbance respectively.

[0050] The discrete-time state equation and measurement equation of the attitude estimation system are given by the following formula:

[0051] x k = F k-1 x k-1 + G k-1 w k-1

[0052] z k = Hx k + vk (2)

[0053] Wherein, w k-1 = g v k-1 g v k-1 a w d,k-1 m w d,k-1 T ;

[0054] In the above formula, c1 and c2 are constant values, z k is the measurement value at time k, -T s [·×] g v k-1 is the offset error caused by the gyroscope error, g v k-1 is the zero-mean Gaussian white noise of the gyroscope at time k-1, F k-1 is the state matrix, H is the measurement matrix, G k-1 is the process noise matrix, v k is the measurement noise vector, w k-1 is the process noise vector composed of the Gaussian white noise g v k-1 , the Gaussian white noise of the motion acceleration a w d,k-1 and the Gaussian white noise of the ferromagnetic perturbation m w d,k-1 .

[0055] Step 2, train the SigKAN model. Use the measurement information of the IMU as the input of the SigKAN model, input the process noise covariance output by the network into the Kalman filter, use the attitude information captured by the OptiTrack system as the label value of the network, calculate the loss function, backpropagate, calculate the gradient and optimize the network parameters.

[0056] Step 3, use the trained SigKAN model to predict the dynamic characteristics of the noise. Obtain the process noise covariance Q k .

[0057] Step 4, use the Kalman filtering algorithm to separate the perturbation components in the inertial measurement unit. Initialize the prior estimate x0 of the state variable and its covariance matrix P0, and obtain the prediction of the state variable at time k and its covariance matrix P k|k-1 .

[0058] ​Step 5: Using the measurement information z collected by the IMU k , calculate the residual and its covariance. Calculate the Kalman gain K k , and obtain the estimated value of the state variable at time k and its covariance matrix P k|k .

[0059] Step 6: Compose the gravitational acceleration and the geomagnetic field vector in the Kalman filter estimate into an observation vector, and obtain the gravitational acceleration and the geomagnetic field in the stationary state as reference vectors. Perform attitude calculation through the QUEST algorithm to obtain the attitude estimation quaternion at time k.

[0060] Step 7: Repeat Steps 3 - 6 to obtain the attitude estimations at all times.

[0061] As Figure 2 shown, establish the SigKAN model. The model consists of a SigKAN layer, a flattening layer, and a linear layer. Among them, the SigKAN layer is composed of a learnable path signature layer, a KAN layer, a dropout layer, a gated unit linear layer, and a normalization layer.

[0062] Represent the gyroscope, accelerometer, and magnetometer data collected by the IMU as: ω k = [ω x,k ω y,k ω z,k T , a k = [a x,k a y,k a z,k T , m k = [m x,k m y,k m z,k T , and the input of the neural network is The neural network obtains the output through the forward propagation process. Replace the process noise covariance in the Kalman filter with the network prediction result. The Kalman filter is used to separate the perturbation component and the reference component. First, initialize the state variable x0 and its covariance matrix P0. According to the established state equation and measurement equation, calculate the state variable at time k - 1 and its covariance matrix P k|k-1 , and further, according to the measurement information z k obtained by the IMU, calculate the residual and its covariance. Calculate the Kalman gain K k to obtain the posterior estimation of the state variable and its covariance matrix P k|k . The calculation process of the Kalman filter is given by the following formula: ​​​

[0063]

[0064] K k = P k|k-1 H T (HP k|k-1 H T + R) -1 (5)

[0065]

[0066] P k|k = (I 12 - K k H)P k|k-1 (7)

[0067] Take the reference component in the posterior estimate as the output of the network, denoted as According to the mean square error loss function, calculate the loss error between the label value and y k and use the optimization algorithm to implement backpropagation to adjust the parameter weights of the network. After the model training is completed, input the IMU data to obtain the process noise covariance Q at time k k .

[0068] For attitude calculation, form the observation vector from the gravitational acceleration vector and the geomagnetic field vector estimated by the Kalman filter, form the reference vector from the gravitational acceleration and the geomagnetic field in the stationary state, use the QUEST algorithm for attitude calculation, calculate the sum of squares matrix γ of the differences between the observation vector and the reference vector and the weighted sum of squares matrix β of the differences between the observation vector and the reference vector, obtain the variance σ of the measurement error, simplify the gain function through the γ, β, and σ variables, calculate the K matrix, and find the optimal quaternion when the matrix K corresponds to the eigenvector of the maximum eigenvalue λ max . The specific process is given by the following formula:

[0069]

[0070] Repeat the execution until all attitude estimates at time k are obtained.

[0071] The attitude estimation method based on the SigKAN network-assisted Kalman filter proposed by the present invention can effectively combine the advantages of strong feature learning ability of the neural network and fast convergence speed of the Kalman filter, overcome the estimation error problem caused by inaccurate noise covariance of the traditional Kalman filter, and realize an attitude estimation method with high accuracy and strong anti-interference ability.

[0072] Although embodiments of the present invention have been shown and described, it will be understood by those of ordinary skill in the art that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention, and the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A Kalman filter attitude estimation method based on SigKAN network assistance, characterized in that: The following steps are involved: Step 1: Establish the discrete time state equation and measurement equation of the attitude estimation system, and model the disturbance component and reference component; Step 2: Train the SigKAN model. The input of the SigKAN model is the measurement information of the IMU. The process noise covariance of the network output is substituted into the Kalman filter. The label value of the network uses the posture information captured by the 0ptiTrack system to calculate the loss function, calculate the gradient through back propagation and optimize the network parameters. Step 3: Use the trained SigKAN model to predict the dynamic characteristics of the noise and obtain the process noise covariance Q at time k. k ; Step 4: Use the Kalman filter algorithm to remove the disturbance component, initialize the state variable prior estimate x0 and its covariance matrix P0, and update the attitude information prediction value filtered at time k according to the discrete time model of the attitude estimation system. and its covariance matrix P k|k-1 ; Step 5: Use the measurement information collected by IMU k , calculate the residual and its covariance, calculate the Kalman gain K k , get the estimated value of the state variable at time k and its covariance matrix Pk|k; Step 6: Use the Kalman filter estimate x k|k The gravitational acceleration and geomagnetic field in are used as observation vectors, and the reference vector in the static state is obtained. The attitude is solved by the QUEST algorithm to obtain the attitude estimation quaternion at time k. Step 7: Repeat steps 3 to 6 to obtain the pose estimation at all times.

2. A SigKAN network-assisted Kalman filter attitude estimation method as claimed in claim 1, characterized in that: The step 1 includes: modeling the disturbance component and the reference component The right subscript k represents the time, the right superscript s represents the signal based on the sensor coordinate frame, and f s is the reference component, d is the disturbance component, I3 is the unit matrix of size 3×3, T s is the discrete time sampling period, g y k-1 is the gyroscope signal, g v k-1 is the gyroscope error, [·×] represents the skew-symmetric matrix operation, is a constant value, w d,k-1 is the zero-mean Gaussian white noise disturbed at time k; The disturbance component and the reference component are combined to obtain the state variable: in, and are the gravitational acceleration vector and the geomagnetic field vector, respectively. a d k-1 and m d k-1 are motion acceleration and ferromagnetic disturbance respectively; Establish the discrete time state equation and measurement equation of the attitude estimation system: x k =F k-1 x k-1 +G k-1 w k-1 z k =Hx k +v k (2) in, w k-1 =[ g v k-1 g v k-1 a w d,k-1 m w d,k-1 ] T ; In the above formula, I3, 03 are the unit matrix and zero matrix of size 3×3, c1, c2 are constant values, z k is the measured value at time k, -T s [·×] g v k-1 is the offset error caused by the gyroscope error, g v k-1 is the zero-mean Gaussian white noise of the gyroscope at time k-1, F k-1 is the state matrix, H is the measurement matrix, G k-1 is the process noise matrix, v k is the measurement noise vector, w k-1 is the Gaussian white noise of the gyroscope g v k-1 , Gaussian white noise of motion acceleration a w d,k-1 and Gaussian white noise of ferromagnetic disturbance m w d,k-1 The process noise vector composed of .

3. The SigKAN network-assisted Kalman filter attitude estimation method according to claim 1, characterized in that: The measurement information of the IMU in step 2 includes the gyroscope signal, accelerometer signal and magnetometer signal in the sensor coordinate system, that is, angular velocity, acceleration and magnetic field.

4. The SigKAN network-assisted Kalman filter attitude estimation method according to claim 1, characterized in that: The SigKAN model in step 2 is composed of a SigKAN layer, a flattening layer, and a linear layer, wherein the SigKAN layer is composed of a learnable path signature layer, a KAN layer, a discard layer, a gated unit linear layer, and a normalization layer, and the loss function is a mean square error function.

5. The SigKAN network-assisted Kalman filter attitude estimation method according to claim 1, characterized in that: The state variable prediction of the attitude estimation system at time k in step 4 and its covariance matrix P k|k-1 The calculation of is given by: in, is a priori estimate, P k-1|k-1 is the covariance matrix of the prior estimate, Q k-1 is the process noise covariance of the neural network prediction, F k-1 The transposed matrix of G k-1 The transposed matrix of .

6. The SigKAN network-assisted Kalman filter attitude estimation method according to claim 1, characterized in that: The residuals and their covariance described in step 5 are used to calculate the posterior estimate The specific process is given by the following formula: K k =P k|k-1 H T (HP k|k-1 H T +R) -1 (5) P k|k =(I 12 -K k H)P k|k-1 (7) Where R is the measurement noise covariance, K k is the Kalman gain, I 12 is the identity matrix of size 12×12, H T is the transposed matrix of H, is the state estimate, P k|k is the covariance matrix of the state estimate.

7. The SigKAN network-assisted Kalman filter attitude estimation method according to claim 1, characterized in that: In step 6, the observation vector is composed of the estimated gravitational acceleration and geomagnetic field, and the reference vector is composed of the gravitational acceleration and geomagnetic field in the same coordinate system; the QUEST algorithm will find an optimal quaternion that maximizes the gain function To realize the attitude solution of reference vector and observation vector, the specific formula is as follows: Where h(·) is the gain function, is the attitude quaternion, a i , i = 1, ..., n is a set of non-negative weights, σ ​​is the variance of the measurement error, γ is the square sum matrix of the difference between the observation vector and the reference vector, β is the weighted square sum matrix of the difference between the observation vector and the reference vector, K is the weight matrix, when the matrix K corresponds to the maximum eigenvalue λ max The optimal quaternion is found when the eigenvector of

Images