Update EKFFilter.cpp

slight fix