Potential mathematics mistake

[tchoopojcharoen]

robot_localization/src/ekf.cpp

Line 327 in bab5f26

double dFY_dP = (sr * tp * cpi * pitchVel + cr * tp * cpi * yawVel) * delta;

I think it should be

double dFY_dP = (-sr * cpi * cpi * pitchVel - cr * cpi * cpi * yawVel) * delta;
But I'm not 100% certain. This is based on the mapping between the angular velocity and the orientation that the author wrote in the code. I determine the nonlinear function and perform differentiation to find this particular part of the Jacobian.

[ayrton04]

See #591 and #592.

[tchoopojcharoen]

After brief rework on my math, I've realized I made a calculation mistake by not applying the proper chain rules. It matched with the code now. Thank you so much for making times for this, and sorry for wasting your time. At least, I'm able to figure out the underlying model that you've been using. :)

[ayrton04]

Happy to help! Thanks for reviewing.