EKF3核心算法