Initial code for additional systematic odometry error states.
When  I set the uncertainty real low, appears to work, but havent tested
extensively yet. Still need to add robustifying code.