diff --git a/src/integration.zig b/src/integration.zig
index 7e5cf65..1e7578c 100644
--- a/src/integration.zig
+++ b/src/integration.zig
@@ -36,7 +36,7 @@ pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *dro
     );
 
     const qdot = vec3.quatMul(half_omege_quat, s.q);
-    s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt));
+    s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt)).normalized();
     return;
 }
 
@@ -76,4 +76,13 @@ test "Validate timesteps" {
     try std.testing.expectApproxEqAbs(0, state.x_cg.x(), 1e-12);
     try std.testing.expectApproxEqAbs(0, state.x_cg.y(), 1e-12);
     try std.testing.expectApproxEqAbs(-0.24975483375, state.x_cg.z(), 1e-11);
+
+    try std.testing.expectApproxEqAbs(0, state.omega.x(), 1e-12);
+    try std.testing.expectApproxEqAbs(50, state.omega.y(), 1e-12);
+    try std.testing.expectApproxEqAbs(2.5, state.omega.z(), 1e-11);
+
+    try std.testing.expectApproxEqAbs(0, state.q.x(), 1e-12);
+    try std.testing.expectApproxEqAbs(0.12403234937465502, state.q.y(), 1e-12);
+    try std.testing.expectApproxEqAbs(0.006201617468732751, state.q.z(), 1e-11);
+    try std.testing.expectApproxEqAbs(0.9922587949972401, state.q.w(), 1e-11);
 }