Possibly fixed EKF

Signed-off-by: Jake Reher <jake.reher@gmail.com>