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

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

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.

]]>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

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.

Thank you very much

Regards

Pavel ]]>