Analytical Koopman-based linear MPC for quadrotors on SE(3)
Real-time trajectory tracking as a convex QP — no training data, no nonlinear program.
Skydio X2 in MuJoCo tracking a Bernoulli lemniscate under KQ‑LMPC. Red beads are the reference; the amber ball is the set-point being chased right now.
This is the same KQ‑LMPC implementation as
santoshrajkumar/kq-lmpc-quadrotor with two things added:
- A performance-enhanced solver path. The optimal control problem is now solved in-process as a condensed QP by DAQP, and every model evaluation on the hot path runs through compiled CasADi functions. acados is no longer a dependency, and a control step costs about a millisecond.
- MuJoCo demos on two real airframe models, both from MuJoCo Menagerie: the 1.3 kg Skydio X2 and the 27 g Bitcraze Crazyflie 2. In each case the Koopman model is built from that vehicle's own mass and inertia as compiled by MuJoCo, and the wrench is put through the airframe's own actuation — the X2's four rotors via the exact mixer implied by its actuator sites and gears, the Crazyflie's thrust-and-moment actuators directly. Same controller, same five trajectories, two vehicles a fiftieth of each other's mass apart.
Everything else; the observables, the reference generator, the LQR fallback, the numerical experiment is the implementation the paper describes.
| Oiginal | Here | |
|---|---|---|
| OCP backend | acados (nonlinear-program interface, C codegen, external install) | KqLmpcOcpSolver: dense condensed QP solved by DAQP, pure pip install |
| Decision variables | one stage block per shooting node | states eliminated by condensing → 4N variables |
| Model evaluation | per-step Python loops of matrix_power / inv over 3×3 blocks |
CasADi Function.map(N) — a whole horizon in one dispatch |
| Cost of a step | ~16 ms on that evaluation path, ~95% of it interpreter overhead rather than the QP | ~1 ms mean at the default settings |
| Install | acados + its Python interface, Linux/macOS only | six pip packages |
The solver's public surface deliberately mirrors the subset of the acados
AcadosOcpSolver API the demos use (set, constraints_set, solve, get,
get_stats), so scripts written against the original package run unchanged.
The lifting makes the dynamics linear in the lifted state and in the modified control for a frozen seed trajectory, so the OCP is an exact QP. There was never a nonlinear program to solve — only one being paid for.
All five tasks that ship with the package, each flown by every demo. The clips below are
example_scripts/skydio_x2_demo.py recording its own runs — reproduce any of them
with --traj <name>, and see the same five on the Crazyflie
further down.
 1 · hover Take off from the pad and hold a fixed point. The set-point is the whole reference, so it is drawn as a single ball. |
 2 · line Minimum-snap line from the origin to a goal point, then hold. A take-off and a translation in one task. |
|
3 · lemniscate Bernoulli figure eight at constant altitude. Curvature reverses twice a lap — the attitude has to follow it. |
 4 · helix Circle in x–y while climbing steadily in z. A constant turn with a constant vertical rate on top of it. |
 5 · knot A torus knot — the hardest of the five. The path winds in all three axes at once, so no axis ever settles. |
|
example_scripts/crazyflie_demo.py, recording its own runs. Same controller, same five
references, an airframe a fiftieth of the X2's mass — flown small (0.4 m, 0.5 rad/s)
because the Menagerie model caps its body moments at 1e-5 N·m.
 hover climb from the pad, hold the point |
 line minimum-snap line, then hold |
 lemniscate figure eight at constant altitude |
|||
 helix circle in x–y, climbing in z |
 knot torus knot, all three axes at once |
