Complementary filter implementation-documentation discrepancies

[Orestis Kaparounakis]

The documentation:
https://ahrs.readthedocs.io/en/latest/filters/complementary.html

Attitude propagation

The following line adds a sign that seems to contradict the documentation. Why is that?

ahrs/ahrs/filters/complementary.py

Lines 247 to 253 in 9e02c5d

 w = -0.5*dt*omega 
 A = np.array([ 
     [1.0,  -w[0], -w[1], -w[2]], 
     [w[0],   1.0,  w[2], -w[1]], 
     [w[1], -w[2],   1.0,  w[0]], 
     [w[2],  w[1], -w[0],   1.0]]) 
 q_omega = A @ q 

Filter

Negate q_am in some cases? Why?

ahrs/ahrs/filters/complementary.py

Lines 315 to 319 in 9e02c5d

 if np.linalg.norm(q_omega + q_am) < np.sqrt(2): 
     q = (1.0 - self.gain)*q_omega - self.gain*q_am 
 else: 
     q = (1.0 - self.gain)*q_omega + self.gain*q_am 
 return q/np.linalg.norm(q) 

However, the filter seems to work OK...

[Mario Garcia]

Hi, yes, there have been some discrepancies between the implementation and the documentation due to different reference frames being used, and weren't changed in the documentation. The negation of the signs were to ensure the right angle (and not its opposite) was being estimated.

Most of these troubles came from its implementation with Quaternions. However, a new implementation of the Complementary Filter has been done with roll-pitch-yaw angles, which is a more widespread version in practice.

These angles are then converted to Quaternions, Rotation Matrices, etc.

I think it is a much simpler and clearer approach. I'll update the documentation and its unit tests, so that we can have a Complementary Filter working as expected, and properly explained too.