diff --git a/Cargo.lock b/Cargo.lock
index a320926..6127173 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -2,6 +2,241 @@
 # It is not intended for manual editing.
 version = 4
 
+[[package]]
+name = "approx"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "autocfg"
+version = "1.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53"
+
+[[package]]
+name = "bytemuck"
+version = "1.25.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797"
+
+[[package]]
+name = "glam"
+version = "0.30.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "19fc433e8437a212d1b6f1e68c7824af3aed907da60afa994e7f542d18d12aa9"
+
+[[package]]
+name = "glam"
+version = "0.31.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "556f6b2ea90b8d15a74e0e7bb41671c9bdf38cd9f78c284d750b9ce58a2b5be7"
+
+[[package]]
+name = "glam"
+version = "0.32.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f70749695b063ecbf6b62949ccccde2e733ec3ecbbd71d467dca4e5c6c97cca0"
+
+[[package]]
+name = "glam"
+version = "0.33.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "37bfe73dc8ec21f54d181e5a60554327ec3f1c600c455a75ccbfc65d2106a6c4"
+
 [[package]]
 name = "lyapunov"
 version = "0.1.0"
+dependencies = [
+ "nalgebra",
+ "uom",
+]
+
+[[package]]
+name = "lyapunov-demo"
+version = "0.1.0"
+dependencies = [
+ "lyapunov",
+ "nalgebra",
+ "uom",
+]
+
+[[package]]
+name = "matrixmultiply"
+version = "0.3.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3f607c237553f086e7043417a51df26b2eb899d3caff94e6a67592ff992fedc7"
+dependencies = [
+ "autocfg",
+ "rawpointer",
+]
+
+[[package]]
+name = "nalgebra"
+version = "0.35.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "adc43a60c217b0c6ff46e47f26911015ad8d2e5a8be1af668c67e370d99a4346"
+dependencies = [
+ "approx",
+ "glam 0.30.10",
+ "glam 0.31.1",
+ "glam 0.32.1",
+ "glam 0.33.12",
+ "matrixmultiply",
+ "nalgebra-macros",
+ "num-complex",
+ "num-rational",
+ "num-traits",
+ "simba",
+ "typenum",
+]
+
+[[package]]
+name = "nalgebra-macros"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "num-bigint"
+version = "0.4.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c89e69e7e0f03bea5ef08013795c25018e101932225a656383bd384495ecc367"
+dependencies = [
+ "num-integer",
+ "num-traits",
+]
+
+[[package]]
+name = "num-complex"
+version = "0.4.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-integer"
+version = "0.1.47"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7ce2d95d4b3734dc35aa2f45e1aa22cd416814592a4f9d9205e11affd5b8e10b"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-rational"
+version = "0.4.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824"
+dependencies = [
+ "num-bigint",
+ "num-integer",
+ "num-traits",
+]
+
+[[package]]
+name = "num-traits"
+version = "0.2.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841"
+dependencies = [
+ "autocfg",
+]
+
+[[package]]
+name = "proc-macro2"
+version = "1.0.107"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "985e7ec9bb745e6ce6535b544d84d6cd6f7ad8bd711c398938ae983b91a766d9"
+dependencies = [
+ "unicode-ident",
+]
+
+[[package]]
+name = "quote"
+version = "1.0.47"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1fbf4db142a473a8d80c26bbf18454ed458bf8d26c8219c331daecfdbd079001"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "rawpointer"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
+
+[[package]]
+name = "safe_arch"
+version = "1.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "42c6efa15875e6ecb39ca61fb0b0c1a40b84fac5a5ffe71eef7d1000c8eb3f5f"
+dependencies = [
+ "bytemuck",
+]
+
+[[package]]
+name = "simba"
+version = "0.10.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f1a7200d82ff1c7b4235efd12f11e2537a402c91cf83a5cb97ce80eef787fde7"
+dependencies = [
+ "approx",
+ "num-complex",
+ "num-traits",
+ "wide",
+]
+
+[[package]]
+name = "syn"
+version = "2.0.119"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "872831b642d1a07999a962a351ed35b955ea2cfc8f3862091e2a240a84f17297"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-ident",
+]
+
+[[package]]
+name = "typenum"
+version = "1.20.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b6f5e870be6c3b371b77fe0ee0bafb859fa4964b4404c27de1d380043c4dda20"
+
+[[package]]
+name = "unicode-ident"
+version = "1.0.27"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a2c754d6c33795a1c324727428e5a7dedb5b06195f9890bdbcba760d3e246563"
+
+[[package]]
+name = "uom"
+version = "0.38.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a739f83872836c82a4f2527d4e54b37007b3de68cafe7edde95fd695968bf4b9"
+dependencies = [
+ "num-traits",
+ "typenum",
+]
+
+[[package]]
+name = "wide"
+version = "1.7.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d920ac99c3c8edce110cb8d07dbb324d6d026011dce85b1e9355b70f0adacc4f"
+dependencies = [
+ "bytemuck",
+ "safe_arch",
+]
diff --git a/Cargo.toml b/Cargo.toml
index 2bdcebf..aeba5ee 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,6 +1,7 @@
-[package]
-name = "lyapunov"
-version = "0.1.0"
-edition = "2024"
+[workspace]
+resolver = "2"
+members = ["crates/*"]
 
