Added encoder noise scaling
Parameter tuning is mediocre.
Starting to revise EKF for 3-state functionality