Code in middle of refactoring
Updated to include motion model.
Updated code.  Does not compile currently, in process of reworking.