Simplified expression, removed EKF.

Added helper function dot3 to compute A*B*C.

Removed the ekf code in the predict step as we will write a separate
EKF filter.
Moved tests out of main file.
Implented RKS Smoother. Not sure on correctness yet.