Adding scale EKF