# lqr

Generic [Linear-quadratic regultar (LQR) controller](https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator) 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.

[![Crates.io](https://img.shields.io/crates/v/lqr.svg)](https://crates.io/crates/lqr)
![Build Status](https://gitlab.com/imgeorgiev/lqr/badges/master/pipeline.svg)

## Minimal example

Controls a car with a simple [kinematic bicycle model](https://dingyan89.medium.com/simple-understanding-of-kinematic-bicycle-model-81cac6420357)

```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::<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;
```