add surf* tests and edits in code...at this point

By the end of last night's changes, what is being tested is a P
controller with trajectory generation and correct measurement filtering.