pub fn lqr_compute(err_x: f64, err_v: f64, k1: f64, k2: f64) -> f64
Linear Quadratic Regulator (LQR) feedback control calculation (-K * x)