I am using the 6DOF_GY_KALMAN filter to track the orientation of a vehicle. I am not using 9DOF with magnetometer since I have GPS to provide heading angle, and its not possible for me to calibrate the magnetometer routinely. Mainly, my goal is just to subtract gravity and use the linear accelerations in the local frame. However, I also care about orientation (pitch and roll) as well.

That said, I find the filter is working pretty well in a lab environment where I'm moving the device normally by hand. However, when I mount it in a vehicle on the road, where there is a fair amount of vibration and large linear and angular accelerations, I tend to get +/-15 degree orientation errors when braking to a stop.

I render a 3D model of the orientation of the device, for debugging purposes, which allows me to play back the Sensor Fusion output. What I see is that during fairly dynamic driving, such as braking to a stop, the pitch can "flick" suddenly up or down by +/-10 or 15 degrees sometimes. This offset tends to persist until it gets slowly corrected. For instance, after coming to a stop, it slowly corrects over 3-30 seconds.

**Perhaps what I really want to do is to tell the algorithm to trust the gyro more, and not allow sudden changes in orientation to due to accelerometer input. What parameter would I change to accomplish this? **

I have tried increasing the sensor bandwidth up to 200 Hz with a 500 Hz sampling rate (200 Hz front end sensor bandwidth), calibrating out thermal drifts in the gyro, and playing with the following parameters:

FQVY_6DOF_GY_KALMAN

FQVG_6DOF_GY_KALMAN

FQWB_6DOF_GY_KALMAN

FMIN_6DOF_GY_BPL

FMAX_6DOF_GY_BPL

However, I have not yet found a silver bullet. One thought I had was specifically reducing fMaxGyroOffsetChange, since I already thermally pre-calibrate my gyro, so its never off by more than 0.05 deg/s, in practice. I have the luxury of being able to accurately calibrate my gyro over a range of temperatures while the vehicle is at rest.

**Questions:**

1. What suggestions do you have to improve performance in this type of dynamic and noisy environment?

2. Am I doing something wrong, or is this problem a fundamentally challenging task for IMU fusion?

3. How would I get the algorithm to put more faith in the gyro and not be as aggressive in correcting the orientation due to accelerometer input? I have calibrated my gyro well enough that, when the device sits on the table, it drifts extremely slowly (only over many, many minutes).

The answer to #3 would seem simple: either decrease fMaxGyroOffsetChange by lowering FQWB_6DOF_GY_KALMAN, or maybe increase FQVG_6DOF_GY_KALMAN such that the accelerometer is "trusted" less. But in practice, these parameters are seem coupled in complex ways, and all FQVG_6DOF_GY_KALMAN does is set a floor on the variance, it seems.

Jonathan,

You're going about it the right way. Decreasing FQVY_6DOF_GY_KALMAN will put more emphasis on the gyro. Increasing FQVG_6DOF_GY_KALMAN will de-emphasize the accelerometer. I don't think that playing with the min/max gyro offset parameters will help.

Basically, we treat linear acceleration as noise in the accelerometer output, and it's the gyro's job to help smooth that out. The 6-axis Kalman filter you are using is a subset of our 9-axis, which I recently took the time to diagram:

One knob not listed in the documentation is the box labelled "compute noise variances". It is used to dynamically tinker with the noise values used in the Kalman filter. You might try adjusting that. Look for "fQvGQa = 3.0F * ftmp * ftmp;" inside fRun_6DOF_GY_KALMAN() inside fusion.c. Try adjusting that 3.0F number up and see if it helps.

To answer your second question, it is a challenging problem. We originally designed the fusion library specifically targeting hand held devices. As you noted, it works pretty well there. But when you have sustained accelerations (linear or angular), some of the simplifying assumptions in the design are broken. That's what causes the effects you noticed. To fully fix those, we would need to redesign the filter to take the more complicated automotive dynamics into account, and we've not done that to date.

Regards,

Mike