Function lqr_step 

pub fn lqr_step(
    coefficients: &Coefficients,
    cycle: u32,
    reference: i32,
    position: i32,
    velocity: i32,
) -> Result<Command, InvalidCoefficients>

Apply full-state discrete LQR and a reference prefilter, then saturate output.

The kernel computes floor((nrr-kxy-kv*v)/2^24). Supplied gains need an independently recorded Riccati design and quantized closed-loop stability check. Invalid coefficients do not yield a command.