Electronic – AHRS algorithm under continuous linear accelerations

accelerometeralgorithmgyroimumagnetometer

I have tried several algorithms to get pitch, roll and yaw under continuous linear accelerations and vibrations (smaller than 0.4g, frequency lower than 10HZ). None of them give good results because the readings either drift or are affected too much by linear accelerations. What I want to achieve is when external acceleration is smaller than +-0.4g, the error on pitch and roll should be smaller than +-1deg.

I have tried these algorithms:

  1. Madgwick's algorithm. When Beta gain is set very high, the convergence is fast but angles are more susceptible to linear accelerations. I tuned it down and reduced the error under linear accelerations down to +-0.5deg. However, if the vibration is continuous, the readings will drift and it takes forever to converge to true values. It makes sense because under linear accelerations, gyro is trusted more and calculated angles drift as gyro integration drifts.

  2. Mahony's algoritm. On the contrary to Madgwick's, it doesn't drift at all regardless what values I use for Ki and Kp. However, it is always affected by linear accelerations. (Errors bigger than +-6deg)

  3. Traditional Kalman filter. Lots of time has been spent on tuning those huge R and Q vectors. So far it has the same performance as Mahony's.

I am using razor IMU. I know with cheap sensors it's impossible to achieve the same result as this one.

There are couple more options like UKF but it's a pain to understand or implement.

Any suggestion is welcomed.

Best Answer

Firstly, make sure you understand two key points here:

  1. Attitude determination from IMU data alone is inherently ambiguous in the presence of linear acceleration. Without additional knowledge about the nature of the accelerations, there will always be an upper bound to the accuracy you can achieve.

  2. Accuracy is limited by the drift in the integrated gyroscope measurements. With perfect gyro data and integration, the accelerometer data would not be needed at all. The closer you can get to perfection the more you can ignore the accelerations.

The selection of orientation algorithm is largely irrelevant here. All of them work on the same principle: using the direction of gravitational acceleration to drift correct the integrated gyro data, with some variable amount of weighting between the two. If you have tried tuning the parameters and not achieved the results you want, you are unlikely to do better with a different algorithm.

So, there are essentially two things you can do.

  1. Improve the accuracy of your gyro integration.
  2. Model the nature of the linear accelerations somehow.

The second option is hard to discuss because it depends on the details of the motion you are studying. There are some simple tricks like discarding or de-weighting accelerations outside some given range. Essentially these come down to modelling the linear accelerations as being brief occurences only. If your system is under continuous motion they are not of much help.

There are several things you can do to improve your gyro integration, though:

  1. Get the best possible estimate of gyro bias. Take static gyro readings for several seconds immediately before use, and average these to get your offset values. Don't rely on a one-off prior calibration.
  2. Try to minimise drift due to temperature. Let the IMU warm up to steady state operating temperature before calibration/use. Try to keep it at a steady temperature during operation.
  3. Improve your calibration model. Consider including cross axis effects and non-linearity as well as just scale and offset.
  4. Use a better integration method. There are some ideas in the comments on your question already.
  5. See if you can track gyro drift. If the orientation algorithm is having to consistently correct drift in a particular direction, this can be detected and used to gently adjust the bias values.

You are right that the sensors you are using are not of the highest grade available. However, it is possible to get very good results from consumer sensors if sufficiently well characterised and calibrated.

Related Topic