BYU CS classes

To understand particle filters, and to gain experience with debugging likelihoods.

You may wish to install the python pyqtgraph package. This can be installed via conda install pyqtgraph. This is a plotting package that is significantly faster than matplotlib when scatter plotting many points (ie, particles).

For this lab, you will need the following files:

sensors.mat contains the data you will use (you can also regenerate it directly, using the script below).

lab8_common.py contains utility functions that will enable you to create a simple map, and compute what a robot would see from a given position and angle.

lab8_gen_sonars.py is the script that I used to generate the data for this lab. If you run it, it will generate a new dataset, but it will also produce a nice visualization of the robot as it moves around, along with what the sensors measure.

It can be frustrating to experiment with a particle filter when the interface is too slow. By using pyqtgraph instead of matplotlib, you can speed things up considerably.

Here is some code to create a pyqtgraph window, and show the map.

    from pyqtgraph.Qt import QtGui, QtCore
    import pyqtgraph as pg
 
    app = QtGui.QApplication( [] )
    win = pg.GraphicsWindow( title="Particle filter" )
    win.resize( 600, 600 )
    win.setWindowTitle( 'Particle filter' )
    pg.setConfigOptions( antialias=True )
 
    p3 = win.addPlot( title="Room map" )
 
    for i in range( 0, room_map.shape[0] ):
        p3.plot( [room_map[i,0], room_map[i,2]], [room_map[i,1], room_map[i,3]] )
    p3.setXRange( -0.1, 1.1 )
    p3.setYRange( -0.1, 1.1 ) 

To speed things up further, you should use the .setData method on your plots. This replaces the data used for a particular plotting element, and is faster than deleting and recreating the plot element. This is particularly important when trying to update many particles.

Here's a snippet of my code. ts_plot and ex_plot are initialized to None. My particles are stored in a 3xN matrix called parts, my expected states are stored in a 3xt matrix called exs.

        if ts_plot == None: 
            ts_plot = p3.plot( true_states[0,0:t+1], true_states[1,0:t+1], pen=(0,0,255) )
            ex_plot = p3.plot( exs[0,0:t+1], exs[1,0:t+1], pen=(0,255,0) )
            pts = p3.scatterPlot( parts[0,:], parts[1,:], symbol='o', size=1, pen=(255,100,100) )
 
        else:
            ts_plot.setData( true_states[0,0:t+1], true_states[1,0:t+1] )
            ex_plot.setData( exs[0,0:t+1], exs[1,0:t+1] )
            pts.setData( parts[0,:], parts[1,:] )