Kalman filter fuses six-axis IMU data

Kalman filter

Kalman filter is a commonly used state estimation method. Its basic principle is to optimally estimate the system state. The main idea is to divide the state of the system into a priori state and a posteriori state, and obtain the optimal estimated state by weighting the a priori state and the observed value.

The calculation of the Kalman filter gain is to minimize the sum (trace) of the elements on the diagonal of the posterior error covariance matrix (minimize the covariance)

Kalman filterassumes that the system state and measurement noise both obey Gaussian distribution with zero mean, that is, the expected value of normal distribution is 0. This is because the Kalman filter is a linear Gaussian filter, which assumes that the noise is Gaussian white noise with zero mean, and that the system state transition and measurement model are linear. In practical applications, it is usually possible to estimate the mean value of the interference and then perform compensation processing, so that the expected value of the interference is zero.

System modeling

It can be known from the knowledge of modern control theory:

Where $A$ is the state transition matrix, $B$ is the input matrix, $H$ is the observation matrix.

$W_{k}$ Process noise, consistent with normal distribution, expected to be 0, the covariance matrix is $Q$

$V_{k}$ Measurement noise conforms to the normal distribution, and the expectation is 0, the covariance matrix is $R$

Consider the model into the Kalman filter equation:

$\hat{X{_{k}^{-}}}$ is the a priori estimate of the system state, calculated based on the estimate at the previous moment.

Notice:

System modeling of noise $W_{k}$ and $V_{k}$ will not appear directly, but $Q$ and $R$ Represents the system noise covariance matrix and the observation noise covariance matrix respectively.

Here $Z_{k}$ becomes the observed value (that is, the actual calculation An input in is compared to the predicted system state).

Five formulas:

2 prediction equations

$\hat{X{_{k}^{-}}}$ is a priori state estimate, $A$ is the state transition matrix, $B$ is the input matrix, $\hat{X_{k-1}}$ is the previous one Time state estimation, $P_{k}^{-}$ is the prior error covariance matrix, $Q$ is the system noise covariance matrix .

Update 3 equations

Kalman gain equation:

Error covariance matrix update equation:

Or equivalently simplified to the following formula:

State quantity update equation:

Example: six-axis acceleration gyroscope attitude calculation

Fusion of three-axis acceleration and three-axis angular velocity

Model the 3-axis angular velocity and construct 2 prediction equations.

Observe 3-axis acceleration

System Modeling

State quantity $\begin{bmatrix} rool_-gyro \ pitch_-gyro \yaw_-gyro \end{bmatrix}$ is the attitude angle calculated from the angular velocity.

State transition matrix $\begin{bmatrix} 1 & amp; 0 & amp; 0 \ 0 & amp; 1 & amp; 0\ 0 & amp; 0 & amp; 1 \end{bmatrix}$ , the input is the angular velocity $\begin{bmatrix} gyro_-x \ gyro_-y \gyro_-z \end{bmatrix}$

$\begin{bmatrix} roll_-acc \ pitch_-acc \ yaw_-acc \end{bmatrix}$ The attitude angle calculated for acceleration, corresponding to $Z_{ k}$ Observed value

$\begin{bmatrix} roll_-esti_k \ pitch_-esti_k \ yaw_-esti_k \end{bmatrix}$ is calculated from the a priori estimated state quantity and observation quantity, corresponding to $\hat{X_{k}}$ Estimated value

Calculation of five equations

According to the modeling content, you can know:

$A = \begin{bmatrix} 1 & amp; 0 & amp; 0 \ 0 & amp; 1 & amp; 0\ 0 & amp; 0 & amp; 1 \end{bmatrix}$ , $B = \begin{bmatrix} \Delta t & amp; 0 & amp; 0 \ 0 & amp; \Delta t & amp; 0\ 0 & amp; 0 & amp; \Delta t \end{bmatrix}$ ,where $\Delta t$ Calculate the interval between two cycle times.

$u_k = \begin{bmatrix} gyro_-x \ gyro_-y \gyro_-z \end{bmatrix}$ is the 3-axis angular velocity, $\hat{x_k^-} = \begin{bmatrix} rool_-gyro_k^- \ pitch_-gyro_k^- \yaw_-gyro_k^- \end{bmatrix}$ is the prior state estimate

Initialization value

Code: source code

