Bug in Madgwick filter _compute_all()

[adscregg]

Using the logic below to create a quaternion array in the _compute_all method overwrites Q[t-1] with the value that should be at Q[t].

for t in range(1, num_samples):
            Q[t] = self.updateMARG(Q[t-1], self.gyr[t], self.acc[t], self.mag[t])
        return Q

This is caused by the updateMARG (and updateIMU) methods doing inplace updates:

q += qDot*dt                                                # (eq. 13)
q /= np.linalg.norm(q)

return q

where q is the priori input into the method. However because of the inplace updates q then becomes the updated input and hence the value Q[t-1] is updated to be equal to Q[t] and as the value is returned at the end of the method as well so Q[t] == Q[t-1] for the final value of t i.e. the last two rows of Q will always be exactly equal which is not correct.

Suggest removing the inplace operations in favour of something like:

q_new = q + qDot*dt                                                # (eq. 13)
q_new /= np.linalg.norm(q_new)

return q_new

[Mayitzin]

Hi @adscregg thanks for bringing this up. I've seen no difference with the changes. I think it is mainly because Python performs the += operation in two separate steps. As per official Python's Documentation:

the statement x += y is equivalent to x = operator.iadd(x, y). Another way to put it is to say that z = operator.iadd(x, y) is equivalent to the compound statement z = x; z += y.

In those examples, note that when an in-place method is called, the computation and assignment are performed in two separate steps.

However, I've decided to refactor the definition of the new quaternion as you suggested to keep it in line with the algorithm explained in the original article. This will help us to understand that q and q_new are two different values.

The changes are in the commit e3c3b24

If there are further questions, please let me know. Cheers!

[adscregg]

Thanks for the update, the difference is subtle and is perhaps most obvious if q0 is set explicitly. In the example screenshot below, the first cell is with the updates in the commit you mentioned and the second cell is with the in place operations (two separate runs of the notebook).