gotta commit some time. Still having the "loop around" problem with the kalman filter responses, meaning the error is going something special
just added settling time as per Alans suggestion. Appears to vastly improve initial KF state but state is still explosive over time
Oh god make it stop