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.

I got it now.
Thank you very much
Regards
Pavel