Code and figure for EKF benchmarking