An algorithm for state estimation of nonlinear systems according to minimum disturbance energy criterion