diff --git a/README.md b/README.md
index 63362496..bc589ed9 100644
--- a/README.md
+++ b/README.md
@@ -18,9 +18,11 @@ Crazyflow is a research simulator for quadrotors. It runs batched, differentiabl
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=4096, n_drones=1, control=Control.state)
-cmd = np.zeros((4096, 1, 13))
+cmd = np.zeros((4096, 1, 16))
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[..., 2] = 0.5  # hover at 0.5 m across all worlds
 
 for _ in range(100):
diff --git a/benchmark/performance.py b/benchmark/performance.py
index 1d7c49d0..6e42ba30 100644
--- a/benchmark/performance.py
+++ b/benchmark/performance.py
@@ -1,13 +1,17 @@
 from __future__ import annotations
 
+import os
 from typing import TYPE_CHECKING
 
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import gymnasium
 import jax
 import numpy as np
 from ml_collections import config_dict
 from pyinstrument import Profiler
 from pyinstrument.renderers.html import HTMLRenderer
+from scipy.spatial.transform import Rotation as R
 
 import crazyflow  # noqa: F401, ensure gymnasium envs are registered
 from crazyflow.sim import Sim
@@ -19,9 +23,11 @@
 def profile_step(sim_config: config_dict.ConfigDict, n_steps: int, device: str):
     sim = Sim(**sim_config)
     device = jax.devices(device)[0]
-    ndim = 13 if sim.control == "state" else 4
+    ndim = 16 if sim.control == "state" else 4
     control_fn = sim.state_control if sim.control == "state" else sim.attitude_control
     cmd = np.zeros((sim.n_worlds, sim.n_drones, ndim))
+    if sim.control == "state":
+        cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     # Ensure JIT compiled dynamics and control
     sim.reset()
     control_fn(cmd)
diff --git a/benchmark/splat.py b/benchmark/splat.py
index 2e130b63..eded62cb 100644
--- a/benchmark/splat.py
+++ b/benchmark/splat.py
@@ -24,12 +24,14 @@
 # splax rasterizes with warp, which needs GPU memory outside JAX's pool. Disable JAX preallocation
 # before it initializes so both share the device. Must run before the first jax import.
 os.environ["XLA_PYTHON_CLIENT_PREALLOCATE"] = "false"
+os.environ["SCIPY_ARRAY_API"] = "1"
 
 import fire
 import jax
 import jax.numpy as jnp
 import numpy as np
 from jax.errors import JaxRuntimeError
+from scipy.spatial.transform import Rotation as R
 from splax.io import fetch
 
 from crazyflow.sim import Sim
