Refactoring of UKF

Mostly renamed variables to be more reasonable named. Moved the scaled UKF
into its own file.