Mahony filter k_I parameter not used correctly

[aftersomemath]

In the documentation of the Mahony filter it is stated that \dot{\hat{q}} is a function of \hat{b} where \hat{b} results from integrating -k_I * w_{meas}.

Howevever, in the code we have:

omega_mes = np.cross(acc/a_norm, v_a)   # Cost function (eqs. 32c and 48a)
b = -self.k_I*omega_mes                 # Estimated Gyro bias (eq. 48c)
Omega = Omega - b + self.k_P*omega_mes  # Gyro correction
p = np.array([0.0, *Omega])
qDot = 0.5*q_prod(q, p)                     # Rate of change of quaternion (eqs. 45 and 48b)

Which does not contain an integration of the b_dot term. In essence, the integral term is disabled (k_I = 0) and k_P has been set to (k_P - k_I). Perhaps the code should be updated to something like:

omega_mes = np.cross(acc/a_norm, v_a)   # Cost function (eqs. 32c and 48a)

bDot = -self.k_I*omega_mes                 # Estimated Gyro bias (eq. 48c)
self.b += bDot * self.Dt

Omega = Omega - self.b + self.k_P*omega_mes  # Gyro correction
p = np.array([0.0, *Omega])
qDot = 0.5*q_prod(q, p)                     # Rate of change of quaternion (eqs. 45 and 48b)

I am happy to make the changes if there is consensus.

[ineshtyne]

@aftersomemath thank you for your remark! Could you tell me please how did you initialised your b? and do you have any tipps how to optimally chose the K_I & K_P?

[aftersomemath]

It seems it would be as easy as adding a b0 parameter to the constructor as is done with q0. That b0 should be used in _compute_all. Then the updateIMU and updateMARG functions should be updated to accept the current value of b as a function parameter and then return the updated b value as is done with q.

I haven't checked to see how states other than q are handled in the other estimators provided by this package. It is possible there is another pattern to follow.

Sorry I am not an expert on choosing the gains for a Mahony filter. If you setup an optimization problem, you can brute force the problem with grid search on a range of K_I and K_P. Then choose the K_I and K_P that result in the lowest error (perhaps RMS^2 error for instance). However, a ground truth orientation source will be needed for that.

[timethy]

Hi, I've created a pull request to address this.

I also optimized some of the code to make it a bit faster, if wanted, I'm happy to create a PR for that as well.

[timethy]

This should be fixed now.

@ineshtyne a good way of getting your first bias $b$ is to simply record some seconds of stationary data.