mc_configuration *conf_now = motor->m_conf;

float R = conf_now->foc_motor_r;
float L = conf_now->foc_motor_l;
float lambda = conf_now->foc_motor_flux_linkage;

// Saturation compensation
const float comp_fact = conf_now->foc_sat_comp * (motor->m_motor_state.i_abs_filter / conf_now->l_current_max);
L -= L * comp_fact;
lambda -= lambda * comp_fact;

// Temperature compensation
if (conf_now->foc_temp_comp) {
R = motor->m_res_temp_comp;
}

float ld_lq_diff = conf_now->foc_motor_ld_lq_diff;
float id = motor->m_motor_state.id;
float iq = motor->m_motor_state.iq;

// Adjust inductance for saliency.
if (fabsf(id) > 0.1 || fabsf(iq) > 0.1) {
L = L - ld_lq_diff / 2.0 + ld_lq_diff * SQ(iq) / (SQ(id) + SQ(iq));
}

const float L_ia = L * i_alpha;
const float L_ib = L * i_beta;
const float R_ia = R * i_alpha;
const float R_ib = R * i_beta;
const float lambda_2 = SQ(lambda);
const float gamma_half = motor->m_gamma_now * 0.5;

switch (conf_now->foc_observer_type) {
case FOC_OBSERVER_ORTEGA_ORIGINAL: {
float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));

// Forcing this term to stay negative helps convergence according to
//
// http://cas.ensmp.fr/Publications/Publications/Papers/ObserverPermanentMagnet.pdf
// and
// https://arxiv.org/pdf/1905.00833.pdf
if (err > 0.0) {
err = 0.0;
}

float x1_dot = v_alpha - R_ia + gamma_half * (*x1 - L_ia) * err;
float x2_dot = v_beta - R_ib + gamma_half * (*x2 - L_ib) * err;

*x1 += x1_dot * dt;
*x2 += x2_dot * dt;
} break;

default:
break;
}

UTILS_NAN_ZERO(*x1);
UTILS_NAN_ZERO(*x2);

// Prevent the magnitude from getting too low, as that makes the angle very unstable.
float mag = NORM2_f(*x1, *x2);
if (mag < (conf_now->foc_motor_flux_linkage * 0.5)) {
*x1 *= 1.1;
*x2 *= 1.1;
}