accelerometer - AHRS algorithm under continuous linear accelerations

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.