I finished to implement nonlinear complementary filter on the SO(3).
The previous problem was roll,pitch and yaw angle from quaternion.
Now it is fixed. 1-2-3 Euler representation is used.
Also accelerometer sign change has been applied.
Merge branch 'master' of https://github.com/limhyon/Firmware