External Tracking / NavState estimation with no IMU

[Asa Hammond]

Hello all.

I am modeling a new tracking problem and looking to get pointed in the best, modern direction for this type of graph. Basically I want to estimate a NavState off of unary pose3 estimates streaming in at a high rate. There will likely be other sensor streams fused in later, but for now this is the sole source of measurements. I see there are some older projects in gtsam_unstable/dynamics for building up a velocity constraint between PoseRTV, but there are notes about its deprecation etc. It seems that expressions would be the bees-knees way to attack this now, but I am not finding any examples that have timestamps / velocity / accel  or numeric derivatives in their graphs.  I have had fantastic results using the PreIntegratedImu factors for velocity estimation in previous projects, but alas, no IMU in this scenario. 

What's the cleanest way to go after estimating dynamics these days? I want to constrain the evolution of the system to some simple constant velocity or constant acceleration models / limits.

An IncrementalFixedLagSmoother with numericalDerivatives22 between X(0) and X(1) for a prior on V(1)?

A NavState expression of wonder? (expressions are very new to me)

I am primarily looking for robustness to dropped/outlier measurements, and predicting states at arbitrary times (and accuracy and speed of course).

Any and all pointers appreciated!

Thank you kindly.

Asa

[Dellaert, Frank]

Asa,

I recently felt the need for a constant velocityfactor myself:-). For prototyping, you can do it with expressionFactors, but I think the most performant will be a custom NonlinearFactor<NavState,NavState> parameterized with dt and possibly angular velocity from a gyro, if available. It would be similar to BetweenFactor.

Would love a PR with unit test in navigation sub folder :-) if you want it in python wrapper (I would!), add it in gtsam.i 

Frank

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Asa Hammond <asaha...@gmail.com>
Sent: Wednesday, February 17, 2021 5:19:45 PM
To: gtsam...@googlegroups.com <gtsam...@googlegroups.com>
Subject: [GTSAM] External Tracking / NavState estimation with no IMU

 

--
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 on the web visit https://groups.google.com/d/msgid/gtsam-users/CAEHOJK-noNC3tSyjd3aFS-7ozFh1g1DTO7fSewuPy6NvM4zTaA%40mail.gmail.com.