#pragma once #include <Eigen/Core> #include <Eigen/Dense> #include <cmath> // @input: // time: time stamp (s) // acc_*: accelerate (m/s^2) actually the unit is not important // gyro_*: gyroscope (rad/s) // @Output: // Eigen::Vector3<T> : roll pitch yaw (rad/s) template <typename Time, typename T> inline Eigen::Vector3<T> KF_6_Axis(Time time, T acc_x, T acc_y, T acc_z, T gyro_x, T gyro_y, T gyro_z) { static Eigen::Matrix<T, 3, 3> Q = decltype(Q)::Identity(); Q << 0.002, 0, 0, 0, 0.002, 0, 0, 0, 0.002; static Eigen::Matrix<T, 3, 3> R = decltype(R)::Identity(); R << 0.2, 0, 0, 0, 0.2, 0, 0, 0, 0.2; static Eigen::Matrix<T, 3, 3> A = decltype(A)::Identity(); static Eigen::Matrix<T, 3, 3> B = decltype(B)::Identity(); static Eigen::Matrix<T, 3, 3> H = decltype(H)::Identity(); H << 1, 0, 0, 0, 1, 0, 0, 0, 0; static Eigen::Vector3<T> X_bar_k_1 = decltype(X_bar_k_1)::Zero(); static Eigen::Vector3<T> u_k = decltype(u_k)::Zero(); static Eigen::Vector3<T> X_bar_k__ = decltype(X_bar_k__)::Zero(); static Eigen::Vector3<T> X_bar_k = decltype(X_bar_k)::Zero(); static Eigen::Vector3<T> Z_k = decltype(Z_k)::Zero(); static Eigen::Matrix<T, 3, 3> P_k_1 = decltype(P_k_1)::Zero(); static Eigen::Matrix<T, 3, 3> P_k__ = decltype(P_k__)::Zero(); static Eigen::Matrix<T, 3, 3> P_k = decltype(P_k)::Identity(); static Eigen::Matrix<T, 3, 3> K_k = decltype(K_k)::Zero(); static Eigen::Matrix<T, 3, 3> I = decltype(I)::Identity(); /*Calculate differential time*/ static Time last_time; // Last sampling time (s) double dt = (time - last_time); // Differential time (s) last_time = time; B = decltype(B)::Identity() * dt; /*Calculate the angular velocity on the x and y axes*/ T roll_dt = gyro_x + ((sin(X_bar_k(1)) * sin(X_bar_k(0))) / cos(X_bar_k(1))) * gyro_y + ((sin(X_bar_k(1)) * cos(X_bar_k(0))) / cos(X_bar_k(1))) * gyro_z; // Angular velocity of roll axis T dpitch_dt = cos(X_bar_k(0)) * gyro_y - sin(X_bar_k(0)) * gyro_z; // Angular velocity of pitch axis T dyaw_dt = sin(X_bar_k(0)) / cos(X_bar_k(1)) * gyro_y + cos(X_bar_k(0)) / cos(X_bar_k(1)) * gyro_z; u_k(0) = droll_dt; u_k(1) = dpitch_dt; u_k(2) = dyaw_dt; //The first step is to calculate the state extrapolation equation X_bar_k__ = A * X_bar_k_1 + B * u_k; //The second step is to calculate the covariance extrapolation equation P_k__ = A * P_k_1 * A.transpose() + Q; //The third step is to calculate the Kalman gain K_k = P_k__ * H.transpose() * (H * P_k__ * H.transpose() + R).inverse(); // The fourth step is to calculate and update the covariance matrix P_k = (I - K_k * H) * P_k__; // Calculate the observed quantity below // roll angle T acc_roll = atan(acc_y / acc_z); //pitch angle T acc_pitch = -1 * atan(acc_x / sqrt(pow(acc_y, 2) + pow(acc_z, 2))); //Assign values to the observation matrix Z_k(0) = acc_roll; Z_k(1) = acc_pitch; Z_k(2) = 0; //The fifth step is to calculate and update the status quantity X_bar_k = X_bar_k__ + K_k * (Z_k - H * X_bar_k__); //Change the historical value to prepare for the next cycle P_k_1 = P_k; X_bar_k_1 = X_bar_k; return {X_bar_k(0), X_bar_k(1), X_bar_k(2)}; }

The knowledge points of the article match the official knowledge files, and you can further learn related knowledge. Algorithm skill tree Home page Overview 56,800 people are learning the system