Added unitests to UnscentedKalmanFilter Class