Should this body velocity also consider angular velocity when doing linear velocity coordinate transformation?

[nathangong95]

gtsam/gtsam/navigation/NavState.cpp

Line 71 in b1e9167

Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const {

See reference: https://physics.stackexchange.com/questions/197009/transform-velocities-from-one-frame-to-an-other-within-a-rigid-body

[varunagrawal]

You have a fair point. The transformation from nav to body frame should be an Adjoint transform (using navTbody as the representational pose) and not a simple rotational transform.

Can you please make a PR with a corresponding unit test?

[varunagrawal]

So I spent more time looking into this. NavState doesn't encapsulate the angular velocity, so for bodyVelocity to do the right thing, we would have to pass it an optional angular velocity omega (e.g. as recorded from an IMU/Gyroscope). Otherwise, if omega is assumed to be zero (which is a very strong assumption) then the current formulation is correct.

[dellaert]

I don't even know what bodyVelocity means, as I very poorly documented it back in the day. I guess the assumption is NavState is the body state in a world frame, and this calculates velocity in body frame, but that should be better explained. And, indeed, angular velocity might have to be taken into account. Would be good to reconstruct why this method exists and where it is used.

[varunagrawal]

That would be the correct definition based on the function definition.
This method only exists to get b_velocity for the NavState::update and isn't used anywhere else (except for in testing).

[varunagrawal]

So the fix is quite simple. If we allow bodyVelocity to take an additional argument b_omega (which is already passed to NavState::update), we can just pick out the relevant part of the Adjoint-twist matrix multiplication.

v_n = R^n_b * v_b + \hat{t}*R^n_b*omega_b

By rearrangement of terms, we get:
v_b = R^n_b.T (v_n - \hat{t}*R^n_b*omega_b)

[dellaert]

Reference for equations? Also, how do we test without making it a self-fulfilling prophecy?
finally, then update needs an omega argument as well? Is that ever warranted at the call-site? (I.e., is it solving an actual bug?)

[varunagrawal]

The stackoverflow link has a link to a set of slides that have the equations. I'll try finding a reference that's more permanent (aka book or paper).

update already accepts omega as an argument. Testing is going to be interesting though. Once I get MHS working, I can think about a way to test this.

[varunagrawal]

This is from equation 2.57 in Murray, Li and Sastry. Adding here for posterity.