EM seems to be working. Need to save the params to a file
and clean up the code. Some of the functionality will be useful
in the other programs using the sensor model