Vaccarella Fusion implemented

Vaccarella needs debugging with invalid points
CircleUKF needs debugging with Orientation
P-Matrices ened to be inverted more stable, maybe catch warning