Add (commented-out) accelerometer filtering test
Clean up and organize