AltIMU v5 Sensor Drifting

Hello!

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: http://forum.arduino.cc/index.php/topic,58048.0.html
After producing that piece of code: http://pastebin.com/FgN67Gqm
but shown angles aren't corrected compared to the protractor.

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

Any help appreciated.

Kind regards.