Testing and Debugging Localization

Relevant Files

The main files of interest for the localization are World.lua, PoseFilter.lua, and the Config File. Note: We keep two localization config files - one for GRASP and one for SPL fields, since they have slightly different measurements.

The PoseFilter.lua file runs the particle filter as discussed in the Localization Overview and loads most parameters straight from the Config file. The World.lua file has higher level functions that get the correct information from odometry and vision, pass them into the particle filter, and then update the shared memory with the results. It also handles calling the correct particle re-initialization functions, updating the observed ball position, and setting various LEDs on the body. The Config file defines many parameters for the localization system including filter parameters and weights, locations of features in the world (corners, goals, spots, etc.), and initialization positions for different roles.

Key Parameters

Some of the important parameters to know about are:

• world.rXXXFilter - filter weight on distance update for feature XXX, a bigger number means an observation of this feature will update the xy pose estimate more.
• world.aXXXFilter - filter weight on angle update for feature XXX, a bigger number means an observation of this feature will update the angle pose estimate more. Setting either the r or a value to 0 will stop updates of this feature. So if you want either a or r to update but not both, set the one you don't want to update to a small weight (i.e. 0.0001). Also, note that the goalie can have different filter weights than field players.
• world.initPositionX - these lists specify starting locations on the field for specific roles depending on whose kickoff it is. The comments and ASCII art in the config file explain these pretty well.
• world.initPositionSidelines - this list contains the expected start location for each player number (and their default roles). When the localization system initializes the filter will be initialized at the specified location for each player, so make sure players get started in the right positions or they will be lost!!
• world.forceSidelinePose - Setting this to 1 will force the localization to reset to the specified sideline pose in world.initPositionSidelines whenever there is a localization reset. Very useful for debugging (see more below)
• world.odomScale - These set the scale for the odometry reporting. Since the locomotion module doesn't report odometry super accurately, tuning these can make a huge difference in the localization accuracy.

Tips and Tricks

Some tips for testing and debugging localization:

• Make sure the odometry scale values are tuned. One of the easiest ways to do this is to start the robot on the sideline with the Monitor running. Get two pieces of tape to cover the cameras so that the vision system will not do any updates. Manually walk the robot across the field until the Monitor shows that the robot has reached the other sideline (yes, just eyeball it, but make sure the field size is set correct, see below) and then measure how far the robot actually walked with a tape measure. Update the scale with new_scale = old_scale*actual_dist/expected_dist. Repeat similar experiments with walking sideways, backwards, and rotating. If you do each experiment a couple times, the scale will pretty quickly converge to a good value. You don't have to get it perfect, just reasonably close.
• Test localization both by manually walking the robot around and having it chase the ball with the main code. Testing that certain updates are working is easier with manual control, but the conditions with the robot walking itself are more representative of the game.
• If you are testing that a specific update is working properly, you can 0 out all the other update weights to check how well that single one works.
• When running the test_vision script (easily started with ./game v) pressing the 'z' key will cause the localization to be reset. If the world.forceSidelinePose flag is set, this will re-initialize the particles to the location specified in world.initPositionSidelines for the current player number.
• In the monitor, you can change the displayed field size by clicking on the bottom button on the far left near the field display. This can help visualize where the player is on the field more accurately.
• In the monitor, you can change the player field display to show the particles from the filter (click on the third from top button labeled Map 1 until it says Pview). This is very helpful for checking how updates affect particle spread or if there is divergence in the particles.
• If you don't care about seeing the particles, you can always use the Team Monitor or the GameState Visualizer to monitor the robots position wirelessly. This can be a lot easier than walking around tethered to the robot for long periods of time.
• Connect to the arb screen to see debug logs from the localization system. The localization system is also already integrated with the Logging system so feel free to add or modify logging commands as needed to help understand what is going on.
• Some old, but still relevant information about some of the World/Localization code can be found here