search

AtsushiSakai / PythonRobotics

Python sample codes for robotics algorithms.
https://atsushisakai.github.io/PythonRobotics/
Other
23.14k stars 6.51k forks source link

Graph SLAM edge angle error term seems incorrect. #134

Closed ZhenghaoFei closed 5 years ago

ZhenghaoFei commented 5 years ago

Hi,

I saw you used the following equations to calculate angle error term in an edge.

hyaw = phi1 - phi2 + angle1 - angle2
edge.e[2, 0] = pi_2_pi(yaw2 - yaw1 - hyaw)

However, phi is a noise-free measurement angle from: def observation(xTrue, xd, u, RFID): Which should not be obtained. Can you please check?

Happy Thanksgiving!

AtsushiSakai commented 5 years ago

@ZhenghaoFei Thank you for your comment. Oh. you are right. I fixed it. Thanks.