Kalman filter erratic but Mahony works fine

[patrickduffyETS]

I'm using this library in a C++ project on a Raspberry PI, with an external gyro/acc/compass board purchased on Amazon. Setup for the Mahony filter seems to work fine but when I switch to Kalman, the output goes crazy and is completely wrong. Is there any scaling or input differences that are needed to run the Kalman filter? I'm using all the default setting for both Kalman and Mahony.

Thanks,
Patrick Duffy

[reefwing]

Hi Patrick,

You need to narrow down the problem and provide more data before I can help. The first thing to check is that you are feeding the correct IMU data into the library via SensorData. The intended inputs are:

• Accelerometer: g
• Gyroscope: degrees/second (DPS)
• Magnetometer: any consistent unit (gauss or µT) since it is normalized/relative in most fusion paths

If your IMU driver returns gyro in rad/s, Mahony can still appear “reasonable” depending on tuning and dt, while the EKF can behave badly because it is far less forgiving of model mismatch.

Next up isolate the fault with the minimum reproducible test code. Something like:

1. Read the IMU once per loop using synthetic data
2. Log the raw/scaled sensor values
3. Runs both algorithms on the same exact sample
4. Log roll/pitch outputs side by side

Kalman filters are very sensitive to dt. On Arduino the library uses a microsecond timer internally. How are you doing this on the RPi? If dt spikes (scheduler jitter on Linux), EKF output can blow up even with good sensor data. Try logging the dt over 5-10 seconds.

What model RPi are you using? The problem is that Linux is not a real time operating system. Mahony and Madgwick filters tolerate this fairly well because they are feedback dominated and implicitly correct drift each update. An EKF is model based and explicitly integrates state using dt. If dt varies or spikes, the filter covariance and state prediction can diverge very quickly.

[patrickduffyETS]

I'm using a raspberry pi 5, and a dedicated thread to run the loop. DT is consistent within a few microseconds, and I tried varying the loop time between 1 to 20 milliseconds. There was virtually no difference in the output, but when I set the gyro to output zero, the filter was stable showing roll/pitch as expected, but the update was extremely slow compared to mahony or madgwick. I tried scaling down the gyro output, but it didn't make any difference, the output was unstable. I switched between degrees/second and radians/second and there was no difference. I thought perhaps there's some initialization that needs to be different with Kalman that I'm missing. I'm using the default init method supplied in the repo. It's not critical that I get Kalman to work. I was more curious to understand the differences and advantages of using different filters.

Regards,
Patrick

[reefwing]

Thanks for the extra information. You may have found a problem with my Kalman code. In ReefwingAHRS, the Extended Kalman option is a very simple 2-state filter which uses body rates not Euler angles. To test if this is the issue, put the IMU flat and still, then do a slow rotation about only one axis at a time:

• roll-only motion (rotate around sensor X), then back to flat
• pitch-only motion (rotate around sensor Y), then back to flat

If EKF behaves for single-axis rotations but goes unstable during combined-axis motion, that points to a state model limitation (not your hardware).

In the meantime, I'll have another look at the Kalman code. If you want another modern sensor fusion option, have a look at Fusion.