Set up for testing.
Added test code for position estimation.