@@ -118,7 +120,8 @@ def benchmark(
 
             # Hold a constant target so the drone keeps moving and each frame renders a distinct
             # pose. A static scene would let XLA hoist the render out of the loop.
-            cmd = np.zeros((sim.n_worlds, sim.n_drones, 13), dtype=np.float32)
+            cmd = np.zeros((sim.n_worlds, sim.n_drones, 16), dtype=np.float32)
+            cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
             cmd[..., 2] = 0.5
             sim.reset()
             sim.state_control(jnp.asarray(cmd, device=sim.device))
diff --git a/crazyflow/control/core.py b/crazyflow/control/core.py
index ccd50dd7..6c18d67e 100644
--- a/crazyflow/control/core.py
+++ b/crazyflow/control/core.py
@@ -38,10 +38,12 @@ def parametrize(
     import numpy as np
     from crazyflow.control import parametrize
     from crazyflow.control.mellinger import state2attitude
+    from scipy.spatial.transform import Rotation as R
 
     ctrl = parametrize(state2attitude, "cf2x_L250")
     pos, quat = np.zeros(3), np.array([0.0, 0.0, 0.0, 1.0])
-    vel, cmd = np.zeros(3), np.zeros(13)
+    vel, cmd = np.zeros(3), np.zeros(16)
+    cmd[9:13] = R.from_euler("z", 0.0).as_quat()
     rpyt, int_pos_err = ctrl(pos, quat, vel, cmd)
     ```
 
@@ -85,14 +87,14 @@ class Control(StrEnum):
     """Control type of the simulated onboard controller."""
 
     state = "state"
-    """State control takes [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate].
-    
+    """State control takes [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz].
+
     Note:
         Recommended frequency is >=20 Hz.
 
     Warning:
-        Currently, we only use positions, velocities, and yaw. The rest of the state is ignored.
-        This is subject to change in the future.
+        Only the yaw of the attitude quaternion is used, as in the firmware. The so_rpy family
+        ignores the body rate setpoint.
     """
     attitude = "attitude"
     """Attitude control takes [roll, pitch, yaw, collective thrust].
@@ -101,7 +103,7 @@ class Control(StrEnum):
         Recommended frequency is >=100 Hz.
     """
     body_rate = "body_rate"
-    """Body rate control takes [roll_rate, pitch_rate, yaw_rate, collective thrust].
+    """Body rate control takes [wx, wy, wz, collective thrust].
 
     Note:
         Recommended frequency is >=200 Hz.
diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py
index 384b6dc8..6e05d7b4 100644
--- a/crazyflow/control/mellinger/control.py
+++ b/crazyflow/control/mellinger/control.py
@@ -58,8 +58,9 @@ def state2attitude(
         pos: Drone position with shape (..., 3).
         quat: Drone orientation as xyzw quaternion with shape (..., 4).
         vel: Drone velocity with shape (..., 3).
-        cmd: Full state command in SI units and rad with shape (..., 13). The entries are
-            [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate].
+        cmd: Full state command in SI units with shape (..., 16). The entries are
+            [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. Only the yaw of the
+            attitude quaternion is used. The body rates are forwarded to the attitude controller.
         pos_err_i: Position integral error (..., 3) from the previous call. If None, it is
             initialised to zero.
         ctrl_freq: Control frequency in Hz
@@ -83,9 +84,8 @@ def state2attitude(
     setpoint_pos = cmd[..., 0:3]
     setpoint_vel = cmd[..., 3:6]
     setpoint_acc = cmd[..., 6:9]
-    setpoint_yaw = cmd[..., 9]
+    setpoint_quat = cmd[..., 9:13]
     dt = 1 / ctrl_freq
-    # setpointRPY_rates = cmd[..., 10:13]
     # From firmware controller_mellinger
     pos_err = setpoint_pos - pos  # l. 145 Position Error (ep)
     vel_err = setpoint_vel - vel  # l. 148 Velocity Error (ev)
@@ -100,7 +100,7 @@ def state2attitude(
     )
     # l. 178 Rate-controlled YAW is moving YAW angle setpoint
     # => only one case here, since the setpoint is always in absolute mode
-    desired_yaw = setpoint_yaw
+    desired_yaw = R.from_quat(setpoint_quat).as_euler("xyz")[..., 2]
     # l. 189 Z-Axis [zB]
     rot = R.from_quat(quat).as_matrix()
     z_axis = rot[..., -1]  # 3rd column or roation matrix is z axis
@@ -489,13 +489,12 @@ def force_torque2rotor_vel(
 
 @dataclass
 class MellingerStateData:
-    cmd: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 13)
+    cmd: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 16)
     """Full state control command for the drone.
 
-    A command consists of [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate].
-    We currently do not use the acceleration and angle rate components. This is subject to change.
+    A command consists of [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz].
     """
-    staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 13)
+    staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 16)
     """Staging buffer to store the most recent command until the next controller tick."""
     steps: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, 1)
     """Last simulation steps that the state control command was applied."""
@@ -512,12 +511,12 @@ def create(
     ) -> MellingerStateData:
         """Create a default set of state data for the simulation."""
         zeros_3d = jnp.zeros((n_worlds, n_drones, 3), device=device)
-        zeros_13d = jnp.zeros((n_worlds, n_drones, 13), device=device)
+        cmd = jnp.zeros((n_worlds, n_drones, 16), device=device).at[..., 12].set(1.0)
         steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device)
         params = load_params(state2attitude, drone, xp=jnp, device=device)
         return MellingerStateData(
-            cmd=zeros_13d.copy(),
-            staged_cmd=zeros_13d.copy(),
+            cmd=cmd,
+            staged_cmd=cmd.copy(),
             steps=steps,
             freq=freq,
             pos_err_i=zeros_3d.copy(),
@@ -528,20 +527,21 @@ def create(
 @dataclass
 class MellingerAttitudeData:
     cmd: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 4)
-    """Full attitude control command for the drone.
-
-    A command consists of [roll, pitch, yaw, collective thrust].
-    """
+    """Attitude control setpoint consisting of [roll, pitch, yaw, collective thrust]."""
     staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 4)
     """Staging buffer to store the most recent command until the next controller tick."""
+    ang_vel_des: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 3)
+    """Body rate setpoint [wx, wy, wz] of the attitude controller."""
+    staged_ang_vel_des: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 3)
+    """Staging buffer to store the most recent body rate setpoint until the next controller tick."""
     steps: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, 1)
     """Last simulation steps that the attitude control command was applied."""
     freq: int = field(pytree_node=False)
     """Frequency of the attitude control command."""
     r_int_error: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 3)
     """Integral errors of the attitude control command."""
-    last_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 3)
-    """Last angular velocity of the drone."""
+    prev_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 3)
+    """Previous angular velocity of the drone."""
     # Parameters for the attitude controller
     params: dict[str, Array]
 
@@ -557,10 +557,12 @@ def create(
         return MellingerAttitudeData(
             cmd=zeros_4d.copy(),
             staged_cmd=zeros_4d.copy(),
+            ang_vel_des=zeros_3d.copy(),
+            staged_ang_vel_des=zeros_3d.copy(),
             steps=steps,
             freq=freq,
             r_int_error=zeros_3d.copy(),
-            last_ang_vel=zeros_3d.copy(),
+            prev_ang_vel=zeros_3d.copy(),
             params=params,
         )
 
@@ -570,7 +572,7 @@ class MellingerBodyRateData:
     cmd: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 4)
     """Body rate control command for the drone.
 
-    A command consists of [roll_rate, pitch_rate, yaw_rate, collective thrust].
+    A command consists of [wx, wy, wz, collective thrust].
     """
     staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 4)
     """Staging buffer to store the most recent command until the next controller tick."""
@@ -580,8 +582,8 @@ class MellingerBodyRateData:
     """Frequency of the body rate control command."""
     r_int_error: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 3)
     """Integral errors of the body rate control command."""
-    last_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 3)
-    """Last angular velocity of the drone."""
+    prev_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1})  # (N, M, 3)
+    """Previous angular velocity of the drone."""
     # Parameters for the body rate controller
     params: dict[str, Array]
 
@@ -600,7 +602,7 @@ def create(
             steps=steps,
             freq=freq,
             r_int_error=zeros_3d.copy(),
-            last_ang_vel=zeros_3d.copy(),
+            prev_ang_vel=zeros_3d.copy(),
             params=params,
         )
 
@@ -650,7 +652,9 @@ def control_state2attitude(data: SimData) -> SimData:
         **state_ctrl.params,
     )
     state_ctrl = leaf_replace(state_ctrl, mask, steps=data.core.steps, pos_err_i=pos_err_i)
-    attitude_ctrl = leaf_replace(data.controls.attitude, mask, staged_cmd=rpyt)
+    attitude_ctrl = leaf_replace(
+        data.controls.attitude, mask, staged_cmd=rpyt, staged_ang_vel_des=state_ctrl.cmd[..., 13:16]
+    )
     return data.replace(controls=data.controls.replace(state=state_ctrl, attitude=attitude_ctrl))
 
 
@@ -660,21 +664,30 @@ def control_attitude2force_torque(data: SimData) -> SimData:
     attitude_ctrl: MellingerAttitudeData = data.controls.attitude
     assert attitude_ctrl is not None, "Using attitude controller without initialized data"
     mask = controllable(data.core.steps, data.core.freq, attitude_ctrl.steps, attitude_ctrl.freq)
-    attitude_ctrl = leaf_replace(attitude_ctrl, mask, cmd=attitude_ctrl.staged_cmd)
-    force, torque, r_int_error = attitude2force_torque(
+    prev_ang_vel_des = attitude_ctrl.ang_vel_des
+    attitude_ctrl = leaf_replace(
+        attitude_ctrl,
+        mask,
+        cmd=attitude_ctrl.staged_cmd,
+        ang_vel_des=attitude_ctrl.staged_ang_vel_des,
+    )
+    force, torque, r_int_error = _attitude2force_torque(
         states.quat,
         states.ang_vel,
-        attitude_ctrl.cmd,
-        r_int_error=attitude_ctrl.r_int_error,
-        ctrl_freq=attitude_ctrl.freq,
-        prev_ang_vel=attitude_ctrl.last_ang_vel,
+        attitude_ctrl.cmd[..., :3],
+        attitude_ctrl.ang_vel_des,
+        attitude_ctrl.cmd[..., 3],
+        attitude_ctrl.prev_ang_vel,
+        prev_ang_vel_des,
+        attitude_ctrl.r_int_error,
+        attitude_ctrl.freq,
         **attitude_ctrl.params,
     )
     attitude_ctrl = leaf_replace(
         attitude_ctrl,
         mask,
         r_int_error=r_int_error,
-        last_ang_vel=states.ang_vel,
+        prev_ang_vel=states.ang_vel,
         steps=data.core.steps,
     )
     ft_ctrl = leaf_replace(
@@ -697,7 +710,7 @@ def control_body_rate2force_torque(data: SimData) -> SimData:
         states.quat,
         states.ang_vel,
         body_rate_ctrl.cmd,
-        prev_ang_vel=body_rate_ctrl.last_ang_vel,
+        prev_ang_vel=body_rate_ctrl.prev_ang_vel,
         prev_cmd=prev_cmd,
         r_int_error=body_rate_ctrl.r_int_error,
         ctrl_freq=body_rate_ctrl.freq,
@@ -707,7 +720,7 @@ def control_body_rate2force_torque(data: SimData) -> SimData:
         body_rate_ctrl,
         mask,
         r_int_error=r_int_error,
-        last_ang_vel=states.ang_vel,
+        prev_ang_vel=states.ang_vel,
         steps=data.core.steps,
     )
     ft_ctrl = leaf_replace(
diff --git a/crazyflow/sim/functional.py b/crazyflow/sim/functional.py
index 7a592f2b..0bc193d3 100644
--- a/crazyflow/sim/functional.py
+++ b/crazyflow/sim/functional.py
@@ -16,7 +16,7 @@
 def state_control(data: SimData, controls: Array) -> SimData:
     """State control function."""
     assert data.controls.mode == Control.state, f"control type {data.controls.mode} not enabled"
-    assert controls.shape == (data.core.n_worlds, data.core.n_drones, 13), "controls shape mismatch"
+    assert controls.shape == (data.core.n_worlds, data.core.n_drones, 16), "controls shape mismatch"
     controls = jnp.asarray(controls)
     data = data.replace(
         controls=data.controls.replace(state=data.controls.state.replace(staged_cmd=controls))
diff --git a/docs/get-started/quick-start.md b/docs/get-started/quick-start.md
index 745b13f4..04b19fd9 100644
--- a/docs/get-started/quick-start.md
+++ b/docs/get-started/quick-start.md
@@ -17,29 +17,29 @@ sim.reset()
 
 ## State and command
 
-The default control mode is `Control.state`. A state command is a 13-element vector that sets the desired position, velocity, acceleration, yaw, and body angular rates.
+The default control mode is `Control.state`. A state command is a 16-element vector that sets the desired position, velocity, acceleration, attitude, and body rates.
 
 | Index | Variable | Units |
 |---|---|---|
 | 0–2 | Position \(x, y, z\) | m |
 | 3–5 | Velocity \(\dot{x}, \dot{y}, \dot{z}\) | m/s |
 | 6–8 | Acceleration \(\ddot{x}, \ddot{y}, \ddot{z}\) | m/s² |
-| 9 | Yaw | rad |
-| 10 | Roll rate | rad/s |
-| 11 | Pitch rate | rad/s |
-| 12 | Yaw rate | rad/s |
+| 9–12 | Attitude quaternion \(q_x, q_y, q_z, q_w\) | |
+| 13–15 | Body rates \(\omega_x, \omega_y, \omega_z\) | rad/s |
 
-The command array has shape `(n_worlds, n_drones, 13)`.
+Only the yaw of the attitude quaternion is used, as in the firmware. The setpoint must contain a valid quaternion. The command array has shape `(n_worlds, n_drones, 16)`.
 
 ```python
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=1, n_drones=1, freq=500, control=Control.state)
 sim.reset()
 
-cmd = np.zeros((1, 1, 13), dtype=np.float32)
+cmd = np.zeros((1, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[0, 0, 2] = 0.5  # target height: 0.5 m
 ```
 
@@ -51,11 +51,13 @@ cmd[0, 0, 2] = 0.5  # target height: 0.5 m
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=1, n_drones=1, freq=500, control=Control.state)
 sim.reset()
 
-cmd = np.zeros((1, 1, 13), dtype=np.float32)
+cmd = np.zeros((1, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[0, 0, 2] = 0.5
 
 for _ in range(10):
@@ -71,11 +73,13 @@ All simulation state lives in `sim.data.states`. Arrays are indexed as `[world,
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=1, n_drones=1, freq=500, control=Control.state)
 sim.reset()
 
-cmd = np.zeros((1, 1, 13), dtype=np.float32)
+cmd = np.zeros((1, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[0, 0, 2] = 0.5
 
 for _ in range(10):
@@ -96,11 +100,13 @@ Increase `n_worlds` to run independent simulations in a single batched call. All
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=4, n_drones=1, freq=500, control=Control.state)
 sim.reset()
 
-cmd = np.zeros((4, 1, 13), dtype=np.float32)
+cmd = np.zeros((4, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[:, 0, 2] = np.array([0.2, 0.4, 0.6, 0.8])  # different target heights per world
 
 for _ in range(10):
@@ -112,17 +118,19 @@ pos = sim.data.states.pos[:, 0, :]  # (4, 3) — position of drone 0 in each wor
 
 ## Simulate multiple drones
 
-Increase `n_drones` to place multiple drones inside a single world. Each drone has its own independent state; all receive commands from the same `(n_worlds, n_drones, 13)` array.
+Increase `n_drones` to place multiple drones inside a single world. Each drone has its own independent state; all receive commands from the same `(n_worlds, n_drones, 16)` array.
 
 ```python
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=1, n_drones=4, freq=500, control=Control.state)
 sim.reset()
 
-cmd = np.zeros((1, 4, 13), dtype=np.float32)
+cmd = np.zeros((1, 4, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[0, :, 2] = np.array([0.2, 0.4, 0.6, 0.8])  # different height per drone
 
 for _ in range(10):
diff --git a/docs/index.md b/docs/index.md
index ac9f7616..522ed396 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -225,12 +225,14 @@ See [Installation](get-started/installation.md) for GPU, developer, and from-sou
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=1, n_drones=1, control=Control.state)
 sim.reset()
 
-# State command: [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate]
-cmd = np.zeros((1, 1, 13), dtype=np.float32)
+# State command: [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]
+cmd = np.zeros((1, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[0, 0, 2] = 0.5  # hover at 0.5 m
 
 sim.state_control(cmd)
diff --git a/docs/user-guide/control/batching.md b/docs/user-guide/control/batching.md
index 4d12c91c..ad9f2187 100644
--- a/docs/user-guide/control/batching.md
+++ b/docs/user-guide/control/batching.md
@@ -6,6 +6,7 @@ All controllers are built on Array API operations that broadcast over leading di
 import numpy as np
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250")
 
@@ -13,7 +14,8 @@ N = 100
 pos = np.zeros((N, 3))
 quat = np.tile(np.array([0.0, 0.0, 0.0, 1.0]), (N, 1))
 vel = np.zeros((N, 3))
-cmd = np.zeros((N, 13))
+cmd = np.zeros((N, 16))
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 
 rpyt, int_pos_err = ctrl(pos, quat, vel, cmd)
 rpyt.shape  # (100, 4)
@@ -27,6 +29,7 @@ Any number of leading dimensions works. A common pattern is a grid of environmen
 import numpy as np
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250")
 
@@ -34,7 +37,8 @@ ctrl = parametrize(state2attitude, "cf2x_L250")
 pos = np.zeros((10, 5, 3))
 quat = np.broadcast_to(np.array([0.0, 0.0, 0.0, 1.0]), (10, 5, 4)).copy()
 vel = np.zeros((10, 5, 3))
-cmd = np.zeros((10, 5, 13))
+cmd = np.zeros((10, 5, 16))
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 
 rpyt, _ = ctrl(pos, quat, vel, cmd)
 rpyt.shape  # (10, 5, 4)
diff --git a/docs/user-guide/control/controllers.md b/docs/user-guide/control/controllers.md
index 2dd8891b..66f28aa2 100644
--- a/docs/user-guide/control/controllers.md
+++ b/docs/user-guide/control/controllers.md
@@ -12,7 +12,7 @@ The Mellinger controller [[1]](#references) is split into three stages that form
 
 | Stage | Function | Takes | Produces |
 |---|---|---|---|
-| 1 | [`state2attitude`](mellinger.md#state-to-attitude) | State + 13-element setpoint | RPYT command + position integral error |
+| 1 | [`state2attitude`](mellinger.md#state-to-attitude) | State + 16-element setpoint | RPYT command + position integral error |
 | 2 | [`attitude2force_torque`](mellinger.md#attitude-to-force-torque) | Attitude + RPYT command | Collective force, body torques + angular velocity integral error |
 | 3 | [`force_torque2rotor_vel`](mellinger.md#force-torque-to-rotor-velocities) | Force + torques | 4 motor speeds [RPM] |
 
diff --git a/docs/user-guide/control/index.md b/docs/user-guide/control/index.md
index 5fafdbd8..12d5e597 100644
--- a/docs/user-guide/control/index.md
+++ b/docs/user-guide/control/index.md
@@ -4,12 +4,12 @@ Crazyflow provides multiple control modes, from high-level position setpoints do
 
 ## Control hierarchy
 
-Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger controller; an attitude command is converted to force/torque by the geometric controller; force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control.
+Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger position controller, the attitude command is converted to force/torque by the geometric controller, and force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control. The rate setpoint in the state command is forwarded to the rate controller.
 
 ```
-State (13D)
-  └─ Mellinger controller
-       └─ Attitude (4D: roll, pitch, yaw, thrust)  |  Body rates (4D: ωx, ωy, ωz, thrust)
+State (16D)
+  └─ Mellinger position controller
+       └─ Attitude (4D: roll, pitch, yaw, thrust) + body rates (3D: ωx, ωy, ωz)  |  Body rates (4D: ωx, ωy, ωz, thrust)
             └─ Geometric controller
                  └─ Force/torque (4D: Fc, Tx, Ty, Tz)
                       └─ Mixer
@@ -28,29 +28,31 @@ sim = Sim(control=Control.state, state_freq=100, attitude_freq=500)
 sim.reset()
 ```
 
-Command shape: `(n_worlds, n_drones, 13)`
+Command shape: `(n_worlds, n_drones, 16)`
 
 | Index | Variable | Units |
 |---|---|---|
 | 0–2 | Target position \(x, y, z\) | m |
 | 3–5 | Target velocity \(\dot{x}, \dot{y}, \dot{z}\) | m/s |
 | 6–8 | Target acceleration \(\ddot{x}, \ddot{y}, \ddot{z}\) | m/s² |
-| 9 | Yaw | rad |
-| 10 | Roll rate | rad/s |
-| 11 | Pitch rate | rad/s |
-| 12 | Yaw rate | rad/s |
+| 9–12 | Attitude quaternion \(q_x, q_y, q_z, q_w\) | |
+| 13–15 | Body rates \(\omega_x, \omega_y, \omega_z\) | rad/s |
 
-Set unused elements to zero. A common hover command sets only the z position:
+As in the firmware's full state setpoint, only the yaw of the attitude quaternion is used. The body rates are the angular velocity in the body frame. The so_rpy family ignores them.
+
+Set unused elements to zero. The attitude quaternion must be valid. A common hover command sets only the z position:
 
 ```python
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(control=Control.state)
 sim.reset()
 
-cmd = np.zeros((1, 1, 13), dtype=np.float32)
+cmd = np.zeros((1, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[0, 0, 2] = 1.0  # hover at 1 m
 
 sim.state_control(cmd)
@@ -110,9 +112,9 @@ Command shape: `(n_worlds, n_drones, 4)`
 
 | Index | Variable | Units |
 |---|---|---|
-| 0 | Roll rate \(\omega_x\) | rad/s |
-| 1 | Pitch rate \(\omega_y\) | rad/s |
-| 2 | Yaw rate \(\omega_z\) | rad/s |
+| 0 | Body rate \(\omega_x\) | rad/s |
+| 1 | Body rate \(\omega_y\) | rad/s |
+| 2 | Body rate \(\omega_z\) | rad/s |
 | 3 | Collective thrust | N |
 
 Zero rates and hover thrust hold the current attitude:
diff --git a/docs/user-guide/control/integral-errors.md b/docs/user-guide/control/integral-errors.md
index 6280cdde..33ab5f81 100644
--- a/docs/user-guide/control/integral-errors.md
+++ b/docs/user-guide/control/integral-errors.md
@@ -13,12 +13,14 @@ You have two ways to start the integral error at zero:
 import numpy as np
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250")
 pos = np.zeros(3)
 quat = np.array([0.0, 0.0, 0.0, 1.0])
 vel = np.zeros(3)
-cmd = np.zeros(13)
+cmd = np.zeros(16)
+cmd[9:13] = R.from_euler("z", 0.0).as_quat()
 
 # Option 1: let the controller initialise the integral error.
 rpyt, pos_err_i = ctrl(pos, quat, vel, cmd, pos_err_i=None)
@@ -37,12 +39,14 @@ Pass the returned error straight back as `pos_err_i` on the next call:
 import numpy as np
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250")
 pos = np.zeros(3)
 quat = np.array([0.0, 0.0, 0.0, 1.0])
 vel = np.zeros(3)
-cmd = np.zeros(13)
+cmd = np.zeros(16)
+cmd[9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[0] = 1.0  # 1 m setpoint error in x
 
 pos_err_i = None
@@ -60,6 +64,7 @@ for _ in range(10):
 import numpy as np
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import attitude2force_torque, state2attitude
+from scipy.spatial.transform import Rotation as R
 
 state_ctrl = parametrize(state2attitude, "cf2x_L250")
 att_ctrl = parametrize(attitude2force_torque, "cf2x_L250")
@@ -68,7 +73,8 @@ pos = np.zeros(3)
 quat = np.array([0.0, 0.0, 0.0, 1.0])
 vel = np.zeros(3)
 ang_vel = np.zeros(3)
-cmd = np.zeros(13)
+cmd = np.zeros(16)
+cmd[9:13] = R.from_euler("z", 0.0).as_quat()
 
 pos_err_i = None
 r_int_error = None
diff --git a/docs/user-guide/control/jit.md b/docs/user-guide/control/jit.md
index 7aa707fc..8e6a1bb9 100644
--- a/docs/user-guide/control/jit.md
+++ b/docs/user-guide/control/jit.md
@@ -7,6 +7,7 @@ import jax
 import jax.numpy as jnp
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250", xp=jnp)
 jit_ctrl = jax.jit(ctrl)
@@ -14,7 +15,7 @@ jit_ctrl = jax.jit(ctrl)
 pos = jnp.zeros(3)
 quat = jnp.array([0.0, 0.0, 0.0, 1.0])
 vel = jnp.zeros(3)
-cmd = jnp.zeros(13)
+cmd = jnp.zeros(16).at[9:13].set(R.from_euler("z", 0.0).as_quat())
 
 rpyt, int_pos_err = jit_ctrl(pos, quat, vel, cmd)
 ```
@@ -28,6 +29,7 @@ import jax
 import jax.numpy as jnp
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250", xp=jnp)
 jit_ctrl = jax.jit(ctrl)
@@ -35,7 +37,7 @@ jit_ctrl = jax.jit(ctrl)
 pos = jnp.zeros(3)
 quat = jnp.array([0.0, 0.0, 0.0, 1.0])
 vel = jnp.zeros(3)
-cmd = jnp.zeros(13)
+cmd = jnp.zeros(16).at[9:13].set(R.from_euler("z", 0.0).as_quat())
 
 pos_err_i = jnp.zeros(3)  # initialise to zero, so the function compiles only once
 for _ in range(10):
@@ -51,6 +53,7 @@ import jax
 import jax.numpy as jnp
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250", xp=jnp)
 jit_ctrl = jax.jit(ctrl)
@@ -59,7 +62,7 @@ N = 1_000
 pos = jnp.zeros((N, 3))
 quat = jnp.broadcast_to(jnp.array([0.0, 0.0, 0.0, 1.0]), (N, 4))
 vel = jnp.zeros((N, 3))
-cmd = jnp.zeros((N, 13))
+cmd = jnp.zeros((N, 16)).at[..., 9:13].set(R.from_euler("z", 0.0).as_quat())
 
 rpyt, _ = jit_ctrl(pos, quat, vel, cmd)
 rpyt.shape  # (1000, 4)
diff --git a/docs/user-guide/control/mellinger.md b/docs/user-guide/control/mellinger.md
index a9386370..0c61126b 100644
--- a/docs/user-guide/control/mellinger.md
+++ b/docs/user-guide/control/mellinger.md
@@ -24,7 +24,7 @@ All three stages share the same state convention:
 | `pos` | `(..., 3)` | Current position [m] |
 | `quat` | `(..., 4)` | Current attitude, xyzw |
 | `vel` | `(..., 3)` | Current velocity [m/s] |
-| `cmd` | `(..., 13)` | Setpoint: `[x, y, z, vx, vy, vz, ax, ay, az, yaw, avx, avy, avz]` |
+| `cmd` | `(..., 16)` | Setpoint: `[x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]` |
 | `pos_err_i` | `(..., 3)` or `None` | Position integral error from the previous call. `None` initialises to zero |
 | `ctrl_freq` | `float` | Control frequency in Hz (default 100) |
 
@@ -35,17 +35,21 @@ All three stages share the same state convention:
 | `rpyt` | `(..., 4)` | Attitude + thrust: `[roll_rad, pitch_rad, yaw_rad, thrust_N]` |
 | `pos_err_i` | `(..., 3)` | Position integral error. Pass back as `pos_err_i` on the next call |
 
+As in the firmware, only the yaw of the quaternion `qx, qy, qz, qw` is used.
+
 ```python
 import numpy as np
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250")
 
 pos = np.zeros(3)
 quat = np.array([0.0, 0.0, 0.0, 1.0])
 vel = np.zeros(3)
-cmd = np.zeros(13)  # setpoint at origin, yaw = 0
+cmd = np.zeros(16)  # setpoint at origin
+cmd[9:13] = R.from_euler("z", 0.0).as_quat()
 
 rpyt, pos_err_i = ctrl(pos, quat, vel, cmd)
 rpyt.shape  # (4,)
@@ -101,7 +105,7 @@ torque.shape  # (3,)
 |---|---|---|
 | `quat` | `(..., 4)` | Current attitude, xyzw |
 | `ang_vel` | `(..., 3)` | Current angular velocity in body frame [rad/s] |
-| `cmd` | `(..., 4)` | Body rate command: `[roll_rate, pitch_rate, yaw_rate, thrust_N]` |
+| `cmd` | `(..., 4)` | Body rate command: `[wx, wy, wz, thrust_N]` |
 | `prev_ang_vel` | `(..., 3)` or `None` | Angular velocity from the previous call. `None` initialises to zero |
 | `prev_cmd` | `(..., 4)` or `None` | Command from the previous call, used for the setpoint derivative. `None` assumes a constant setpoint |
 | `r_int_error` | `(..., 3)` or `None` | Angular velocity integral error from the previous call. `None` initialises to zero |
@@ -125,7 +129,7 @@ params["kR"], params["ki_m"] = np.zeros(3), np.zeros(3)  # pure body rate tracki
 
 quat = np.array([0.0, 0.0, 0.0, 1.0])
 ang_vel = np.zeros(3)
-cmd = np.array([0.5, 0.0, 0.0, 0.3])  # 0.5 rad/s roll rate, 0.3 N thrust
+cmd = np.array([0.5, 0.0, 0.0, 0.3])  # 0.5 rad/s body rate about x, 0.3 N thrust
 
 force, torque, r_int_err = body_rate2force_torque(quat, ang_vel, cmd, **params)
 force.shape  # (1,)
@@ -173,6 +177,7 @@ from crazyflow.control.mellinger import (
     force_torque2rotor_vel,
     state2attitude,
 )
+from scipy.spatial.transform import Rotation as R
 
 state_ctrl = parametrize(state2attitude, "cf2x_L250")
 att_ctrl = parametrize(attitude2force_torque, "cf2x_L250")
@@ -182,7 +187,8 @@ pos = np.array([0.0, 0.0, 1.0])  # 1 m altitude
 quat = np.array([0.0, 0.0, 0.0, 1.0])
 vel = np.zeros(3)
 ang_vel = np.zeros(3)
-cmd = np.zeros(13)
+cmd = np.zeros(16)
+cmd[9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[:3] = np.array([0.0, 0.0, 1.0])  # hover at 1 m
 
 rpyt, _ = state_ctrl(pos, quat, vel, cmd)
diff --git a/docs/user-guide/control/parametrize.md b/docs/user-guide/control/parametrize.md
index 0795cea3..006eadcd 100644
--- a/docs/user-guide/control/parametrize.md
+++ b/docs/user-guide/control/parametrize.md
@@ -24,12 +24,14 @@ Because `parametrize` returns a `functools.partial`, the bound parameters are ju
 import numpy as np
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250")
 pos = np.zeros(3)
 quat = np.array([0.0, 0.0, 0.0, 1.0])
 vel = np.zeros(3)
-cmd = np.zeros(13)
+cmd = np.zeros(16)
+cmd[9:13] = R.from_euler("z", 0.0).as_quat()
 
 # Simulate with a heavier drone for this call only.
 rpyt, _ = ctrl(pos, quat, vel, cmd, mass=0.035)
@@ -66,12 +68,14 @@ Pass the drone name as a plain string:
 import numpy as np
 from crazyflow.control import parametrize
 from crazyflow.control.mellinger import state2attitude
+from scipy.spatial.transform import Rotation as R
 
 ctrl = parametrize(state2attitude, "cf2x_L250")
 pos = np.zeros(3)
 quat = np.array([0.0, 0.0, 0.0, 1.0])
 vel = np.zeros(3)
-cmd = np.zeros(13)
+cmd = np.zeros(16)
+cmd[9:13] = R.from_euler("z", 0.0).as_quat()
 rpyt, _ = ctrl(pos, quat, vel, cmd)
 ```
 
diff --git a/docs/user-guide/oo-api.md b/docs/user-guide/oo-api.md
index 277ae22c..441022d6 100644
--- a/docs/user-guide/oo-api.md
+++ b/docs/user-guide/oo-api.md
@@ -50,18 +50,20 @@ All control methods take an array of shape `(n_worlds, n_drones, command_dim)` a
 
 ### State control
 
-The highest-level interface. A 13-element command sets desired position, velocity, acceleration, yaw, and angular rates. An internal Mellinger controller converts this to attitude commands.
+The highest-level interface. A 16-element command sets desired position, velocity, acceleration, attitude, and body rates. The yaw part of the attitude commands the heading. The body rate setpoint is forwarded to the attitude controller.
 
 ```python
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=1, n_drones=1, control=Control.state)
 sim.reset()
 
-# [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate]
-cmd = np.zeros((1, 1, 13), dtype=np.float32)
+# [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]
+cmd = np.zeros((1, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[0, 0, 2] = 0.5  # hover at 0.5 m
 
 sim.state_control(cmd)
@@ -164,12 +166,14 @@ A full reset restores everything except the rng key. A mask selects along the wo
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=4, n_drones=1, control=Control.state)
 sim.reset()  # reset all worlds
 
 # Stage a command and advance 50 dynamics steps (controllers fire at their rate)
-cmd = np.zeros((4, 1, 13), dtype=np.float32)
+cmd = np.zeros((4, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 cmd[..., 2] = 0.5
 sim.state_control(cmd)
 sim.step(50)
@@ -189,11 +193,13 @@ Access any state field through `sim.data.states`:
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(n_worlds=2, n_drones=3, control=Control.state)
 sim.reset()
 
-cmd = np.zeros((2, 3, 13), dtype=np.float32)
+cmd = np.zeros((2, 3, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 for _ in range(10):
     sim.state_control(cmd)
     sim.step(sim.freq // sim.control_freq)
diff --git a/docs/user-guide/sim-overview.md b/docs/user-guide/sim-overview.md
index 997ed112..5100ac74 100644
--- a/docs/user-guide/sim-overview.md
+++ b/docs/user-guide/sim-overview.md
@@ -72,10 +72,11 @@ This means you can advance multiple dynamics steps in a single `sim.step(n_steps
 import numpy as np
 from crazyflow.sim import Sim
 from crazyflow.control import Control
+from scipy.spatial.transform import Rotation as R
 
 sim = Sim(freq=500, control=Control.state)
-sim.reset()
-cmd = np.zeros((1, 1, 13), dtype=np.float32)
+cmd = np.zeros((1, 1, 16), dtype=np.float32)
+cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
 sim.state_control(cmd)
 sim.step(sim.freq // sim.control_freq)  # 500 // 100 = 5 dynamics steps, controller fires once
 ```
diff --git a/examples/contacts/crash.py b/examples/contacts/crash.py
index ce8ae4b2..ffa1183a 100644
--- a/examples/contacts/crash.py
+++ b/examples/contacts/crash.py
@@ -1,4 +1,9 @@
+import os
+
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control
 from crazyflow.sim import Sim
@@ -14,7 +19,8 @@ def main():
 
     print("Phase 1: Hovering at [0, 0, 0.5] for 3 seconds")
     hover_duration = 3.0
-    hover_cmd = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    hover_cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    hover_cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     hover_cmd[..., :3] = [0.0, 0.0, 0.5]  # x, y, z position
     sim.state_control(hover_cmd)
 
@@ -26,7 +32,8 @@ def main():
 
     print("Phase 2: Dropping to [-5, 0, -0.5] for 3 seconds")
     drop_duration = 3.0
-    drop_cmd = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    drop_cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    drop_cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     drop_cmd[..., :3] = [-5.0, 0.0, -0.5]  # x, y, z position
     sim.state_control(drop_cmd)
 
diff --git a/examples/control/attitude.py b/examples/control/attitude.py
index 3b4ed69b..b1b383a5 100644
--- a/examples/control/attitude.py
+++ b/examples/control/attitude.py
@@ -1,6 +1,11 @@
+import os
+
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 from functools import partial
 
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control, parametrize
 from crazyflow.control.mellinger import state2attitude
@@ -9,11 +14,11 @@
 
 def control(t: float, pos_start: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
     """Compute the attitude command to track a circle with a slow climb."""
-    cmd = np.zeros(13)
+    cmd = np.zeros(16)
     cmd[:3] = pos_start + np.array([np.cos(t) - 1, np.sin(t), 0.2 * t])
     cmd[3:6] = np.array([-np.sin(t), np.cos(t), 0.2])
     cmd[6:9] = np.array([-np.cos(t), -np.sin(t), 0.0])
-    cmd[9] = t  # Yaw
+    cmd[9:13] = R.from_euler("z", t).as_quat()
     return cmd
 
 
diff --git a/examples/control/body_rate.py b/examples/control/body_rate.py
index f979c099..00c8d20b 100644
--- a/examples/control/body_rate.py
+++ b/examples/control/body_rate.py
@@ -17,11 +17,11 @@
 
 def trajectory(t: float, pos_start: np.ndarray) -> np.ndarray:
     """Compute the full state command of a circle with a slow climb."""
-    cmd = np.zeros(13)
+    cmd = np.zeros(16)
     cmd[:3] = pos_start + np.array([np.cos(t) - 1, np.sin(t), 0.2 * t])
     cmd[3:6] = np.array([-np.sin(t), np.cos(t), 0.2])
     cmd[6:9] = np.array([-np.cos(t), -np.sin(t), 0.0])
-    cmd[9] = t  # Yaw
+    cmd[9:13] = R.from_euler("z", t).as_quat()
     return cmd
 
 
@@ -48,7 +48,7 @@ def main():
     # to body rates. This could be any controller that outputs [w_x, w_y, w_z, thrust].
     position_ctrl = partial(parametrize(state2attitude, sim.drone), ctrl_freq=sim.control_freq)
     pos_err_i = np.zeros(3)
-    cmd = np.zeros((sim.n_worlds, sim.n_drones, 4))  # [roll_rate, pitch_rate, yaw_rate, thrust]
+    cmd = np.zeros((sim.n_worlds, sim.n_drones, 4))  # [wx, wy, wz, thrust]
     pos_start = np.asarray(sim.data.states.pos[0, 0])
     for i in range(int(duration * sim.control_freq)):
         pos, quat = np.asarray(sim.data.states.pos[0, 0]), np.asarray(sim.data.states.quat[0, 0])
diff --git a/examples/control/change_pos.py b/examples/control/change_pos.py
index 5c83940b..960c2575 100644
--- a/examples/control/change_pos.py
+++ b/examples/control/change_pos.py
@@ -1,4 +1,9 @@
+import os
+
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.sim import Sim
 
@@ -18,7 +23,8 @@ def main():
             rotor_vel=sim.data.states.rotor_vel.at[0, 0].set(np.ones(4) * 20000),
         )
     )
-    control = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    control = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    control[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     control[..., :3] = np.array([[0.0, 0.0, 0.3]])
 
     for _ in range(3 * sim.control_freq):
diff --git a/examples/control/dynamics.py b/examples/control/dynamics.py
index 98ace94f..86437a9d 100644
--- a/examples/control/dynamics.py
+++ b/examples/control/dynamics.py
@@ -1,5 +1,10 @@
+import os
+
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import jax.numpy as jnp
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control
 from crazyflow.control.transform import motor_force2rotor_vel
@@ -15,7 +20,8 @@ def figure_eight(t: float) -> np.ndarray:
     """Return the position, velocity, and acceleration reference at time ``t``."""
     omega = 2.0 * np.pi / DURATION
     phase = omega * t
-    cmd = np.zeros((1, 1, 13))
+    cmd = np.zeros((1, 1, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[..., 0:3] = TRAJECTORY_CENTER + TRAJECTORY_SIZE * np.array(
         [np.sin(phase), 0.0, np.sin(2.0 * phase)]
     )
diff --git a/examples/control/hover.py b/examples/control/hover.py
index c597e596..c4761b7f 100644
--- a/examples/control/hover.py
+++ b/examples/control/hover.py
@@ -1,4 +1,9 @@
+import os
+
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control
 from crazyflow.sim import Dynamics, Sim
@@ -20,8 +25,9 @@ def main():
     duration = 5.0
     fps = 60
 
-    # State cmd is [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate]
-    cmd = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    # State cmd is [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]
+    cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[..., :3] = 0.1
 
     for i in range(int(duration * sim.control_freq)):
diff --git a/examples/control/spiral.py b/examples/control/spiral.py
index 8f7b556a..3ee7adc6 100644
--- a/examples/control/spiral.py
+++ b/examples/control/spiral.py
@@ -1,4 +1,9 @@
+import os
+
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control
 from crazyflow.sim import Sim
@@ -6,7 +11,8 @@
 
 def control(start_xy: np.ndarray, t: float) -> np.ndarray:
     circle = np.array([np.cos(t) - 1, np.sin(t)])
-    cmd = np.zeros((*start_xy.shape[:-1], 13))
+    cmd = np.zeros((*start_xy.shape[:-1], 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[..., :2] = start_xy + circle
     cmd[..., 2] = 0.2 * t
     return cmd
diff --git a/examples/plugins/action_delay.py b/examples/plugins/action_delay.py
index cc438d55..5764d79f 100644
--- a/examples/plugins/action_delay.py
+++ b/examples/plugins/action_delay.py
@@ -5,10 +5,14 @@
 
 from __future__ import annotations
 
+import os
 from typing import TYPE_CHECKING
 
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import jax.numpy as jnp
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow import Sim
 from crazyflow.sim.pipeline import prepend_fn
@@ -19,7 +23,8 @@
 
 
 def control(t: float) -> np.ndarray:
-    cmd = np.zeros((1, 1, 13))
+    cmd = np.zeros((1, 1, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[..., :3] = [np.cos(t) - 1, np.sin(t), 0.2 * t]
     return cmd
 
diff --git a/examples/plugins/disturbance.py b/examples/plugins/disturbance.py
index bd692b86..077c1771 100644
--- a/examples/plugins/disturbance.py
+++ b/examples/plugins/disturbance.py
@@ -28,7 +28,8 @@ def disturbance_fn(data: SimData) -> SimData:
 
 def main(plot: bool = False):
     sim = Sim(control="state")
-    control = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    control = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    control[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     control[..., :3] = 0.2
 
     # First run
diff --git a/examples/plugins/estimation.py b/examples/plugins/estimation.py
index 1686129d..16fd9c53 100644
--- a/examples/plugins/estimation.py
+++ b/examples/plugins/estimation.py
@@ -5,11 +5,15 @@
 
 from __future__ import annotations
 
+import os
 from typing import TYPE_CHECKING
 
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import jax
 import jax.numpy as jnp
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow import Sim
 from crazyflow.control.transform import motor_force2rotor_vel
@@ -40,7 +44,8 @@ def trajectory(t: float, t_total: float = 20.0) -> np.ndarray:
     center = np.array([0.0, 0.0, 1.0])
     size = np.array([1.0, 0.75, 0.0])
     omega = 2 * np.pi / t_total
-    cmd = np.zeros((1, 1, 13))
+    cmd = np.zeros((1, 1, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     pos = center + size * np.array([np.sin(omega * t), np.sin(2 * omega * t), 0.0])
     vel = size * omega * np.array([np.cos(omega * t), 2 * np.cos(2 * omega * t), 0.0])
     acc = size * omega**2 * np.array([-np.sin(omega * t), -4 * np.sin(2 * omega * t), 0.0])
diff --git a/examples/plugins/randomize.py b/examples/plugins/randomize.py
index ce4d6212..941abe5b 100644
--- a/examples/plugins/randomize.py
+++ b/examples/plugins/randomize.py
@@ -5,10 +5,15 @@
 default parameters as the base value ensures that repeated resets do not compound.
 """
 
+import os
+
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import jax
 import jax.numpy as jnp
 import numpy as np
 from jax import Array
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control
 from crazyflow.sim import Sim
@@ -138,7 +143,8 @@ def main():
     fps = 60
 
     for _ in range(3):
-        cmd = np.zeros((sim.n_worlds, sim.n_drones, 13))
+        cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
+        cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
         cmd[..., 2] = 0.4
         cmd[..., :2] = grid_2d(sim.n_drones) * 0.25
 
diff --git a/examples/rendering/cam_config.py b/examples/rendering/cam_config.py
index f70d64d5..6080e9c4 100644
--- a/examples/rendering/cam_config.py
+++ b/examples/rendering/cam_config.py
@@ -1,6 +1,11 @@
 """Simple example on how to change the camera configuration for rendering."""
 
+import os
+
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.sim import Sim
 
@@ -14,7 +19,8 @@ def main(cam_config: dict | None = None):
     duration = 5.0
     fps = 60
 
-    cmd = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[..., :3] = 0.2
 
     for i in range(int(duration * sim.control_freq)):
diff --git a/examples/rendering/cameras.py b/examples/rendering/cameras.py
index c07b59eb..4e09f718 100644
--- a/examples/rendering/cameras.py
+++ b/examples/rendering/cameras.py
@@ -1,11 +1,15 @@
 """Example showing how to change the used camera and how to extract the pixel information."""
 
+import os
 import time
 
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import matplotlib.pyplot as plt
 import mujoco
 import numpy as np
 from matplotlib import animation
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control
 from crazyflow.dynamics import Dynamics
@@ -16,10 +20,11 @@
 def control(t: float, t_tot: float) -> np.ndarray:
     phi = 2 * np.pi * t / t_tot + np.pi
     circle = np.array([np.cos(phi), np.sin(phi)])
-    cmd = np.zeros((1, 1, 13))
+    yaw = 2 * np.pi * t / t_tot
+    cmd = np.zeros((1, 1, 16))
     cmd[..., :2] = circle  # xy
     cmd[..., 2] = 0.1 + 0.5 * t / t_tot  # z
-    cmd[..., -4] = 1.9 * np.pi * t / t_tot  # yaw
+    cmd[..., 9:13] = R.from_euler("z", yaw).as_quat()
 
     return cmd
 
diff --git a/examples/rendering/led_deck.py b/examples/rendering/led_deck.py
index ba427145..e3fb4888 100644
--- a/examples/rendering/led_deck.py
+++ b/examples/rendering/led_deck.py
@@ -1,7 +1,11 @@
+import os
 import tempfile
 from pathlib import Path
 
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control
 from crazyflow.sim import Sim
@@ -50,7 +54,8 @@ def main():
         rgbas[..., 3] = 1.0
 
         init_pos = np.array(sim.data.states.pos[0, :, :])
-        cmd = np.zeros((sim.n_worlds, sim.n_drones, 13))
+        cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
+        cmd[:, :, 9:13] = R.from_euler("z", 0.0).as_quat()
         cmd[:, :, :3] = init_pos
         cmd[:, :, 2] += 1.5
 
diff --git a/examples/rendering/splat_camera.py b/examples/rendering/splat_camera.py
index d24e7ed7..a3acf3fa 100644
--- a/examples/rendering/splat_camera.py
+++ b/examples/rendering/splat_camera.py
@@ -28,13 +28,13 @@
 
 def control(t: float, n_worlds: int, n_drones: int) -> np.ndarray:
     """Circle both drones around the center, each yawed to look across at its partner."""
-    drones = np.arange(n_drones)[None, :]
-    angle = t + (2 * np.pi / n_drones) * drones
-    cmd = np.zeros((n_worlds, n_drones, 13))
+    angle = t + (2 * np.pi / n_drones) * np.arange(n_drones)
+    cmd = np.zeros((n_worlds, n_drones, 16))
     cmd[..., 0] = RADIUS * np.cos(angle)
     cmd[..., 1] = RADIUS * np.sin(angle)
     cmd[..., 2] = HEIGHT
-    cmd[..., 9] = angle + np.pi  # yaw faces the opposite point on the circle, where the partner is
+    yaw = angle + np.pi  # yaw faces the opposite point on the circle, where the partner is
+    cmd[..., 9:13] = R.from_euler("z", yaw[:, None]).as_quat()
     return cmd
 
 
diff --git a/examples/rendering/splat_depth.py b/examples/rendering/splat_depth.py
index ba15232b..6b90b1a1 100644
--- a/examples/rendering/splat_depth.py
+++ b/examples/rendering/splat_depth.py
@@ -42,10 +42,11 @@ def control(t: float) -> np.ndarray:
         A state command placing the drone on the ellipse with its yaw along the tangent.
     """
     angle = 2 * np.pi * t
-    cmd = np.zeros((1, 1, 13))
+    cmd = np.zeros((1, 1, 16))
     cmd[..., :2] = CENTER + RADII * np.array([np.cos(angle), np.sin(angle)])
     cmd[..., 2] = HEIGHT
-    cmd[..., 9] = np.arctan2(RADII[1] * np.cos(angle), -RADII[0] * np.sin(angle))
+    yaw = np.arctan2(RADII[1] * np.cos(angle), -RADII[0] * np.sin(angle))
+    cmd[..., 9:13] = R.from_euler("z", yaw).as_quat()
     return cmd
 
 
@@ -60,7 +61,7 @@ def main(show_plot: bool = False):
     # Start on the lap so the controller does not have to fly in from the origin first
     cmd = control(0.0)
     states = sim.data.states.replace(pos=sim.data.states.pos.at[..., :].set(cmd[..., :3]))
-    states = states.replace(quat=states.quat.at[...].set(R.from_euler("z", cmd[..., 9]).as_quat()))
+    states = states.replace(quat=states.quat.at[...].set(cmd[..., 9:13]))
     sim.data = sim.data.replace(states=states)
 
     fig, ax = plt.subplots(figsize=(7, 5))
diff --git a/examples/rendering/splat_viewer.py b/examples/rendering/splat_viewer.py
index 9ad3646d..3d3ca7e4 100644
--- a/examples/rendering/splat_viewer.py
+++ b/examples/rendering/splat_viewer.py
@@ -12,9 +12,13 @@
 from __future__ import annotations
 
 import logging
+import os
 import time
 
+os.environ["SCIPY_ARRAY_API"] = "1"
+
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 from splax.io import fetch
 
 from crazyflow.sim import Sim
@@ -25,7 +29,8 @@
 
 
 def control(t: float) -> np.ndarray:
-    cmd = np.zeros((1, 1, 13))
+    cmd = np.zeros((1, 1, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[..., :3] = [0.5 * (np.cos(t) - 1), 0.5 * np.sin(t), 1.0 + 0.2 * np.sin(0.5 * t)]
     return cmd
 
diff --git a/tests/integration/test_disturbance.py b/tests/integration/test_disturbance.py
index 359c9584..d268a05e 100644
--- a/tests/integration/test_disturbance.py
+++ b/tests/integration/test_disturbance.py
@@ -1,6 +1,7 @@
 import jax
 import numpy as np
 import pytest
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.sim import Dynamics, Sim
 from crazyflow.sim.data import SimData
@@ -19,7 +20,8 @@ def disturbance_fn(data: SimData) -> SimData:
 @pytest.mark.integration
 def test_disturbance(dynamics: Dynamics):
     sim = Sim(n_worlds=2, n_drones=3, control="state", dynamics=dynamics)
-    control = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    control = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    control[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     control[..., :3] = 1.0
     n_steps = 10
 
diff --git a/tests/integration/test_interfaces.py b/tests/integration/test_interfaces.py
index 131da498..7f63d508 100644
--- a/tests/integration/test_interfaces.py
+++ b/tests/integration/test_interfaces.py
@@ -17,7 +17,8 @@ def test_state_interface(dynamics: Dynamics):
 
     # Simple P controller for attitude to reach target height
     target_height = 0.5
-    cmd = np.zeros((1, 1, 13), dtype=np.float32)
+    cmd = np.zeros((1, 1, 16), dtype=np.float32)
+    cmd[0, 0, 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[0, 0, 2] = target_height
     steps = int(2 * sim.control_freq)  # Run simulation for 2 seconds
 
@@ -31,6 +32,31 @@ def test_state_interface(dynamics: Dynamics):
     assert distance < 0.1, f"Failed to reach target height with {dynamics} dynamics"
 
 
+@pytest.mark.integration
+def test_state_interface_body_rate_feedforward():
+    """The body rates of the state command must reach the attitude controller as feedforward."""
+    yaw_rate = 1.5
+
+    def yaw_ramp_error(feedforward: bool) -> float:
+        sim = Sim(dynamics=Dynamics.first_principles, control=Control.state)
+        cmd = np.zeros((1, 1, 16))
+        cmd[..., 2] = 0.5
+        errors = []
+        for i in range(int(4 * sim.control_freq)):
+            yaw_des = yaw_rate * i / sim.control_freq
+            cmd[..., 9:13] = R.from_euler("z", yaw_des).as_quat()
+            cmd[..., 15] = yaw_rate if feedforward else 0.0
+            sim.state_control(cmd)
+            sim.step(sim.freq // sim.control_freq)
+            yaw = R.from_quat(sim.data.states.quat[0, 0]).as_euler("xyz")[2]
+            errors.append(abs((yaw - yaw_des + np.pi) % (2 * np.pi) - np.pi))
+        return np.mean(errors[sim.control_freq :])  # Skip the first second of transients
+
+    error_ff, error_no_ff = yaw_ramp_error(True), yaw_ramp_error(False)
+    assert error_ff < 0.05, f"Yaw lag with body rate feedforward: {error_ff}"
+    assert error_ff < error_no_ff / 2, f"Feedforward must reduce the yaw lag ({error_no_ff})"
+
+
 @pytest.mark.integration
 @pytest.mark.parametrize("dynamics", Dynamics)
 def test_attitude_interface(dynamics: Dynamics):
@@ -39,7 +65,8 @@ def test_attitude_interface(dynamics: Dynamics):
     jit_state2attitude = jax.jit(parametrize(state2attitude, drone=sim.drone))
 
     pos_err_i = np.zeros((1, 1, 3))
-    cmd = np.zeros((1, 1, 13))
+    cmd = np.zeros((1, 1, 16))
+    cmd[0, 0, 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[0, 0, 2] = 1.0
     steps = int(3 * sim.control_freq)
 
@@ -108,7 +135,8 @@ def test_swarm_control(dynamics: Dynamics):
     sim = Sim(n_worlds=n_worlds, n_drones=n_drones, dynamics=dynamics, control=Control.state)
     start_pos = np.asarray(sim.data.states.pos)
     target_pos = sim.data.states.pos + np.array([0.3, 0.3, 0.3])
-    cmd = np.zeros((n_worlds, n_drones, 13))
+    cmd = np.zeros((n_worlds, n_drones, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     steps = int(3 * sim.control_freq)
 
     for i in range(steps):
@@ -130,9 +158,9 @@ def test_yaw_rotation(dynamics: Dynamics):
     sim = Sim(dynamics=dynamics, control=Control.state, state_freq=100)
     sim.reset()
 
-    cmd = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
     cmd[..., :3] = 0.2
-    cmd[..., 9] = np.pi / 2  # Test if the drone can rotate in yaw
+    cmd[..., 9:13] = R.from_euler("z", np.pi / 2).as_quat()  # Test if the drone can rotate in yaw
 
     sim.state_control(cmd)
     sim.step(200 * sim.freq // sim.control_freq)  # Run simulation for 2 seconds
diff --git a/tests/unit/control/test_mellinger.py b/tests/unit/control/test_mellinger.py
index b4a518db..8d19a5ef 100644
--- a/tests/unit/control/test_mellinger.py
+++ b/tests/unit/control/test_mellinger.py
@@ -30,12 +30,12 @@ def test_state2attitude(drone: str) -> None:
     controller = parametrize(state2attitude, drone)
     # Single input
     pos, quat, vel, ang_vel = create_rnd_states()
-    rpyt, pos_err_i = controller(pos, quat, vel, np.ones(13), ctrl_freq=100)
+    rpyt, pos_err_i = controller(pos, quat, vel, np.ones(16), ctrl_freq=100)
     assert rpyt.shape == (4,)
     assert pos_err_i.shape == (3,)
     # Batch input
     pos, quat, vel, ang_vel = create_rnd_states((5, 4))
-    rpyt, pos_err_i = controller(pos, quat, vel, np.ones((5, 4, 13)), ctrl_freq=100)
+    rpyt, pos_err_i = controller(pos, quat, vel, np.ones((5, 4, 16)), ctrl_freq=100)
     assert rpyt.shape == (5, 4, 4)
     assert pos_err_i.shape == (5, 4, 3)
 
@@ -67,7 +67,7 @@ def test_body_rate2force_torque(drone: str) -> None:
     controller = parametrize(body_rate2force_torque, drone)
     # Single input
     _, quat, _, ang_vel = create_rnd_states()
-    cmd = np.array([0.1, 0.1, 0.1, 1.0])  # roll rate, pitch rate, yaw rate, thrust command
+    cmd = np.array([0.1, 0.1, 0.1, 1.0])  # body rates and thrust command
     force_des, torque_des, r_int_error = controller(quat, ang_vel, cmd)
     assert force_des.shape == (1,)
     assert torque_des.shape == (3,)
@@ -110,7 +110,8 @@ def test_state2attitude_at_setpoint(drone: str) -> None:
     pos = np.zeros(3)
     quat = np.array([0.0, 0.0, 0.0, 1.0])
     vel = np.zeros(3)
-    cmd = np.zeros(13)  # setpoint at origin, zero vel/acc, yaw=0
+    cmd = np.zeros(16)  # setpoint at origin, zero vel/acc
+    cmd[9:13] = R.from_euler("z", 0.0).as_quat()
     rpyt, _ = controller(pos, quat, vel, cmd)
     assert np.allclose(rpyt[:3], 0.0, atol=1e-6), f"RPY at setpoint should be ~0, got {rpyt[:3]}"
     assert rpyt[3] > 0.0, "Hovering thrust must be positive"
@@ -126,7 +127,8 @@ def test_state2attitude_integral_error_accumulation(drone: str) -> None:
     pos = np.zeros(3)
     quat = np.array([0.0, 0.0, 0.0, 1.0])
     vel = np.zeros(3)
-    cmd = np.zeros(13)
+    cmd = np.zeros(16)
+    cmd[9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[0] = 1.0  # 1 m setpoint error in x
     ctrl_freq = 100.0
     dt = 1.0 / ctrl_freq
@@ -261,7 +263,7 @@ def test_state2attitude_batch_consistency(drone: str):
     controller = parametrize(state2attitude, drone)
     batch = (3, 2)
     pos, quat, vel, _ = create_rnd_states(batch)
-    cmd = np.random.randn(*batch, 13)
+    cmd = np.random.randn(*batch, 16)
     rpyt_batch, err_batch = controller(pos, quat, vel, cmd)
     for i in range(batch[0]):
         for j in range(batch[1]):
diff --git a/tests/unit/test_functional.py b/tests/unit/test_functional.py
index 77a02dbb..b780a3f5 100644
--- a/tests/unit/test_functional.py
+++ b/tests/unit/test_functional.py
@@ -123,28 +123,28 @@ def test_functional_state_control(state_freq: int):
     can_control_2 = np.array([0, 0, 1, 2, 3, 4]) * state_freq % sim.freq < state_freq
 
     for i in range(6):
-        cmd = np.random.rand(sim.n_worlds, sim.n_drones, 13)
+        cmd = np.random.rand(sim.n_worlds, sim.n_drones, 16)
         # Check controllable status
         controllable = F.controllable(data)
         assert jnp.all(controllable[0] == can_control_1[i]), f"Controllable 1 mismatch at t={i}"
         assert jnp.all(controllable[1] == can_control_2[i]), f"Controllable 2 mismatch at t={i}"
         # Apply control
         data = F.state_control(data, cmd)
-        last_attitude = data.controls.attitude.staged_cmd
+        prev_attitude = data.controls.attitude.staged_cmd
         data = step_fn(data, 1)
         attitude = data.controls.attitude.staged_cmd
 
-        last_att, att = last_attitude[0], attitude[0]
+        prev_att, att = prev_attitude[0], attitude[0]
         if can_control_1[i]:
-            assert not jnp.all(att == last_att), f"Controls haven't been applied at t={i}"
+            assert not jnp.all(att == prev_att), f"Controls haven't been applied at t={i}"
         else:
-            assert jnp.all(att == last_att), f"Controls should be unchanged at t={i}"
+            assert jnp.all(att == prev_att), f"Controls should be unchanged at t={i}"
 
-        last_att, att = last_attitude[1], attitude[1]
+        prev_att, att = prev_attitude[1], attitude[1]
         if can_control_2[i]:
-            assert not jnp.all(att == last_att), f"Controls haven't been applied at t={i}"
+            assert not jnp.all(att == prev_att), f"Controls haven't been applied at t={i}"
         else:
-            assert jnp.all(att == last_att), f"Controls should be unchanged at t={i}"
+            assert jnp.all(att == prev_att), f"Controls should be unchanged at t={i}"
         if i == 0:  # Make world 2 asynchronous
             data = reset_fn(data, default_data, np.array([False, True]))
 
@@ -160,7 +160,7 @@ def test_functional_state_control_device(device: str):
     """Test that functional state control maintains JAX arrays on correct device."""
     sim = Sim(n_worlds=2, n_drones=3, control=Control.state, device=device)
     data = sim.build_data()
-    cmd = np.random.rand(sim.n_worlds, sim.n_drones, 13)
+    cmd = np.random.rand(sim.n_worlds, sim.n_drones, 16)
     data = F.state_control(data, cmd)
     controls = data.controls.state
     assert isinstance(controls.cmd, jnp.ndarray), "Buffers must remain JAX arrays"
diff --git a/tests/unit/test_gradients.py b/tests/unit/test_gradients.py
index 3209cf78..fe270372 100644
--- a/tests/unit/test_gradients.py
+++ b/tests/unit/test_gradients.py
@@ -4,6 +4,7 @@
 import jax.numpy as jnp
 import pytest
 from jax import Array
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.dynamics import Dynamics
 from crazyflow.sim import Sim
@@ -25,7 +26,8 @@ def height(cmd: Array, data: SimData) -> Array:
         )
         return sim_step(data, sim.freq // sim.control_freq).states.pos[0, 0, 2]
 
-    cmd = jnp.zeros((1, 1, 13), dtype=jnp.float32)
+    cmd = jnp.zeros((1, 1, 16), dtype=jnp.float32)
+    cmd = cmd.at[..., 9:13].set(R.from_euler("z", 0.0).as_quat())
     cmd = cmd.at[..., 2].set(1.01)
 
     grad = jax.jit(jax.grad(height))(cmd, data)
diff --git a/tests/unit/test_sim.py b/tests/unit/test_sim.py
index 6d5cabda..088e1fa1 100644
--- a/tests/unit/test_sim.py
+++ b/tests/unit/test_sim.py
@@ -11,6 +11,7 @@
 import pytest
 from conftest import skip_if_headless
 from jax import Array
+from scipy.spatial.transform import Rotation as R
 
 from crazyflow.control import Control
 from crazyflow.exception import ConfigError
@@ -78,8 +79,8 @@ def test_sim_init(dynamics: Dynamics, device: str, control: Control, n_worlds: i
     # Test control buffer shapes
     if control == Control.state:
         assert isinstance(sim.data.controls.state, ControlData)
-        array_meta_assert(sim.data.controls.state.staged_cmd, (n_worlds, n_drones, 13), device)
-        array_meta_assert(sim.data.controls.state.cmd, (n_worlds, n_drones, 13), device)
+        array_meta_assert(sim.data.controls.state.staged_cmd, (n_worlds, n_drones, 16), device)
+        array_meta_assert(sim.data.controls.state.cmd, (n_worlds, n_drones, 16), device)
     else:
         assert sim.data.controls.state is None
     # Test attitude buffer shapes
@@ -227,6 +228,23 @@ def test_sim_step(n_worlds: int, n_drones: int, dynamics: Dynamics, control: Con
     sim.step(2)
 
 
+@pytest.mark.unit
+def test_state_control_forwards_body_rates():
+    """State control must forward the body rates of the command to the attitude controller."""
+    sim = Sim(n_worlds=2, n_drones=3, control=Control.state)
+    cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
+    cmd[..., 13:16] = np.random.rand(sim.n_worlds, sim.n_drones, 3)
+    sim.state_control(cmd)
+    sim.step()
+    assert np.allclose(sim.data.controls.attitude.ang_vel_des, cmd[..., 13:16])
+    # Attitude control never sets a body rate setpoint
+    sim = Sim(n_worlds=2, n_drones=3, control=Control.attitude)
+    sim.attitude_control(np.random.rand(sim.n_worlds, sim.n_drones, 4))
+    sim.step()
+    assert np.all(sim.data.controls.attitude.ang_vel_des == 0.0)
+
+
 @pytest.mark.unit
 @pytest.mark.parametrize("attitude_freq", [33, 50, 100, 200])
 def test_sim_attitude_control(attitude_freq: int):
@@ -271,23 +289,23 @@ def test_sim_state_control(state_freq: int):
     can_control_1 = np.arange(6) * state_freq % sim.freq < state_freq
     can_control_2 = np.array([0, 0, 1, 2, 3, 4]) * state_freq % sim.freq < state_freq
     for i in range(6):
-        cmd = np.random.rand(sim.n_worlds, sim.n_drones, 13)
+        cmd = np.random.rand(sim.n_worlds, sim.n_drones, 16)
         assert jnp.all(sim.controllable[0] == can_control_1[i]), f"Controllable 1 mismatch at t={i}"
         assert jnp.all(sim.controllable[1] == can_control_2[i]), f"Controllable 2 mismatch at t={i}"
         sim.state_control(cmd)
-        last_attitude = sim.data.controls.attitude.staged_cmd
+        prev_attitude = sim.data.controls.attitude.staged_cmd
         sim.step()
         attitude = sim.data.controls.attitude.staged_cmd
-        last_att, att = last_attitude[0], attitude[0]
+        prev_att, att = prev_attitude[0], attitude[0]
         if can_control_1[i]:
-            assert not jnp.all(att == last_att), f"Controls haven't been applied at t={i}"
+            assert not jnp.all(att == prev_att), f"Controls haven't been applied at t={i}"
         else:
-            assert jnp.all(att == last_att), f"Controls should be unchanged at t={i}"
-        last_att, att = last_attitude[1], attitude[1]
+            assert jnp.all(att == prev_att), f"Controls should be unchanged at t={i}"
+        prev_att, att = prev_attitude[1], attitude[1]
         if can_control_2[i]:
-            assert not jnp.all(att == last_att), f"Controls haven't been applied at t={i}"
+            assert not jnp.all(att == prev_att), f"Controls haven't been applied at t={i}"
         else:
-            assert jnp.all(att == last_att), f"Controls should be unchanged at t={i}"
+            assert jnp.all(att == prev_att), f"Controls should be unchanged at t={i}"
         if i == 0:
             sim.reset(np.array([False, True]))  # Make world 2 asynchronous
 
@@ -295,7 +313,7 @@ def test_sim_state_control(state_freq: int):
 @pytest.mark.unit
 def test_sim_state_control_device(device: str):
     sim = Sim(n_worlds=2, n_drones=3, control=Control.state, device=device)
-    cmd = np.random.rand(sim.n_worlds, sim.n_drones, 13)
+    cmd = np.random.rand(sim.n_worlds, sim.n_drones, 16)
     sim.state_control(cmd)
     controls = sim.data.controls.state
     assert isinstance(controls.cmd, jnp.ndarray), "Buffers must remain JAX arrays"
@@ -348,7 +366,8 @@ def test_control_frequency(dynamics: Dynamics):
     sim_1000 = Sim(freq=1000, dynamics=dynamics, control="state")
 
     # Set same initial state and controls
-    cmd = np.zeros((1, 1, 13))  # Single world, single drone, state control
+    cmd = np.zeros((1, 1, 16))  # Single world, single drone, state control
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     # Target position of (1, 1, 1). Needs to be off-center to check attitude integration error
     cmd[..., :3] = 1.0
 
@@ -634,7 +653,8 @@ def test_compile(dynamics: Dynamics, device: str):
 def test_scan_results(dynamics: Dynamics):
     sim = Sim(n_worlds=2, n_drones=3, dynamics=dynamics, control=Control.state, device="cpu")
     sim.reset()
-    cmd = np.zeros((sim.n_worlds, sim.n_drones, 13))
+    cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
+    cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
     cmd[..., :3] = sim.data.states.pos + np.array([0.3, 0.3, 0.3])
     sim.state_control(cmd)
     n_steps, n_iters = sim.freq // sim.control_freq, 100  # 1 second at 100Hz