Performed some good tests to show the UKF works even for the stageK dataset
Implemented and tested the UKF that estimates k alongside px, py and theta. It provided better results than the previous version.
Finished testing the first version of UKF. Next step is switch to the dual quaternion representation
Implemented the dual quaternion representation UKF, but it is too slow