Sign error in transferFunctionJacobian_

[abojda]

I was just diving deeper into the math behind EKF and its implementation in robot_localization and I believe I found sign error in formula for transfer function Jacobian. I think that at line 327 In file ekf.cpp the correct formula should be:
"double dFY_dP = (sr * tp * cpi * pitchVel + cr * tp * cpi * yawVel) * delta;"
instead of
"double dFY_dP = (sr * tp * cpi * pitchVel - cr * tp * cpi * yawVel) * delta;"

[ayrton04]

I think you're right, good catch. The transfer function (ignoring the time delta term) is:

new_yaw = yaw + v_pitch * sin(roll) * cos(pitch)^-1 + v_yaw * cos(roll) * cos(pitch)^-1

So then we'd have

dFYaw/dPitch = v_pitch * sin(roll) * - (-sin(pitch)) * cos(pitch)^-2 + v_yaw * cos(roll) * - (-sin(pitch)) * cos(pitch)^-2
             = v_pitch * sin(roll) * tan(pitch) * cos(pitch)^-1 + v_yaw * cos(roll) * tan(pitch) * cos(pitch)^-1

Care to submit a PR?

Will go see if I still have this in my notebook from when I originally computed that.

[abojda]

I got the same equation for dFYaw/dPitch.
Will submit a PR in a moment.

[ljburtz]

Hello and awesome to see an update. Do you know if this has an impact on the performance of the filter estimate? I have a deadline soon and wondering if I should update to this latest. Thanks for any input on the magnitude of the change/test results you might have done!

[abojda]

Actually, I didn't run any tests, just noticed the problem when I was analytically calculating Jacobian matrix.

As far as I know, this issue shouldn't affect 2D case (most common case with mobile robots) as the whole part v_yaw * cos(roll) * tan(pitch) * cos(pitch)^-1 is equal to 0 as tan(pitch) = 0, so the sign doesn't matter.

I think this could affect 3D localization, but can't say how much of a performance impact it is. It should depend on your specific filter/system setup. Anyway, if you work on 3D localization I would definitely consider an update.

[ljburtz]

I am indeed doing 3D (some slopes so pitch and roll are important)
Thank you so much for that quick answer.
And if I have some meaninfgul test to compare then I will share them here too!