diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock
index 6962b6c4..a7e17d7a 100644
--- a/firmware/Cargo.lock
+++ b/firmware/Cargo.lock
@@ -362,6 +362,15 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "dcmimu"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "692b1c8923315ac22103c1890512597baf543b95f2eebd66f572ffb1ff8f0170"
+dependencies = [
+ "libm 0.1.4",
+]
+
 [[package]]
 name = "defmt"
 version = "0.3.2"
@@ -916,6 +925,7 @@ dependencies = [
  "color-eyre",
  "cortex-m",
  "cortex-m-rt",
+ "dcmimu",
  "defmt",
  "defmt-bbq",
  "defmt-rtt",
@@ -1133,6 +1143,12 @@ version = "0.2.139"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "201de327520df007757c1f0adce6e827fe8562fbc28bfd9c15571c66ca1f5f79"
 
+[[package]]
+name = "libm"
+version = "0.1.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7fc7aa29613bd6a620df431842069224d8bc9011086b1db4c0e0cd47fa03ec9a"
+
 [[package]]
 name = "libm"
 version = "0.2.6"
@@ -1213,7 +1229,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "78ad9db142f17e7e850abb7089a683396a7c5e89cb3b51d5512ded0b844551e4"
 dependencies = [
  "embedded-hal 0.2.7",
- "libm",
+ "libm 0.2.6",
 ]
 
 [[package]]
@@ -1374,7 +1390,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd"
 dependencies = [
  "autocfg",
- "libm",
+ "libm 0.2.6",
 ]
 
 [[package]]
diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml
index 85f967ab..9cb55278 100644
--- a/firmware/Cargo.toml
+++ b/firmware/Cargo.toml
@@ -194,6 +194,9 @@ defmt-bbq = { version = "0.1", optional = true }
 mpu6050-dmp = "0.3"
 bmi160 = "0.1"
 
+# Sensor fusion
+dcmimu = "0.2"
+
 # Other crates
 static_cell = "1"
 nb = "1"
diff --git a/firmware/src/imu/fusion/dcm.rs b/firmware/src/imu/fusion/dcm.rs
new file mode 100644
index 00000000..2a2caff1
--- /dev/null
+++ b/firmware/src/imu/fusion/dcm.rs
@@ -0,0 +1,40 @@
+use dcmimu::DCMIMU;
+use embassy_time::Instant;
+use firmware_protocol::ImuType;
+
+use crate::imu::{FusedData, Fuser, Imu, Quat, UnfusedData};
+
+/// Extended Kalman filtering in direction cosine matrix formation
+pub struct Dcm {
+	dcm: DCMIMU,
+	last: Instant,
+}
+
+impl Dcm {
+	pub fn new() -> Self {
+		Self {
+			dcm: DCMIMU::new(),
+			last: Instant::now(),
+		}
+	}
+}
+
+impl Fuser for Dcm {
+	fn process(&mut self, unfused: &UnfusedData) -> FusedData {
+		let last = self.last;
+		self.last = Instant::now();
+		let elapsed = self.last - last;
+
+		let UnfusedData { accel, gyro } = unfused;
+
+		// TODO: Check that these euler angle convention matches
+		let (euler, _) = self.dcm.update(
+			(gyro.x, gyro.y, gyro.z),
+			(accel.x, accel.y, accel.z),
+			elapsed.as_micros() as f32 / 1_000_000.0,
+		);
+
+		let q = Quat::from_euler_angles(euler.roll, euler.pitch, euler.roll);
+		FusedData { q }
+	}
+}
diff --git a/firmware/src/imu/fusion/mod.rs b/firmware/src/imu/fusion/mod.rs
index 359db1c2..ee70a67d 100644
--- a/firmware/src/imu/fusion/mod.rs
+++ b/firmware/src/imu/fusion/mod.rs
@@ -1 +1 @@
-//! TODO: Add fusion
+pub mod dcm;