-[dependencies]
+[workspace.package]
+edition = "2024"
+version = "0.1.0"
diff --git a/crates/lyapunov-demo/Cargo.toml b/crates/lyapunov-demo/Cargo.toml
new file mode 100644
index 0000000..b02401e
--- /dev/null
+++ b/crates/lyapunov-demo/Cargo.toml
@@ -0,0 +1,9 @@
+[package]
+name = "lyapunov-demo"
+version.workspace = true
+edition.workspace = true
+
+[dependencies]
+lyapunov = { path = "../lyapunov" }
+nalgebra = "0.35"
+uom = "0.38"
diff --git a/crates/lyapunov-demo/src/main.rs b/crates/lyapunov-demo/src/main.rs
new file mode 100644
index 0000000..9674ae3
--- /dev/null
+++ b/crates/lyapunov-demo/src/main.rs
@@ -0,0 +1,30 @@
+use lyapunov::{Controller, Integrator, LinearFeedback, LinearSystem, Rk4};
+use nalgebra::SVector;
+use uom::si::f64::Time;
+use uom::si::time::millisecond;
+
+fn main() {
+    let system = LinearSystem::<2, 1>::double_integrator(1.0, -0.1);
+    let u_ff = system
+        .constant_cancelling_input()
+        .unwrap_or_else(SVector::zeros);
+
+    let p = LinearFeedback::<2, 1>::from_gains([2.0, 3.0]);
+
+    let mut x = SVector::<f64, 2>::new(10.0, 0.0);
+    let r = SVector::<f64, 2>::new(0.0, 0.0);
+    let dt = Time::new::<millisecond>(50.0);
+
+    for step in 0..200 {
+        let t = dt * (step as f64);
+
+        let u_fb = p.calculate(&system, t, &x, &r);
+        let u = u_fb + u_ff;
+        x = Rk4.step(&system, t, &x, &u, dt);
+
+        println!(
+            "t={:.2}s  u={:.3}  pos={:.3}m  vel={:.3}m/s",
+            t.value, u[0], x[0], x[1],
+        );
+    }
+}
diff --git a/crates/lyapunov/Cargo.toml b/crates/lyapunov/Cargo.toml
new file mode 100644
index 0000000..cd9921c
--- /dev/null
+++ b/crates/lyapunov/Cargo.toml
@@ -0,0 +1,8 @@
+[package]
+name = "lyapunov"
+version.workspace = true
+edition.workspace = true
+
+[dependencies]
+uom = "0.38"
+nalgebra = "0.35"
diff --git a/crates/lyapunov/src/controllers/linear_feedback.rs b/crates/lyapunov/src/controllers/linear_feedback.rs
new file mode 100644
index 0000000..5f7040c
--- /dev/null
+++ b/crates/lyapunov/src/controllers/linear_feedback.rs
@@ -0,0 +1,39 @@
+use nalgebra::{SMatrix, SVector};
+use uom::si::f64::Time;
+
+use crate::traits::{Controller, System};
+
+pub struct LinearFeedback<const N: usize, const M: usize> {
+    pub gain_matrix: SMatrix<f64, M, N>,
+}
+
+impl<S, const N: usize, const M: usize> Controller<S, N, M> for LinearFeedback<N, M>
+where
+    S: System<N, M, State = SVector<f64, N>, Input = SVector<f64, M>, Derivative = SVector<f64, N>>,
+    S::State: Copy + std::ops::Sub<Output = S::State>,
+{
+    fn calculate(&self, _system: &S, _t: Time, x: &S::State, r: &S::State) -> S::Input {
+        let error = *r - *x;
+        self.gain_matrix * error
+    }
+}
+
+impl<const N: usize> LinearFeedback<N, 1> {
+    pub fn from_gains(gains: [f64; N]) -> Self {
+        let mut gain_matrix = SMatrix::<f64, 1, N>::zeros();
+        for (j, &g) in gains.iter().enumerate() {
+            gain_matrix[(0, j)] = g;
+        }
+        Self { gain_matrix }
+    }
+}
+
+impl<const N: usize> LinearFeedback<N, N> {
+    pub fn diagonal(gains: [f64; N]) -> Self {
+        let mut gain_matrix = SMatrix::<f64, N, N>::zeros();
+        for i in 0..N {
+            gain_matrix[(i, i)] = gains[i];
+        }
+        Self { gain_matrix }
+    }
+}
diff --git a/crates/lyapunov/src/controllers/mod.rs b/crates/lyapunov/src/controllers/mod.rs
new file mode 100644
index 0000000..a1091e0
--- /dev/null
+++ b/crates/lyapunov/src/controllers/mod.rs
@@ -0,0 +1 @@
+pub mod linear_feedback;
diff --git a/crates/lyapunov/src/integrators/mod.rs b/crates/lyapunov/src/integrators/mod.rs
new file mode 100644
index 0000000..b8911b4
--- /dev/null
+++ b/crates/lyapunov/src/integrators/mod.rs
@@ -0,0 +1,5 @@
+pub mod rk4;
+pub mod traits;
+
+pub use rk4::Rk4;
+pub use traits::Accumulate;
diff --git a/crates/lyapunov/src/integrators/rk4.rs b/crates/lyapunov/src/integrators/rk4.rs
new file mode 100644
index 0000000..e87f08b
--- /dev/null
+++ b/crates/lyapunov/src/integrators/rk4.rs
@@ -0,0 +1,34 @@
+use uom::si::f64::Time;
+
+use crate::{
+    integrators::traits::Accumulate,
+    traits::{Integrator, System},
+};
+
+pub struct Rk4;
+
+impl<S, const N: usize, const M: usize> Integrator<S, N, M> for Rk4
+where
+    S: System<N, M>,
+    S::State: Clone + Accumulate<S::Derivative>,
+{
+    fn step(&self, system: &S, t: Time, x: &S::State, u: &S::Input, dt: Time) -> S::State {
+        let half_dt = dt * 0.5;
+
+        let k1 = system.derivatives(t, x, u);
+
+        let x2 = x.add_scaled(half_dt, &k1);
+        let k2 = system.derivatives(t + half_dt, &x2, u);
+
+        let x3 = x.add_scaled(half_dt, &k2);
+        let k3 = system.derivatives(t + half_dt, &x3, u);
+
+        let x4 = x.add_scaled(dt, &k3);
+        let k4 = system.derivatives(t + dt, &x4, u);
+
+        x.add_scaled(dt / 6.0, &k1)
+            .add_scaled(dt / 3.0, &k2)
+            .add_scaled(dt / 3.0, &k3)
+            .add_scaled(dt / 6.0, &k4)
+    }
+}
diff --git a/crates/lyapunov/src/integrators/traits.rs b/crates/lyapunov/src/integrators/traits.rs
new file mode 100644
index 0000000..5e77074
--- /dev/null
+++ b/crates/lyapunov/src/integrators/traits.rs
@@ -0,0 +1,12 @@
+use nalgebra::SVector;
+use uom::si::f64::Time;
+
+pub trait Accumulate<D> {
+    fn add_scaled(&self, dt: Time, derivative: &D) -> Self;
+}
+
+impl<const N: usize> Accumulate<SVector<f64, N>> for SVector<f64, N> {
+    fn add_scaled(&self, dt: Time, derivative: &SVector<f64, N>) -> Self {
+        self + dt.get::<uom::si::time::second>() * derivative
+    }
+}
diff --git a/crates/lyapunov/src/lib.rs b/crates/lyapunov/src/lib.rs
new file mode 100644
index 0000000..c2cdb72
--- /dev/null
+++ b/crates/lyapunov/src/lib.rs
@@ -0,0 +1,9 @@
+pub mod controllers;
+pub mod integrators;
+pub mod systems;
+pub mod traits;
+
+pub use controllers::linear_feedback::LinearFeedback;
+pub use integrators::rk4::Rk4;
+pub use systems::linear_system::LinearSystem;
+pub use traits::{Controller, Integrator, System};
diff --git a/crates/lyapunov/src/systems/linear_system.rs b/crates/lyapunov/src/systems/linear_system.rs
new file mode 100644
index 0000000..feb16b3
--- /dev/null
+++ b/crates/lyapunov/src/systems/linear_system.rs
@@ -0,0 +1,47 @@
+use crate::traits::System;
+use nalgebra::{DMatrix, DVector, SMatrix, SVector};
+use uom::si::f64::Time;
+
+pub struct LinearSystem<const N: usize, const M: usize> {
+    pub state_matrix: SMatrix<f64, N, N>,
+    pub input_matrix: SMatrix<f64, N, M>,
+    pub constant: SVector<f64, N>,
+}
+
+impl<const N: usize, const M: usize> LinearSystem<N, M> {
+    pub fn constant_cancelling_input(&self) -> Option<SVector<f64, M>> {
+        let constant_cancel = -self.constant;
+
+        let input_matrix_dyn = DMatrix::from_column_slice(N, M, self.input_matrix.as_slice());
+        let constant_cancel_dyn = DVector::from_column_slice(constant_cancel.as_slice());
+
+        let svd = input_matrix_dyn.svd(true, true);
+        let input_dyn = svd.solve(&constant_cancel_dyn, 1e-10).ok()?;
+
+        let mut input = SVector::<f64, M>::zeros();
+        for i in 0..M {
+            input[i] = input_dyn[i];
+        }
+        Some(input)
+    }
+}
+
+impl<const N: usize, const M: usize> System<N, M> for LinearSystem<N, M> {
+    type State = SVector<f64, N>;
+    type Input = SVector<f64, M>;
+    type Derivative = SVector<f64, N>;
+
+    fn derivatives(&self, _t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative {
+        self.state_matrix * x + self.input_matrix * u + self.constant
+    }
+}
+
+impl LinearSystem<2, 1> {
+    pub fn double_integrator(input_gain: f64, disturbance: f64) -> Self {
+        Self {
+            state_matrix: SMatrix::<f64, 2, 2>::from_row_slice(&[0.0, 1.0, 0.0, 0.0]),
+            input_matrix: SMatrix::<f64, 2, 1>::from_column_slice(&[0.0, input_gain]),
+            constant: SVector::<f64, 2>::from_column_slice(&[0.0, -disturbance]),
+        }
+    }
+}
diff --git a/crates/lyapunov/src/systems/mod.rs b/crates/lyapunov/src/systems/mod.rs
new file mode 100644
index 0000000..69683f8
--- /dev/null
+++ b/crates/lyapunov/src/systems/mod.rs
@@ -0,0 +1,3 @@
+pub mod linear_system;
+
+pub use linear_system::LinearSystem;
diff --git a/crates/lyapunov/src/traits.rs b/crates/lyapunov/src/traits.rs
new file mode 100644
index 0000000..dc3c815
--- /dev/null
+++ b/crates/lyapunov/src/traits.rs
@@ -0,0 +1,28 @@
+use uom::si::f64::*;
+
+// Represents a dynamical system with state dimension N and input dimension M.
+pub trait System<const N: usize, const M: usize> {
+    type State;
+    type Input;
+    type Derivative;
+
+    // Evaluate the derivative vector x_dot at (t, x, u)
+    // TODO Create time-invariant system trait?
+    fn derivatives(&self, t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative;
+}
+
+// Computes the evolution of a system over (t, x, u)
+pub trait Integrator<S, const N: usize, const M: usize>
+where
+    S: System<N, M>,
+{
+    // Performs a single simulation step of size dt for the system
+    fn step(&self, system: &S, t: Time, x: &S::State, u: &S::Input, dt: Time) -> S::State;
+}
+
+pub trait Controller<S, const N: usize, const M: usize>
+where
+    S: System<N, M>,
+{
+    fn calculate(&self, system: &S, t: Time, x: &S::State, r: &S::State) -> S::Input;
+}
diff --git a/src/main.rs b/src/main.rs
deleted file mode 100644
index e7a11a9..0000000
--- a/src/main.rs
+++ /dev/null
@@ -1,3 +0,0 @@
-fn main() {
-    println!("Hello, world!");
-}