World velocity to body frame velocity and uncertainty.

[Mike]

GTSAM normally keeps state information in world coordinations, including velocity. When not ingesting an external yaw source it's possible for yaw error to grow unbounded which leads to world velocity uncertainty growing unbounded. On the other hand in-body velocity coordinates should always be constrained within fixed bounds since in-body velocity is an observable quantity within VIO systems(IE you can always take a world coordinate frame as your current pose3 and optimize for velocity with respect to this coordinate frame - assuming your movement has sufficient excitation, your in-body velocity can be recovered).

What I am curious about is how these two things can be true simultaneously, in other words, if we take GTSAM world state uncertainty and convert to body-frame, will the velocity uncertainty in world frame related to attitude drop away in body frame? In this manner you can also build up relative constraints between poses.

Thanks for any insight 

[Mike]

To put this in further context, I'm using the marginals function to get velocity covariance which I then transform to body frame, ideally I would like to get that covariance in body frame while still having the rest of the states marginalized out but I don't see a way to do this. For uncertainty propagation I'm currently manually transforming world to body velocity covariance using the standard linear transformation formula for Normals, but I suspect there is attitude uncertainty being added(since marginals returns in world frame) which ideally we want to remove.

[José Luis Blanco-Claraco]

Hi Mike:

I think that what you need to do here is to retrieve the marginals for
both, pose and velocities (both in World frame), then use that 2x2
covariance blocks to find the body-frame velocity by propagating the
uncertainty though the inverse rotation function, which should in
theory "reduce" the uncertainty in the velocity vector in global frame
due to global attitude uncertainty. I can't tell if GTSAM has a
one-liner function to do that.

Alternatively, and what I normally do, is to define independent gtsam
variables for velocity in the local body frame, and add the required
factors so they behave as such.
I'm also interested in learning if other alternatives exist...

Cheers,
JL

> --
> You received this message because you are subscribed to the Google Groups "gtsam users" group.
> To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
> To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/d6ac6036-4418-4d99-bcf7-d40022cdff07n%40googlegroups.com.



--

/**
* Jose Luis Blanco-Claraco
* Universidad de Almería - Departamento de Ingeniería
* [Homepage]( https://w3.ual.es/~jlblanco/ )
* [GH profile]( https://github.com/jlblancoc )
*/

[Mike]

Hi JL, 

That makes sense though at a surface level it does seem odd that a frame transformation would reduce uncertainty(I believe it is possible - particularly through correlation stuff). 

I have since read other information that implies the returned velocity and pose3 covariance is in the body frame to begin with and not the world frame and the marginal function returns the body frame covariance. Do you know which it is for sure? Reading through the pose3 code I think I see the local conversion function generating a parameterization with respect to body-frame but velocity with the imu-preintegration factor is less clear. I also wasnt sure if the marginal function did additional frame transformation stuff.

 
Thanks  

[Dellaert, Frank]

GTSAM uses the concept of concentrated Gaussians to represent uncertainty, with

X ~ N(\hat X, P)

meaning a perturbation \xi is drawn from a zero-mean Gaussian and 

\xi ~ N(0,P), X = \hat X \exp(\xi)

This is for all​ manifold and groups in GTSAM. If X is Pose 3 or NavState, and represents the state of a vehicle in the navigation/world frame, then \xi above is indeed in the body frame already (as it appears on the right side of X).

That being said, in NavState, the velocity is the “velocity in world coordinates”, even though its uncertainty is represented as above. 

To convert velocity to the body frame, you’d use

b_v= bRn n_v = nRb' n_v

which is implemented in NavState::bodyVelocity:

```

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

const Rot3& nRb = R_;

const Vector3 n_v = t_.col(1);

Matrix3 D_bv_nRb;

Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0);

if (H)

*H << D_bv_nRb, Z_3x3, I_3x3;

return b_v;

}

```

As you can see, that has a 3x9 Jacobian H, so if your NavState has 9x9 covariance P, then the body velocity uncertainty is H*P*H’.

Note that the last block of H = I_3x3, so indeed the uncertainty on n_v is already in body frame, but​ there is a contribution from the rotation uncertainty which is non-zero.

Best

Frank

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/b970fcc1-d4c0-4c27-8f42-aa4f592465dbn%40googlegroups.com.

[Mike]

Hi Frank, 

Thanks for all the info - does this also apply when using separate Pose3 and Velocity3/Vector3 to represent the full system state?

I'm still a bit confused on velocity uncertainty being in body-frame already while to get body-frame velocity, you need to include an additional attitude term, something isn't really meshing in my mind since it seems that the in-body velocity uncertainty already exists as a term without the attitude component. Should the body-frame velocity uncertainty term(before the transform) be thought of as a literal uncertainty in body-frame velocity or does it mean something beyond this naive understanding? Taken by itself, what does it mean? 

Thanks again

[Dellaert, Frank]

Yes, because you’d need to use Rit3::unrotate, and I think you’ll find it does exactly the same thing :-)

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Mike <lowel...@gmail.com>

Sent: Sunday, March 22, 2026 9:37:25 PM

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/1e94ec19-9300-4f88-89e1-c67a1d151c3en%40googlegroups.com.

[Mike]

Thanks Frank,

I have been going through the source code slowly and I think I figured out one of the reasons I am confused. When using the combinedIMU factor the local coordinate parametrization changes depending upon if your state is at the start or end of the integration period. So for example if you have states V1, V2 and V3, V1 to V2 combinedIMU factor has V2 perturbation in V1 frame, but V2-V3 combinedIMU factor has V2 perturbation in V2 frame(though maybe there can't be a perturbation, since the frame always aligns with V2? - though the combinedIMU factor residual from V2-V3 is dependent upon both V2 and V3 which means V2 must have a local parametrization, what does it end up being?).

Basically I'm not seeing which frame V2 perturbations is actually in since it is present in both V1 frame and V2, the only thing which connects both frames is the V2 world state itself, so doesn't the marginalized parametrization have to be world frame? If not, how does the local parametrization in V1 frame make it into V2 frame or vice-versa? Each frames perturbation of V2 cant be independent variables since it is the same state, just in different reference frames.

Sorry this conversation has drawn out but I think it's valuable to me to understand what is happening.

Thanks

[Dellaert, Frank]

Sorry for the delay in replying, but perturbations live in values, not in factors. In gtsam these are always separated: factors specify the objective function, and values is an estimate of the unknown variables.  After you optimize, you can ask the marginals object for covariances on the perturbations.

By the way, this just has gotten a lot faster. See this PR: https://github.com/borglab/gtsam/pull/2476

I’ll also tweet about it this week…

Frank

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/56fb43af-1408-4f73-bd4a-241e4023b1f8n%40googlegroups.com.

[Mike]

Hi Frank,

Thanks - after reading the code more I understand I think. The key thing I didn't get before(correct me if I am wrong) is that in GTSAM velocity perturbation is applied after attitude perturbation and not before. This is different than the velocity returned by a sensor attached to the body which gives velocity with respect to the world in the true body aligned frame - in this sensor case one adds velocity noise before attitude noise. So when doing the conversion from GTSAM velocity uncertainty to body aligned frame velocity uncertainty, you need to incorporate this. 

I also see what you're saying about the factors, it just requires a little bit different differentiation formula depending on if you're the starting or ending state of the IMU pre integration.

Thanks for the help.