Pulled the system update into a dedicated subfunction
Moved the system update BEFORE the GPS measurement update.
This will hopfully pin down the update timing so it is not dependent on the unreliable ROS timer (??)
Added debugging stuff for the 7-State EKF.
Everything seems to work now??