Resample

Fixed up the resampling algorithms, now program can compile, run and
resample without crashing. Next is simulation motion using uncertainty.