Noise removal, data management

Removes initial measurement noise and also adds smoothing for the
estimated KF angles. Some commenting and an updated TO TO list.