Integrating GPS into Graph SLAM (how orientation fixed?)

Question

I'm working on Graph SLAM to estimate robot poses (x, y, z, roll, pitch, yaw). Now I want to integrate GPS measurement (x, y, z, of course no angles).

I implemented GPS as pose's prior. But I have a problem.

• Position(x, y, z) is perfectly corrected by graph optimization
• But orientaiton(roll, pitch, yaw) is very unpredictable(unstable) after optization.

i.e. It looks like position is fitted by the sacrifice of orientation.

I'm very confused about what's the right way of integrating GPS into graph SLAM. GPS should be handled as prior? or landmark? or one of pose-vertices?

...Thanks for your help in advance.

PS

I use g2o as a graph-optimization library. In g2o, I implemented GPS measurement with EdgeSE3_Prior. GPS's quality is RTK so it's enough precise.

Answer 1

You can use a very low information matrix value at the orientation elements of your state, given that the information matrix is the inverse of the covariance matrix.

The covariance matrix represents the uncertainty about the measurement, and the information matrix the certainty about it.

So, the GPS constraints would have a small value at the elements of the information matrix corresponding to the orientation

Answer 2

Frankly speaking, I don't have any experience with implementing a SLAM algorithm. That said, GPS gives you a position and SLAM gives you a position. My advice would be to use a data fusion technique, such as a Kalman filter or maybe even the Madgwick filter, to combine the position estimates.

Answer 3

You can try this paper "The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures"