AnsweredAssumed Answered

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

Question asked by Jonathan Cox on Jun 20, 2017
Latest reply on Jan 23, 2018 by

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:



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.



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.