Added basic motion model

The quadcopter can now go to goal using the net thrust vector. Also
added class to keep track of the state of each motor (not used yet).