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.


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.


3 Answers 3


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


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.


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

  • 1
    $\begingroup$ Hi, please could you expand upon your answer and include the main relevant details of the link and a summary? This is because, if the link dies, then your answer will be, effectively, useless. $\endgroup$ Aug 1, 2018 at 12:21

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.