Stopping barrel rolls

[Zack]

I have a non linear factor graph setup with between factors for odometry measurements and GPS factors attached at a lower frequency. I initialise the node heading using an approximation with the GPS but set the pitch and roll to 0 with sigma errors in the range of 2.5 -> 10. My sensors are on a ground vehicle so pitch and roll should typically stay close to 0.

My graph is fine with the odometry on its own and the XY components look good when I optimise together with GPS. However, my angles can spiral out of control. At times the roll keeps on increasing (to about 2000 degrees). 

Is this a commonly observed artefact? If so, what are some means of handling it? I am presuming there is too much freedom for the optimiser. I thought about adding a PartialPriorFactor to each node setting pitch and roll to 0, with some degree of error to see if I can reduce this.

Any advice? 

[Dellaert, Frank]

If you have an IMU that measures gravity there is no reason this should happen. If no IMU, you could use the "AttitudeFactor". Look at its unit tests for usage examples.

FD

---

From: 'Zack' via gtsam users <gtsam...@googlegroups.com>
Sent: Tuesday, August 17, 2021 05:02
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Stopping barrel rolls

 

--
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/77d0d3d6-ce1d-417c-9817-949a6982dffan%40googlegroups.com.

[Zack]

Ah I see that makes sense that gravity would prevent that. I do have an IMU but I wanted to build up by graph slowly (odometry only first, then with GPS, then with IMU) to make sure I had each step correct. Sounds like a good time to start working on adding the IMU into the graph too. Thanks I will give it a go!