6DOF GY KALMAN Yaw drift

[gars]

First after all I want to thank you for making open source such a complete and optimized sensor fusion library.

Setting:

I am running on a MCU (without an RTOS) your sensor fusion library, utilizing an accelerometer and gyroscope from different vendors. I specifically make use of the fRun_3DOF_G_BASIC and fRun_6DOF_GY_KALMAN functions. I created a program that runs on the PC and displays a 3D cube changing its orientation based on the quaternion orientation  calculated by the MCU and sent to the PC via Wifi.

Problem:

Using the fRun_3DOF_G_BASIC function the cube behaves correctly but using the fRun_6DOF_GY_KALMAN's a posteriori orientation quaternion "fqPl", the cube behaves strangely on the jaw (z-axis). Initially if we put the sensors still on a table first there is no drift (cube not spinning on the z axis) but as we move it or shake it, even we put the sensors again on the table, the starts spinning very quickly on the z-axis. Then, if we tilt the sensors, the spinning speed gets reduced.

Debugging the code I realized that the element [2][2] of the covariance matrix P+ "fPPlus9x9[9][9]" increases very fast and it decreases when I tilt the sensors.

Question:

I'm not an expert in this subject but I guess this constant spinning over the z axis is due to the bias and drift of the gyroscope (which is not calibrated before Kalman) since the accelerometer cannot provide information to compensate the z axis (yaw) when the sensor is parallel to the table. Is this a limitation of 6axis Gyro/Acc Fusion configuration?

Also I wonder if the Kalman Constants (kalman filter noise variances, initialization of Qw covariance matrix, linear acceleration time constant) included in the code pay a role in this problem, since I haven't change them despite using other sensors, since I cannot find similar values on the datasheets.

Thank you very much!
Sincerely

[michaelestanley · Accepted solution]

Carlos,

You are correct.  If you are using a 6-axis accel+mag filter, and any of X, Y or Z sensor axes is aligned with gravity, then there is no new information coming into the filter to help correct gyro offset estimates.  When you tilted the device, you introduced new information into the computations for that axis, and it started to correct.  This is precisely why we add a magnetometer to the mix for the 9-axis fusion.

You might improve performance by tinkering a bit with the various constants, but we've found that the code ports pretty well as-is to other sensors.

If you haven't already, you might try the new version 5.00 library I posted to this space last weekend.  It was redesigned specifically to try to improve yaw drift.

Regards,

Mike

[gars]

Thank you very much from your prompt reply, it is very useful for me. The problem with my application is that the the Kalman filter continuously predicts a very big rotation over the yaw causing my 3D cube to continue to spin at many rotations per second, so I fixed this by reinitializing the Kalman filter every 50 iterations making it stop, but there is still a little rotation when having the IMU still before that 50th iteration is reached and more importantly rotating the IMU it difficult to rotate the cube in one direction(smaller rotation) of Z while it is easier to rotate(normal rotation) it in the other Z direction, the direction of the drift. I wonder if this very big drift is product of a double integration? I guess that there most be always some drift but not as big I'm experimenting as I have seen this working well utilizing madgwick filter, otherwise the yaw information of the 6DOF Acc/Gyro kalman filter would be of no use even for simple applications. But I cannot find what I'm doing wrong. I have downloaded the new version of the fusion library and I can see it has more documentation that will be very useful for me, Thank you! =)

[michaelestanley]

Carlos,

You mentioned you weren't using Freescale hardware.  Are you able to communicate to the Sensor Fusion Toolbox running on Windows?  It's designed specifically to help identify these kinds of problems.  You can see sensor and filter outputs in real time, which is helpful in tracking down problems.  Without it you're probably flying blind.

You should not have to reinitialize the Kalman filter like you are doing.  I would suggest investing in a FRDM-KL26Z plus the new FRDM-STBC-AGM01 9-Axis Sensor Board so you can run one of the canned projects and see how it is supposed to work.

Regards,

Mike