both steps of EKF are running now....almost...side by side.
minor changes and parameter adjustance during testing
H matrix is now been calculated