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:
-
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.
-
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)
-
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:
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.
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.
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:
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.