Unit tests for dynamics jacobians
stephanietsuei opened this issue · 2 comments
stephanietsuei commented
Write unit tests for dynamics jacobians that compare analytical values to first-order numerical approximations with a small step size. (Similar unit tests have been implemented in src/tests/unittest_jacobians_instate.cpp for the measurement update.)
If the unit tests fail, then fix the dynamics jacobians.
stephanietsuei commented
Jacobians are coded (correctly, I believe) in branch dynamics_jacobians. However, Rodrigues's formula is highly nonlinear and creates numeric difficulties, as is evidenced in file unitests_jacobians.cpp and the commit history.
stephanietsuei commented
Tried again in jacobians_take_two branch after switching to Sophus. Same problems came up.