code examples for IMU velocity estimation

Question

Do you maybe know where I can find code/example for velocity estimation from IMU (Inertial Measurement Unit, accelerometer + gyro + magnetometer) data?
I calculated biases from data where IMU stands still. I want to implement velocity estimation with some kind of filter (Kalman/Complementary), so far couldn't find any.
I also have camera velocity estimation, maybe it can help as some kind of fusion?

Answer 1

I don't have an example code that exactly works for your case. But this approach can help (based on past experience),

Kalman filter:

1. Decide and formulate the states X, control inputs U, outputs, prediction and observation equations.
2. Implement/ reuse some implementation of Kalman Filter. Here is a Simulink based implementation for reference.
3. Set the measurement noise and prediction error variances. It may require some fine tuning later.
4. Verify that the KF works against some reference. If you have another way to measure velocity, check the KF velocity against it.

States and Control inputs:

States could be a array containing

1. Linear velocities [Vx, Vy, Vz]
2. Angular velocities [omega_x, omega_y, omega_z]
3. Bias in gyroscope. This bias is largely constant but can change with temperature and other factors. Accelerometer measurements will be used by KF to correct for gyro bias.
4. Bias in Accelerometer. This bias is largely constant but can change with temperature and other factors. Camera velocity will be used by KF to correct for accel bias.
5. Orientation (Euler angles or quaternion)

Control inputs need not be the actual commands that are being sent to your actuators. In this case, control inputs can be the net force or net acceleration which is,

Accelerometer data (Which is specific force) + Acceleration due to gravity

Prediction equations:

Prediction equations predict the states for next time step based on current states and control inputs.

This MathWorks documentation has a good reference for prediction equations relevant to IMU.

Observation/measurement model:

Relates measurements with states.

Accel data is already used in prediction. Ignore it here.

Gyro data is [gx, gy, gz] = [omega_x + gyro_bias_x, ....] + errors

One way to handle magnetometer is to obtain yaw angle from it - arctan(y/x) and then use the yaw_mag as measurement.

Camera data is [vx_cam, vy_cam, vz_cam] = [Vx, Vy, Vx] + errors

Finally append all the rows and bring it to Y=C*X + noise form.

Y denotes the measurements from different sensors and X represents the states.

Y would be [gx, gy, gz, yaw_mag, vx,cam, vy_cam, vz_cam] in this case.

Disclaimer: I am a MathWorks employee and links are shared from MathWorks documentation.