considerable work towards Simple K and new UKF

Simple Kalman has better Results than old UKF
This is due to some errors that were in the implementation
new UKF uses a better Quaternion handling, but doesnt show a good
response in OT stream