Extend kalman filter

Hi all,
I'am working for a while on Madgwick filter (with samd21 chip), but now I'd like to try Extend Kalmna filter (ekf) is anyone here have already use it ? I've searched for example on github, but the only source code I found doesn't match with my needs, I needs ekf for 6 dof just accelerometer and gyroscope data.
If anyone have source code or very good example of EKF for 6dof I'll be very useful for me :grinning:

To get the best results from a Kalman filter requires you to come up with an appropriate system model. You can't just download code and expect it to work well.

To learn more about how Kalman filters work, I strongly recommend the book by Dan Simon "Optimal State Estimation".

1 Like

I've already implement kalman filter it's work fine, but now I'd like to use ekf beceause ekf used Quaternion and with Quaternion I don't have gimbal lock...

Do the math. Quaternion representation has nothing at all to do with whether the filter equations are linear or nonlinear.

1 Like

Ok so do you think I can use Quaternion with simple Kalman filter??

Of course, and it has been done many times. RTIMUlib is one example, for a "generic" Kalman filter.

It works about as well as the Madgwick or Mahony filters.

Ok beceause I've also used Madgwick filter, but I have some problems, Madgwick filter seems to be slow, I have a data rate sensor at 100Hz and I update my madgwick filter also at 100Hz, but for example if I rotate very rapidly my captor, I don't detect any rotation with Madgwick (I have maybe 1s delay to detect the angle).
I use this Madgwick filter so I have set my filter at 100 with begin function, and I have replaced betaDef value.
If I augment betaDef value madgwick filter is faster but I haven't smothness quaternion

Sounds like your particular implementation of the Madgwick filter is not working at all.

One of the most common (and fatal) mistakes by beginners is to use code intended for one type of sensor to process data from another type of sensor, with incorrect axis assignment, data scaling or calibration.

All sensors need to be individually calibrated in order for the filters to work.

Yes but the source code I gave you doesn’t use any captor. I used a lsm6dso32 captor gyro + accel. I set my captor to 104Hz data rate.
I Calibrated my captor by a 500 iterations while my captor doesn’t move, so After that I have an offset.
And after I use calibrateIMU function to fused my gyro and accel values.
I use millis to call my filter function every 10ms (100Hz).
So I really don’t understand what I’am doing wrong

Remember that a Kalman filter is a prediction model. It will be slow to respond to transients like a sudden large change because that is its purpose.

This image should explain the lag you see.

Ok I understanf but I've also slow respond with Madgwick algorithm is it normal also ?