Monitoring for localization failures

[tbohne]

• our localization: IMU + odometry + GNSS -> Kalman filter to bring these information together (robot_localization pkg)
• what I need: measure quality of localization
• in absolute values, you can't know that -> there's no way of knowing that..
• what you could do: compare to other localization method - BUT, we have no other (if we'd have a redundant system, we could compare the results)
  • e.g. parallel landmark localization
  • e.g. fixed measured (known) reference points that the robot could go to and compare to its localization
  • however, we don't have these options atm
• all we can do: estimate plausibility of localization based on sensor data
  • GNSS is the simplest, if it jumps a lot, it's probably bad (already part of connection monitoring -> describe that)
  • not so easy for IMU + odometry
  • for IMU: stop (stand still) and check whether IMU has zero values -> take known states and check values
  • for odometry: don't send any control values to the motors -> odom should stand still as well
• drive 1m straight based on GPS and compare to odometry result
• if you move, you get one GPS trajectory and one odometry trajectory
• compare those two -> how much do they diverge? don't delve too deep into this topic -> it's hard

[tbohne]

GNSS is the easiest - start there!

• is quite good to monitor and often so accurate that you can use it to check the others
• e.g. if GNSS says we drove 10m straight and odom says 15m
• I get these information from the corresponding topics -> all part of the msgs
• visualizing GPS in rViz is not so easy because it's lat / lng
  • robot_localization publishes on odometry_gps or gps_filtered or something: these are the pure GNSS data, but transformed as odometry data (PoseStamped) - just the format
• odometry can be visualized in rViz: small arrows building up a trajectory - there's a topic for that..
• then there's filtered odometry (using IMU) - result of robot_localization: KF fusions odom + IMU
• that means what is different in filtered_odom is due to the IMU
• just check those three: unfortunately, it's pretty usual that they diverge heavily

[tbohne]

• strength of odom: almost continuous - if you want to track your position continuously
  • GPS, on the other hand, only provides every n seconds new info
  • odom is very good when you just drive straight ahead
  • trajectory mostly very accurate regarding distance, but very inaccurate with curves (e.g. you drive to the left about 90°, but odom shows only 45° -> rest of the trajectory points in completely wrong direction)
• strength of GPS: very good at measuring / estimating the ABSOLUTE position
  • however, does not have an orientation AT ALL -> with one GPS value, you don't get any orientation and you only get data every few seconds, kind of a low frequency
  • therefore, a fine-grained navigation is not possible only based on GPS, you'd have to wait for each new data after moving a short distance
• IMU provides quite good information about CHANGE, but no absolute values
  • could try to integrate IMU acceleration values and compute velocity vlaues
  • integrate that again and compute distances
  • however, gets very inaccurate -> numerically unstable
• that's why you commonly combine those three sensor types - weaknesses and strengths balance each other out quite well
• GPS, for example, can be used to correct the completely wrong directions of the odom trajectory after curves (+ IMU to improve)

[tbohne]

• generally, localization is hard
• best thing to start with: how accurate is my GPS atm?
  • based on how many satellites I see + how many jumps do I have?
  • e.g. just log the last 5 satellite information and check how much they "jump"
  • basically the thing that Google Maps visualizes with the blue circle in pedestrian mode
  • in cars, it aligns the position quite well with streets, but that's not possible for pedestrians
  • blue circle sometimes bigger / sometimes smaller -> basically the standard deviation of the position (or variance?)
  • essentially how much the positions are spread -> the center (your position) is just the average
  • the statement then is: I think you are at your current position, but I get information from this whole circle
  • so, what you could check: is the variance getting bigger atm?
  • would be very relevant to notice: variance in centimeter or in 20m range
  • with RTK we could even get to millimeter range and with GPS only at bad day 20m are possible and everything in between
  • e.g. take the information (GPS points) from the topics, compute variance -> compute average

[tbohne]

orientation would be even more important for us

• try: do my sensor data fit to the map I have? -> aligned?
• how many sensor points do I see that are not part of the map? -> define metric
• only works when near objects that are part of the map
• e.g. you drive around a wall that is part of the map - you would expect to see sensor information aligning with the wall..
• basically relying on knowledge of the map
• but again: hard, could be an own thesis topic..

[tbohne]

What I could try:

• when I know GPS is very good atm (e.g. very low variance), you could deduce that we have RTK
• then you could check: Ok, I get the following trajectory based on the good GPS data, GPS itself doesn't give me an orientation, but if I drive from A to B, I can interpolate and assume that I probably faced the direction AB and not to the left or right
• this estimated orientation can be compared to odometry -> difference of trajectories
• then it's a defined limit question: is the divergence acceptable / too high?

[tbohne]

mapping errors: actually you would have to generate your map completely new -> do I have a mapping error or an incorrect localization is closely related -> maybe I am localized incorrectly because I have a mapping error -> fundamental question behind it, which will not be solved completely in the work