AltIMU v5 Sensor Drifting


For approx. a week I'm stuck with altimu sensor. I'm trying to get more or less reliable angles from fusing accelerometer and gyroscope (LSM6) but without any luck.

I tried with Kalman Filter based on this topic:,58048.0.html
After producing that piece of code:
but shown angles aren't corrected compared to the protractor.

Secondly, I tried to implement simple Complimentary Filter used by in his YMFC quadcopter series.
The code presents itself like so:, but after while angles goes like crazy (+- k * PI) without even touching the sensor itself.

Any help appreciated.

Kind regards.