||||
Position RMSE over the whole run at these defaults, as the demo prints it: line 0.0000 m,
lemniscate 0.0010 m, helix 0.0015 m, knot 0.0016 m, with no QP failures and so no
LQR fallbacks. hover is a take-off from the origin to a point 0.7 m away, so its 0.12 m
figure is the climb, and it ends within 0.1 mm of the set-point.
Real-Time Linear MPC for Quadrotors on SE(3): An Analytical Koopman-Based Realization S. M. Rajkumar, C. Yang, Y. Gu, S. Cheng, N. Hovakimyan and D. Goswami IEEE Robotics and Automation Letters, vol. 10, no. 12, pp. 13018–13025, 2025. doi:10.1109/LRA.2025.3626234
@article{rajkumar2025real,
title = {Real-Time Linear MPC for Quadrotors on SE(3): An Analytical Koopman-Based Realization},
author = {Rajkumar, Santosh Mohan and Yang, Chengyu and Gu, Yuliang and Cheng, Sheng and Hovakimyan, Naira and Goswami, Debdipta},
journal = {IEEE Robotics and Automation Letters},
volume = {10},
number = {12},
pages = {13018--13025},
year = {2025},
doi = {10.1109/LRA.2025.3626234},
publisher = {IEEE}
}If you use this code, please cite the paper.
git clone https://github.com/santoshrajkumar/kq-lmpc-quadrotor-updated.git
cd kq-lmpc-quadrotor-updated
pip install -e . # controller + numerical demo
pip install -e ".[mujoco]" # adds the Skydio X2 demo (mujoco, pillow)The controller needs only numpy, scipy, daqp, casadi, matplotlib and
pyquaternion; the MuJoCo demo adds mujoco and pillow. All of them are plain wheels,
so there is nothing to build or configure before the demos will run.
The example scripts also run straight from a checkout without installing the package:
each one puts the repository root on sys.path itself, so python example_scripts/<demo>.py
works on a bare clone once the dependencies are present. To get just those:
pip install -r requirements.txtpython example_scripts/numerical_demo.pyThe lifted OCP against the nonlinear SE(3) plant integrated with scipy.solve_ivp. No
MuJoCo, no rendering — the reference experiment. Asks which of the five trajectories to
fly; everything else has a default.
python example_scripts/skydio_x2_demo.py # pick a trajectory, watch it fly
python example_scripts/skydio_x2_demo.py --traj knot --no-view
python example_scripts/skydio_x2_demo.py --list # the trajectory menuOpens the MuJoCo viewer and records an animated GIF to media/kqlmpc_<traj>.gif —
the five clips above are its own output. --no-view runs it headless, --no-gif skips the
recording.
The Koopman model is built from the X2's own rigid-body parameters read out of the
compiled model, and the wrench [f, tau] is allocated onto the rotors through the exact
mixer implied by the actuator sites and gears, then clipped to each rotor's ctrlrange.
So the controller is flying the airframe, not an idealised copy of it.
python example_scripts/crazyflie_demo.py # pick a trajectory, watch it fly
python example_scripts/crazyflie_demo.py --traj helix --no-viewThe same five trajectories on a 27 gram airframe instead of a 1.3 kg one, recording to
media/kqlmpc_cf2_<traj>.gif. The Menagerie Crazyflie is actuated by a wrench rather
than by four rotors — one thrust actuator and three body moments at a single site
— which is exactly what KQ-LMPC produces, so there is no mixer to invert: the control
map is read off the actuators' own gear rows.
Two things about that model are worth knowing before reading its numbers. Menagerie's own
README calls its ctrlrange limits "arbitrary and [in need of] further tun[ing]", and the
moment actuators are capped at 1e-5 N·m — far below what a real Crazyflie's rotors
can produce — so the defaults fly small and gently; --tau-max rescales that cap.
And the model asks for air density and viscosity, so MuJoCo applies a fluid drag the
Koopman model knows nothing about. It is left on: rejecting it is the controller's job.
python example_scripts/generate_nominal_control_4_rl.pyFlies a trajectory headlessly and writes down what the nominal controller did at every sampling instant, as training data for a policy. It asks three questions, each with a default: which reference, how many seconds, and the sampling time. Scripted form:
python example_scripts/generate_nominal_control_4_rl.py --traj helix --duration 30 --dt 0.01The run ends with a plot of the flown trajectory against the reference, and one file in
example_scripts/data/<reference>_inputs_positions.npz:
| Key | Shape | What it is |
|---|---|---|
rotor_input |
(n, 4) |
u_1 … u_4, the rotor thrusts written to data.ctrl (post-clip) |
dynamic_input |
(n, 4) |
f, tau_1, tau_2, tau_3, the same command as a body wrench |
s_ref, s |
(n+1, 3) |
reference position and flown position, same instants |
X, Xd |
(18, n+1) |
full state and reference: [s; v; vec(R); omega] |
t |
(n+1,) |
sample instants |
rotor_input_unclipped |
(n, 4) |
the allocation before the ctrlrange clip |
dt, duration, reference, task_id, center, x0 |
— | run configuration |
mass, J, g, mixer, rotor_min/max, wrench_min/max |
— | the plant it was flown on |
dynamic_input is exactly mixer @ rotor_input, and both are the command that produced
the transition from X[:, k] to X[:, k+1] — so a row index means the same thing in
every array, and the file is a complete set of (state, action, next state) transitions.
Where an episode starts. hover and line are take-offs: they begin at the inertial
origin, at rest and level, and the reference lifts the vehicle to the goal point. The three
periodic trajectories are entered in flight, so the vehicle is spawned on the reference,
carrying its whole initial state — position, velocity, attitude and body rate —
so the trajectory is entered at track speed and banked into the turn, with zero initial
error in every channel. --start rest takes the position only.
The state is x = [s; v; vec(R); omega] on SE(3). An analytical set of observables
(no data, no training) lifts it to psi(x), in which the dynamics are linear:
psi_dot = A psi + calB(x*) u_tilde, with calB evaluated along a frozen seed / previous
predicted-optimal trajectory. Tracking cost, input box and lifted-state envelope are all
convex in that coordinate, so the horizon is one QP. The states are eliminated by
condensing, leaving 4N decision variables for DAQP's dense dual active-set method, and a
Koopman LQR on the same lifted model stands behind it as the fallback if a QP ever
fails to solve.
kq_lmpc_quadrotor_2/ the package
├── QuadrotorKoopman.py analytical lifting: observables, A, calB, de-lift
├── mpc/
│ ├── kq_lmpc.py KqLmpcOcpSolver — condensed QP, DAQP backend
│ ├── koopman_fast.py compiled CasADi evaluators for the hot path
│ └── mpc_utils.py horizon seeding and shifting
├── Utils/ SE(3) plant, reference generator, LQR, helpers
└── Viz/ trajectory / error plots and animation
example_scripts/
├── numerical_demo.py nonlinear SE(3) plant, no MuJoCo
├── skydio_x2_demo.py MuJoCo Skydio X2, viewer + GIF
├── crazyflie_demo.py MuJoCo Crazyflie 2, same five trajectories
└── generate_nominal_control_4_rl.py headless run → nominal control dataset
models/
├── skydio_x2/ MJCF model (MuJoCo Menagerie, Apache 2.0)
└── bitcraze_crazyflie_2/ MJCF model (MuJoCo Menagerie, MIT)
media/ the GIFs above
- Timings quoted here are per control step at the default configuration
(
M = 3,N = 2, 1 s horizon, 10 shooting intervals) on a laptop CPU, as printed by the demos themselves. Your numbers will differ; the demos report theirs on every run. - The GIF recorder quantises every frame to one shared palette. The camera is fixed, so that is what keeps the files small — per-frame adaptive palettes measured 2.7× larger on the same footage.
- Developed and tested on Linux with Python 3.10.
MIT — see LICENSE.
Both MJCF models under models/ come from
MuJoCo Menagerie and keep their own
licenses, which travel with the model directories: the Skydio X2 assets were provided by
Skydio under the Apache 2.0 License, and the
Bitcraze Crazyflie 2 description is MIT.
Original implementation: santoshrajkumar/kq-lmpc-quadrotor.
QP solver: DAQP. Airframe model:
MuJoCo Menagerie.
Go Buckeyes! 🌰