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

```rust // 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::::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::::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 currentstate = na::Vector4::::new(x, y, theta, v); let desiredstate = na::Vector4::::new(x + 1.0, y, theta + 0.01, 3.0);

let ufeedforward = na::Vector2::::new(acc, delta); let ufeedback = controller.computeoptimalcontrols(&currentstate, &desiredstate)?; let u = ufeedforward + ufeedback; ```