For a given differential equation

, Assuming that the initial value is already known

, Then its

The numerical integral after time is:

In practice, we usually can’t get

The expression of can only be discretely sampled, and then the discrete integral is used to approximate the real continuous integral. The usual approximate idea is: think

To

Differential

Expressed by a constant constant, the integral expression can be rewritten as:

It can be seen from the above formula that there are two main ways to reduce the integral error:

1. Reduce the sampling interval

, The smaller the time interval, the closer the discrete integration is to continuous integration. The sampling time interval depends on the sampling rate of the sensor, so the higher the sampling rate, the higher the integration accuracy;

2. Calculate the exact constant constant

,against

There are usually three integration methods: Euler integral, median integral and fourth-order Runge-Kutta integral.

Euler integral assumes that the slope is constant in the reciprocal interval, which is taken

The slope of time is

to

The slope of the time period, namely:

It can be seen from the formula that Euler integration is the simplest way of integration, and its approximation error is large, but the amount of calculation is small.

The median integral is improved on the basis of Euler integral. First use Euler integration to approximate the time interval

The midpoint of

Then use the midpoint slope as the approximate slope for the entire time period.

First use Euler's integral to approximate the slope of the midpoint in the time period:

Then we use the obtained midpoint slope of the time period to further approximate the slope of the entire time period:

Obviously, the median integral is more reasonable than Euler's integral.

We directly give the formula of the fourth-order Runge-Kutta integral:

Is the slope of the starting point,

Is the slope of the midpoint,

Is the midpoint slope of the midpoint,

Is the slope of the end point. In fact, the fourth-order Runge-Kutta integral is the weighted result of the slope,

versus

The slope weight of is 2 and the rest is 1. Obviously, the approximate accuracy of this method is the highest. among them

Is the slope in Euler’s integral,

It is the slope in the median integral.

The code for the fourth-order Runge-Kutta integral is as follows:

//Page 11 of Indirect Kalaman Filter for 3D Attitude Estimation //Note we use Vector4f to represent quaternion instead of quaternion //because it is better do not normalize q during integration inline Eigen::Vector4f XpQuaternionDerivative( const Eigen::Vector4f &q, const Eigen::Vector3f &omega) { return -0.5 * XpComposeOmega(omega) * q; } //Angular velocity at time k, angular velocity at time k+1, rotation at time k, dalta_t, rotation at time k+1 void IntegrateQuaternion( const Eigen::Vector3f &omega_begin, const Eigen::Vector3f &omega_end, const XpQuaternion &q_begin, const float dt, XpQuaternion *q_end, XpQuaternion *q_mid, XpQuaternion *q_fourth, XpQuaternion *q_eighth) { CHECK_NOTNULL(q_end); XpQuaternion q_0 = q_begin; //divide dt time interval into num_segment segments //TODO(mingyu): Reduce to 2 or 4 segments as 8 segments may overkill const int num_segment = 8;//number of steps //the time duration in each segment const float inv_num_segment = 1.0/num_segment;//step size const float dt_seg = dt * inv_num_segment; Eigen::Vector3f delta_omega = omega_end-omega_begin; for (int i = 0; i <num_segment; ++i) { //integrate in the region: [i/num_segment, (i+1)/num_segment] //Here uses the measured RK4 integral of the gyroscope to calculate the rotation measured by an imu before and after the frame, JPL format quaternion //Then XpQuaternionDerivative(q_0, omega_tmp) should be-0.5 * omega(w) × qcur_pre(t) Eigen::Vector3f omega_tmp = omega_begin + (i * inv_num_segment) * delta_omega;//imu angular velocity in the current step Eigen::Vector4f k1 = XpQuaternionDerivative(q_0, omega_tmp); omega_tmp = omega_begin + 0.5 * (2 * i + 1) * inv_num_segment * delta_omega; Eigen::Vector4f k2 = XpQuaternionDerivative(q_0 + 0.5 * k1 * dt_seg, omega_tmp); Eigen::Vector4f k3 = XpQuaternionDerivative(q_0 + 0.5 * k2 * dt_seg, omega_tmp); omega_tmp = omega_begin + (i + 1) * inv_num_segment * delta_omega; Eigen::Vector4f k4 = XpQuaternionDerivative(q_0 + k3 * dt_seg, omega_tmp); (*q_end) = q_0 + (dt_seg/6.0) * (k1 + 2 * k2 + 2 * k3 + k4); //store the start point the next region of integration q_0 = (*q_end); //store the intermediate attitude as output if (i == 0 && q_eighth) { (*q_eighth) = (*q_end); } else if (i == 1 && q_fourth) { (*q_fourth) = (*q_end); } else if (i == 3 && q_mid) { (*q_mid) = (*q_end); } } }

Reference: https://cloud.tencent.com/developer/article/1802953 IMU Points in VIO-Cloud + Community-Tencent Cloud