EKF loop might be made simpler by having a flag to denote new data from sensors
EKF loop might be made simpler by having a flag to denote new data from sensors