Improving orientation stability under rapid linear acceleration with 6DOF accel+gyro fusion

[joncox]

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. 

[j_perezdelarray]

Yes, try those 2 options and tune accordingly. Robert is right, the sensor fusion library is great from the point of view of code structure and sensor fusion approachbapproach might not be adapted to your specific needs. You might want to take bits from here and there and tune them accordingly.

Other option is to choose a more specific 6 dog filter out there in the web

[venkidravid]

Hi,

Thank you very much for your reply. I will try those two option and tune to get my application needs.

Regards,

K.Venkatarao.

[j_perezdelarray]

Hello,

I have noticed that roll&pitch stability under strong linear and angular accelerations is much better with the 9DOF gyro stabilized compass  instead of the 6 DOF gaming configuration. Considering how the filter is set up: I agree with Robert that the 9 axis sensor fusion results for roll&pitch are better than the 6 axis filter.

My question is: if heading information is not necessary, how would affect the stability in time of roll&pitch if the magnetometer is not routinely calibrated? 

[venkidravid]

Hi, 

You had mentioned that "I have noticed that roll&pitch stability under strong linear and angular accelerations is much better with the 9DOF gyro stabilized compass  instead of the 6 DOF gaming configuration".

1. Do you remember /specify, what is the linear and angular accelerations you had applied for the testing?

2. In my application, it is required to find attitude and also linear acceleration of 10g. Is 9 DOF gyro stabilized compass algorithm can give stable attitude when acceleration is 10g or so?

Thanks in Advance..

Regards,

Venkat.

[j_perezdelarray]

Hello Venkat,

1. No, I can't remember exactly the g level of the accel. I recon it was something around 5-8g max.

2. As mike stated in his post above, if the magnetic sensor is not calibrated, the 9 DoF algorithm might be worse for attitude stimation. I would rather use the 6 DoF algorithm.

The 6 DoF algorithm has an external acceleration compensation which is not very strong. This means that under strong external accelration the 6 DoF algorithm attitude stimation might be affected.

regards,

[venkidravid]

Hi,

Thank you so much for your reply.

Do you suggest anything to compensate external acceleration?

I will try with > 3 (in fQvGQa = 3.0F * ftmp * ftmp;)  and also try  with exponential curve to the magnitudefor as suggested by Mr. Robert.

Regards,

Venkat.

[michaelestanley]

The equations of motion used in the Kalman filter assume an unchanging vector magnitude for the magnetometer as you rotate the sensor.  But if the sensor has not been calibrated, this may not be the case (due to uncompensated soft-iron effects).  So the filter results would be off.

Regards,

Mike

[j_perezdelarray]

Thank you for the input.

[robertaldridge]

Your typical 9-axis sensor fusion algorithm has a separate weight coefficient for the accelerometer and magnetometer.  The coefficient is dynamic and decides at each iteration of the Kalman filter how much trust you put in that data.  The purpose of the acceleration data in the sensor fusion is to find gravity, whose magnitude is 1.  Therefore use the magnitude of the corrected accelerometer vector as the coefficient.  Apply an exponential curve to the magnitude to have the Kalman filter essentially throw out the acceleration data if the magnitude is much greater than 1.  It isn't purely gating; you want it to be a dynamic weight under most conditions.

Rather than an absolute value for weight or gating, you can use a relative value by calculating the local variance of the acceleration vector limiting the scope to only the most recent few iterations and using that as the weight.  If you are weighing using variance, then note it should be calculated continuously and is a component of the Kalman filter that is already being computed every iteration.

You should still be using a 9-axis sensor fusion algorithm.  3 of your axes will be based on the heading vector provided by the GPS instead of the magnetometer.  You want all data contributing to the state vector to pass through a weighted Kalman filter before fusing.

Treat NXP's Kalman filter sensor fusion library as being the quality of sample code, rather than best in class.  Assume it could first use significant improvement, maybe even a rewrite specific to your purpose, rather than good to go out of the box.

A typical production-quality sensor fusion algorithm for generic IMU + GPS tracking is 21-axis rather than 6-axis.  Also if you are using off the shelf consumer grade MEMs sensors, they aren't very good (noise).

[michaelestanley]

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

[joncox]

Hi Mike, thanks a lot for the very informative diagram and discussion. 

What about playing with fZErr or decoupling fMaxGyroOffsetChange from FQWB_6DOF_GY_KALMAN?

My thinking is that the 3DOF accelerometer gravity vector is being used to "correct" the gyro-based orientation too rapidly. Therefore, I want to limit the rate at which the gravity vector can influence the orientation (trust gyro more). One direct way to do this could be to limit the value of fZErr. Similarly, if I really trust my gyro, I can also more directly limit fMaxGyroOffsetChange, and any associated corrections to fbPl.

The intuition here is that I really only want the gravity vector to very slowly correct the orientation (e.g. over a minute or more). However, if I try to accomplish this by monkeying with variance estimates and so forth, it seems like it can tend to influence other, coupled factors. 

[michaelestanley]

Jonathan,

I would not mess with the max gyro offset parameters.  They only kick in if needed, and since you are precalibrating your gyro, they shouldn't be affecting things.  Similarly, don't mess with fZerr.  The right way to do that is by tweaking the covariance matrix, which is what the parameters in the prior messages do.  By increasing the accelerometer variance numbers, you effectively limit the acceleration effect in fZErr.

Mike