New additions for EKF 3 state. Not Tested
Tested EKF 5 State, which still works mediocre-ly.
Need to get automated testing!!
3 state EKF uns without any run time error. It sucks though, and everything else sucks too
Tuning parameters are as good as I've ever found for 5-state EKF
Merge branch 'master' into ekf_3state
New 3-state Extended Kalman Filter works with GPS and lever arm offset
Tuned the 5-state EKF too!