Wheel angle in EKF

Hello

I intend to use IMU300ZI in our omnidirectional robot (can turn all 4 wheels into arbitrary but the same direction). The robot has speed encoder and wheel angle encoder so I want to use this info in EKF. As a base code I use your INS example project.
I studied your code and think that the best place for wheel angle incorporation is in function Update_PseudoMeasurement() in UpdateFunctions.c. Instead of using frontVelMea variable as scalar I would make it vector the values of wich would be set based on current wheel angle, wheel speed and gKalmanFilter.ppsEulerAngles[YAW]. I would also set gAlgorithm.velocityAlwaysAlongBodyX=FALSE.

Odometer data would be received over SPI.
Do you think this is correct solution or is there anything important I forgot about which could cause failure?

Thank you
Regards
Pavel

Hello,

I think you are right. Just a reminder that the measured velocity in the body frame is no longer [frontVelMean, 0, 0] now. You may also rename Update_PseudoMeasurement() to something else because you are using actual measurements instead of pseudo ones.

Thank you very much for the reply.
I'd like to ask one more question about function _GenerateObservationJacobian_AHRS in UpdateFunctions.c.

There is following comment for uTheta

/* Pitch (including modifications for |q| = 1 constraint)
     *   mu = 2*( q(1)*q(3) - q(2)*q(4) );
     *   % 2/sqrt(1-mu*mu) * [q(3); -q(4); q(1); -q(2)]
     *   2/sqrt(1-mu*mu) * [q(3) - mu*q(1);
     *                      -q(4) - mu*q(2);
     *                     q(1)- mu*q(3);
     *                     -q(2)- mu*q(4)]
     */

I know how to get this line

% 2/sqrt(1-mu*mu) * [q(3); -q(4); q(1); -q(2)]

but don't know how you came to this

2/sqrt(1-mu*mu) * [q(3) - mu*q(1);
     *                      -q(4) - mu*q(2);
     *                     q(1)- mu*q(3);
     *                     -q(2)- mu*q(4)]

which is most probably mentioned constraint for |q| = 1.

Could you please shed more light on it? Your code awesome and great practical EKF tutorial.

Thank you
regards
Pavel

@paha

I found the derivation. I have used numerical differential of quaternion as reference to compare the two formula. The one with constraint did give better accuracy when pitch and roll are not close to 0.
6261bc78-d2b3-4fb5-9452-fa97fbd4c738-图片.png

I got it now.
Thank you very much
Regards
Pavel

Log in to reply