Open aftersomemath opened 3 years ago
@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?
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.
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.
This should be fixed now.
@ineshtyne a good way of getting your first bias $b$ is to simply record some seconds of stationary data.
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 -kI * w{meas}.
Howevever, in the code we have:
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:
I am happy to make the changes if there is consensus.