fix EKF update covariance

Signed-off-by: cheesinglee <cheesinglee@gmail.com>