lqr

Generic Linear-quadratic regultar (LQR) controller in Rust heavily optimized with nalgebra. This can be used as a feedback controller/ trajectory tracker for all kinds of dynamical systems and has been real-world tested with cars and quadcopters.

Minimal example

Controls a car with a simple kinematic bicycle model

// Define state
let x = 2.0;
let y = 2.0;
let theta = 0.34;
let v = 3.0;

// Define controls
let delta = 0.0;
let acc = 0.0;

// Model parameters
let l = 2.0; // wheelbase

// compute matrices for the LQR controller
let a = na::Matrix4::<f64>::new(0.0, 0.0, -v * theta.sin(), theta.cos(),
                                0.0, 0.0, v * theta.cos(),  theta.sin(),
                                0.0, 0.0, 0.0,              delta.tan() / l,
                                0.0, 0.0, 0.0,              0.0);

let b = na::Matrix4x2::<f64>::new(0.0, 0.0,
                                  0.0, 0.0,
                                  v / (l * delta.cos().powf(2.0)), 0.0,
                                  0.0, 1.0);

let q = na::Matrix4::identity();
let r = na::Matrix2::identity();

let mut controller = LQRController::new()?;
controller.compute_gain(&a, &b, &q, &r, 1e-6)?;

// Set states for the optimal control computation
let current_state = na::Vector4::<f64>::new(x, y, theta, v);
let desired_state = na::Vector4::<f64>::new(x + 1.0, y, theta + 0.01, 3.0);

let u_feedforward = na::Vector2::<f64>::new(acc, delta);
let u_feedback = controller.compute_optimal_controls(&current_state, &desired_state)?;
let u = u_feedforward + u_feedback;