Project 2: Localization
Introduction
Eyesight is key for human navigation. It allows us to move quickly and safely through hazardous environments without running into walls, other people, or stumbling into the Sunlab and losing all hope of ever seeing daylight again. Knowing one's location and how to move through an environment safely is equally as important to robotic agents. This is done through a process known as localization. If our equipment and sensors were perfect, this problem would be simply, you would simply find the point on the map that perfectly matched your data. Then, whenever you made a movement, you would simply update your position by that measured movement. However, instruments are not perfect, occasionally someone will walk in front of a sensor, giving it a false reading, or it will simply fail. Localization techniques must not only account for unknowns, but also errors in given data. Through this project, you will only explore the localization part of Simultaneous Localization And Mapping (SLAM), implementing two distinct method of localization, the Particle Filter and the Kalman Filter, to determine your location within the CIT and other maps.
Files & Setup
Setup
Run cs141_install localization to retrieve the stencil code and data for this project. Note: This project makes use of the Python libraries Numpy, Scipy, and Matplotlib. If you are not working on department machines, you may need to install these to complete the project.
Files
There are a number of python files provided for you. They are detailed below.
utils.py Contains all TA support code except the Kalman featurization. featurization.py Contains the Kalman featurization procedure. localization.py This is the file in which you will implement your solution. It contains commented stubs for all function you must implement. You are free to add any helpers you wish, but cannot modify the signatures of the stubbed functions.
Additionally, there is a testing directory that contains several subdirectories, each of which contains sample data for testing. This will be discussed in the testing section.
Running Everything
We have provided a command line interface for ease of use and testing.
Command line flags:
There are a number of python files provided for you. They are detailed below.
| utils.py | Contains all TA support code except the Kalman featurization. |
| featurization.py | Contains the Kalman featurization procedure. |
| localization.py | This is the file in which you will implement your solution. It contains commented stubs for all function you must implement. You are free to add any helpers you wish, but cannot modify the signatures of the stubbed functions. |
Additionally, there is a testing directory that contains several subdirectories, each of which contains sample data for testing. This will be discussed in the testing section.
We have provided a command line interface for ease of use and testing.
Command line flags:
| -l | Specify a grayscale map to load. Default is the CIT map. NOTE: Map should contain only values of 0 and 1 (black and white) |
| -c | Specify a file containing the input controls. |
| -m | Specify a file containing the input measurement observations. |
| -s | Specify a file containing the initial states for the Particle Filter. (If this flag is not included, your starting state generation function will be called) |
| -n | Specify the number of initial states to generate for the Particle Filter. |
| -v | Turn on the visualizer |
Implementation
Throughtout this document and the support code, we will use the following variables
| M | The number of measurements per timestep |
| N | The number of starting states |
| T | The number of timesteps |
| X | The width of the input map |
| Y | The height of the input map |
Particle Filter
The first form of localization that we will explore is the Particle Filter. A particle filter works by choosing many possible initial states, filtering out the unlikely ones, applying noise (to account for error) to the more likely positions, and repeating. Our particle filter will take the following inputs:
| start_states | The starting positions for the particle filter*.This argument is an Nx3 Numpy array. Each starting state consists of an x position, a y position, and a phi, or heading, in that order. |
| controls | The input controls for the robotic agent as it moves through it's environment. This argument is an Tx2 Numpy array. Each control consists of a degree change (with respect to the current phi) and a velocity (which is coordinate distance covered per timestep). |
| measurements | The input measurements the robotic agent receives from its sensors at each timestep. A measurment is taken after each control action is taken, there is no initial measurement for the starting positions. This arguments is a TxMx2 Numpy array. Each timestep contains a set of measurements, one (or several) from each sensor. Each measurement consists of a degree offset from the current phi and a measured distance. |
| lidar_map | A grayscale map that you are attempting to localize in. Every position is marked with either a 0 (empty) or a 1 (occupied). The map is accessed by lidar_map[y,x] (Miles doesn't like this at all, thoughts?). |
| vis=False | A flag that can turn a visualizer on. Helpful for debugging. |
We recommend implementing the Particle Filter in the following steps.
1) Implement the generate_init_states function. Given a map and an integer N, return an Nx3 Numpy array of starting states. Please ensure that every starting state is an unoccupied space. For ease of implementation, you can limit your initial states to being whole numbers. You should randomly generate a phi in the range [0,360).
NOTE: You should be able to choose the same x,y coordinates multiple times.
2) Implement the update_state function. Given a state and a control, modify the current state to account for the specified movement. You should not add any noise at this stage.
At this stage, you can modify the particle_filter function slightly to see your particles move around the screen. But this isn't very useful, because we never select any of the points.
3) Implement the measurement probability function. Given a map, a state, and a set of measurements, determine the probability of seeing the measurements you saw, if you were truly at the input state. Use a Gaussian distribution with a mean of 0 and a variance of 1 to calculate the probability of the difference between the observed and true measurements. Don't forget to normalize the weights when they are returned.
Now you can see the color change in the points as their weight changes.
We now have likelihoods for each point, but we never account for error, which is a key part of the functionality of a particle filter.
4) Implement the resample function. Given an array of states and an array of weights, randomly sample N states. (NOTE: there should be overlap).
If you run the particle_filter function now, you will see points start to vanish. So now we are selecting out likely points, but we still fail to account for noise.
5) Modify your resample function to add Gaussian noise to every point you sample. You should add a sample from a Gaussian with a mean of 0 and a variance of 1 to both the x and y position, as well as a sample from a Gaussian with a mean of 0 and variance of 0.1*pi to the phi.
6) Combine all your functions to create a fully functioning particle filter. Don't forget to test it!
TASK: Why do we add noise to each sample? What does this do for us? Does it help account for error in measurement and controls? How?
No comments:
Post a Comment