NavState velocity constraint

[Radu Cioaca]

Hello,

I am trying to use as much as possible the NavState to take advantage of the "2" factors. 

My question is what is the best way to constrain the individual components of the NavState (Pos, Rot, Vel) with unary factors. For example the GpsFactor2 constrains the Pos part of the NavState. 

But if I also get the velocity measurement from the Gps receiver how can I add a unary factor to constrain the velocity inside the NavState?

Should I create my own factor for this or there are other ways to interact with a variable node that has a NavState as value?

Right now, the only way to constrain the NavState is with GpsFactor2 and ImuFactor2, but I need more flexibility than that.

Thank you!

[Dellaert, Frank]

A very easy way to go is to add an NavState prior with very loose variances on the position and rotation

Best!

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Radu Cioaca <cioac...@gmail.com>
Sent: Tuesday, August 19, 2025 10:48:37 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] NavState velocity constraint

 

You don't often get email from cioac...@gmail.com. Learn why this is important

--
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/a92c07e0-7989-47c9-9b98-a41a48be013cn%40googlegroups.com.

[Radu Cioaca]

That's great. Thank you very much Frank!

[Brice Rebsamen]

It's also pretty easy to use expression factors to extract one part of a variable (e.g. extract velocity from navstate) and make it equate to a measured velocity... It's like 20 lines of code for the factor itself, mostly boilerplate code.

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/78a9d36e-78c5-45c7-b48b-7196e9466f7bn%40googlegroups.com.