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?
I don't have an example code that exactly works for your case. But this approach can help (based on past experience),
States could be a array containing
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 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.
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.