Flying Robots
Controllers
Nov 21, 2025
Assignment 1.5
Progress or Questions
Any progress, questions, or issues?
More in-lab hours next week needed?
Assignment 1.75
Progress or Questions
Any progress, questions, or issues?
(Geometric) Multirotor Control
Outline
Multirotor Control History
Geometric Control
1D, 2D, 3D Multirotor Control
Controller Stability
Multirotor Control History
- Near-hover linear controllers (PD, LQR) (Valenti et al. 2006),(Hoffmann et al. 2007)
- Nonlinear Controllers:
- Dynamic Feedback Linearization (DFL) (Mellinger 2012)
- Backstepping and Sliding Mode Controllers (Bouabdallah and Siegwart 2005)
- Geometric Controllers (Lee, Leok, and McClamroch 2010a)
- Incremental nonlinear dynamic inversion (INDI) (\(\approx 2016\))
- Reinforcement Learning (\(\approx 2018\))
- Model Predictive Control (\(\approx 2020\))
LQR Example
Challenges
- These controllers are based on Euler angles representations
(small angle assumption) - They exhibit singularities in complex rotational maneuvers
Small Angle Assumption for 2D Multirotor (Mellinger 2012)
- \(e_p\) and \(e_d\) are position and velocity error vectors
- \(k_p, k_d, k_{\theta}, k_{\omega} > 0\), positive constant gains
\(f = \frac{m}{\cos(\theta)} + \mathbf{g} + k_p e_p + k_d e_d\)
\(\boldsymbol{\tau_z} = k_{\theta}(\theta_d - \theta) + k_{\omega}(\dot{\theta_d} - \dot{\theta})\)
- if \(\theta=\pi/2\) then \(f = \infty\)
Large Angle Assumption
More details in (Mellinger 2012) to see the problems when using
Euler angles representations
Flipping Example
Geometric Control
Why “Geometric”?
- Control of dynamic systems:
- evolve on nonlinear manifolds
- cannot be globally identified by Euclidean metrics
(i.e. \(SE(3)=\mathbb{R}^3 \times SO(3)\))
- Geometric control adopt the geometric properties of the manifolds and provide insights to control theory
- Had already been done for fully actuated rigid bodies (Bullo and Lewis 2005)
Recall: quadrotors are underactuated systems!
Controllers
1D Multirotor
- State: \(\mathbf{x} = (z, \dot z)^\top \in \mathbb R^2\)
- Height \(z\) [m]
- Vertical velocity \(\dot z\) [m/s]
- Action: \(\mathbf{u} = (f_1) \in \mathbb R\)
- Upward thrust \(f_1\) [N]
- Parameters
- Mass \(m\) [kg], Gravity \(g\) [\(m/s^2\)]
- Dynamics: \[ \begin{align} \mathbf{\dot x} = f(\mathbf x, \mathbf u) = \begin{pmatrix} \dot z\\ \frac{f_1}{m} - g \end{pmatrix} \end{align} \]
1D Multirotor Controller
- Reference trajectory tuple: \((\ddot{z}_d, \dot{z}_d, z_d)\)
- Position and velocity errors
\(e_p = z_d - z\), \(e_d = \dot{z}_d - \dot{z}\)
- PD controller + gravity compensation
\(f_1 = m(\ddot{z}_d + k_pe_p + k_de_d + g)\), \(\quad\forall k_p, k_d > 0\)
2D/3D Multirotor Dynamics
- Dynamics: \[ \begin{align} &\dot{\mathbf{p}} = \mathbf{v}, && m\mathbf{\dot{v}} = f\mathbf{R}\mathbf{e}_z - m g\mathbf{e}_z,\\ &\dot{\mathbf{R}} = \mathbf{R}\boldsymbol{\hat{\omega}}, && \mathbf{J}\dot{\boldsymbol{\omega}} = \mathbf{J}\boldsymbol{\omega}\times \boldsymbol{\omega} + \boldsymbol{\tau}_u, \end{align} \]
2D Multirotor
- \(\mathbf{p} \in \mathbb R^{2}\)
- \(\mathbf{R} \in SO(2)\), \(\hat{\boldsymbol{\cdot}}:\) \(\mathbb R \rightarrow SO(2)\)
- Angular velocity \(w \in \mathbb R\)
- \(\mathbf{u} = (f_1, f_2) \in \mathbb R^2\)
- \(\tau_u \in \mathbb R\)
3D Multirotor
- \(\mathbf{p} \in \mathbb R^{3}\)
- \(\mathbf{R} \in SO(3)\), \(\hat{\boldsymbol{\cdot}}:\) \(\mathbb R^3 \rightarrow SO(3)\)
- Angular velocity \(w \in \mathbb R^{3}\)
- \(\mathbf{u} = (f_1, f_2, f_3, f_4) \in \mathbb R^4\)
- \(\tau_u \in \mathbb R^3\)
2D/3D Multirotor Controller (Lee, Leok, and McClamroch 2010a)
Control Structure
Trajectory Tracking Controller
- Reference trajectory tuple: \(\mathbf{r}_d = (\dddot{\mathbf{p}}_d(t), \ddot{\mathbf{p}}_d(t), \dot{\mathbf{p}}_d(t), \mathbf{p}_d(t))\)
- Position and velocity errors \(\mathbf{e}_p = \mathbf{p}_d - \mathbf{p}\), \(\mathbf{e}_d = \dot{\mathbf{p}}_d - \dot{\mathbf{p}}\)
- Trajectory tracking controller: PD + gravity compensation
\(\mathbf{F}_d = m(\ddot{\mathbf{p}}_d + \mathbf{K}_p \mathbf{e}_p + \mathbf{K}_v \mathbf{e}_v + g \mathbf{e}_z)\)
\(f = \mathbf{F}_d \cdot \mathbf{R} \mathbf{e}_z\)
where \(\mathbf{K}_p, \mathbf{K}_v \in \mathbb R^{3\times 3}\) are positive definite diagonal gain matrices
Attitude Tracking Controller
We need to define a desired rotation \(\mathbf{R}_d\) that
can align \(z_b\) with \(\mathbf{F}_d\)
\(\mathbf{R}_d = [x_{b_d}\quad y_{b_d}\quad z_{b_d}]\)
Attitude Tracking Controller
- Let \(\psi_d\) be the desired heading (yaw angle)
- Let \(n(\mathbf{x})= \mathbf{x}/ \| \mathbf{x}\|\) the normalized vector
- Then define:
\[ \begin{align} \mathbf{x}_{c_d} &= \begin{bmatrix} \cos(\psi_d) & \sin(\psi_d) & 0\end{bmatrix}^\top\\ \mathbf{y}_{c_d} &= \begin{bmatrix} -\sin(\psi_d) & \cos(\psi_d) & 0\end{bmatrix}^\top\\ \mathbf{x}_{b_d} &= n(\mathbf{y}_{c_d} \times \mathbf{F}_d)\\ \mathbf{y}_{b_d} &= n(\mathbf{F}_d \times \mathbf{x}_{b_d})\\ \mathbf{z}_{b_d} &= \mathbf{x}_{b_d} \times \mathbf{y}_{b_d} \end{align} \]
\(\mathbf{R}_d = \begin{bmatrix} \mathbf{x}_{b_d} & \mathbf{y}_{b_d} & \mathbf{z}_{b_d} \end{bmatrix} \in SO(3)\)
Attitude Tracking Controller (Lee, Leok, and McClamroch 2010a)
\(\boldsymbol{\tau}_u = -\mathbf{K}_R \mathbf{e}_R - \mathbf{K}_\omega \mathbf{e}_\omega + \boldsymbol{\omega}\times \mathbf{J}\boldsymbol{\omega}- \mathbf{J}(\hat{\boldsymbol{\omega}} \mathbf{R}^T \mathbf{R}_d \boldsymbol{\omega}_d - \mathbf{R}^T \mathbf{R}_d \dot{\boldsymbol{\omega}}_d)\)
- How to compute errors \(\mathbf{e}_R, \mathbf{e}_\omega\)?
- Define an error metric on \(SO(3)\)
\(\Psi(\mathbf{R}, \mathbf{R}_d) = \frac{1}{2} \text{tr}(\mathbf{I} - \mathbf{R}^T \mathbf{R}_d)\)
- From this, you can derive the orientation error
\(\mathbf{e}_R = \frac{1}{2} \left( \mathbf{R}^T_d \mathbf{R} - \mathbf{R}^T \mathbf{R}_d \right)^\vee\), \((\cdot)^\vee\): \(SO(3) \rightarrow \mathbb R^3\)
- angular velocity error: computed from \(\dot{\mathbf{R}}\) and \(\dot{\mathbf{R}}_d\) (indirectly)
\(\mathbf{e}_\omega = \boldsymbol{\omega}- \mathbf{R}^T \mathbf{R}_d \boldsymbol{\omega}_d\)
Control Law
\(f = m(\ddot{\mathbf{p}}_d + \mathbf{K}_p \mathbf{e}_p + \mathbf{K}_v \mathbf{e}_v + g \mathbf{e}_z) \cdot \mathbf{R}\mathbf{e}_z\)
\(\boldsymbol{\tau}_u = -\mathbf{K}_R \mathbf{e}_R - \mathbf{K}_\omega \mathbf{e}_\omega + \boldsymbol{\omega}\times \mathbf{J}\boldsymbol{\omega}- \mathbf{J}(\hat{\boldsymbol{\omega}} \mathbf{R}^T \mathbf{R}_d \boldsymbol{\omega}_d - \mathbf{R}^T \mathbf{R}_d \dot{\boldsymbol{\omega}}_d)\)
- How to compute \(\boldsymbol{\omega}_d\) and \(\dot{\boldsymbol{\omega}}_d\)?
Desired Angular States (Mellinger and Kumar 2011)
\[ \begin{align} c &= \mathbf{z}_{b_d}^\top (\ddot{\mathbf{p}}_d + g \mathbf{e}_z)\\ \end{align} \]
\[ \begin{align} d_1 &= \mathbf{x}_{b_d}^\top \dddot{\mathbf{p}}_d & d_2 &= -\mathbf{y}_{b_d}^\top \dddot{\mathbf{p}}_d \\ b_3 &= -\mathbf{y}_{c_d}^\top \mathbf{z}_{b_d} & c_3 &= \| \mathbf{y}_{c_d} \times \mathbf{z}_{b_d} \|& d_3 &= \dot \psi_d \mathbf{x}_{c_d}^\top \mathbf{x}_{b_d}\\ \omega_{x_d} &= \frac{d_2}{c} & \omega_{y_d} &= \frac{d_1}{c} & \omega_{z_d} &= \frac{c d_3 - b_3 d_1}{c c_3} \end{align} \]
- \(\boldsymbol{\omega}_d = \begin{bmatrix} \omega_{x_d} & \omega_{y_d} & \omega_{z_d} \end{bmatrix}^\top\)
- For \(\dot{\boldsymbol{\omega}}_d\), compute the derivative of \(\boldsymbol{\omega}_d\) w.r.t time (\(\dot{\boldsymbol{\omega}}_d = \mathbf{0}\) also works for simple cases)
- Will be revisited in the motion planning part
Note that \(\omega_z\) is wrong in (Mellinger and Kumar 2011). Details are in (Faessler, Franchi, and Scaramuzza 2018) (Appendix A), from which we obtained the results on the slides.
Controller Stability Properties
Lyapunov stability analysis
Proof is in the appendix (Lee, Leok, and McClamroch 2010b)
The dynamics are exponentially stable, when the initial conditions satisfy two conditions:
\(\Psi (\mathbf{R}, \mathbf{R}_d) < 2\)
\(\|\mathbf{e}_\omega\|^2 < \frac{2}{\lambda_{\text{max}}(\mathbf J)} k_R (2 - \Psi (\mathbf{R}, \mathbf{R}_d))\),
- where \(\lambda_{\text{max}}(\mathbf J)\) is the maximum eigenvalue of the inertia matrix
- \(k_R\) is the minimum value in the \(\mathbf{K}_R\in \mathbb R^{3\times 3}\) gain diagonal matrix.
Advanced Topics
Incremental nonlinear dynamic inversion (INDI) (Smeur, Chu, and de Croon 2016; Tal and Karaman 2021)
Key insight: the real dynamics are an inaccurate model
Measure the motor RPM, compute dynamics mismatch online, and compensate for it in the controller \[ \begin{align} &\dot{\mathbf{p}} = \mathbf{v}, && m\dot{\mathbf{v}} = f \mathbf{R} \mathbf{e}_z - m g \mathbf{e}_z + \mathbf{f}_a,\\ &\dot{\mathbf{R}} = \mathbf{R}\hat{\boldsymbol{\omega}}, && \mathbf{J}\dot{\boldsymbol{\omega}} = \mathbf{J}\boldsymbol{\omega}\times \boldsymbol{\omega}+ \boldsymbol{\tau}_u + \boldsymbol{\tau}_a \end{align} \]
When measuring motor rpm, we essentially know \(f, \boldsymbol{\tau}_u\) and can compute \(\mathbf{f}_a\) and \(\boldsymbol{\tau}_a\)
controller can be slightly changed to
\[ f = (-\mathbf{K}_p \mathbf{e}_p - \mathbf{K}_v \mathbf{e}_v + m g \mathbf{e}_z + m \ddot{\mathbf{p}}_d - \mathbf{f}_a) \cdot \mathbf{R}\mathbf{e}_z, \]
- Dramatic improvement in tracking error (factor of 5)
Nonlinear MPC
- At a fixed frequency, solve an optimal control problem
\[ \begin{aligned} \arg\min_{\mathbf u} \|\mathbf x_N - \mathbf x_{N_d}\| + \sum_{k=0}^{N-1} \left( \|\mathbf x_k - \mathbf x_{k_d}\| + \|\mathbf u_k - \mathbf u_{k_d}\| \right) s.t.\newline \mathbf x_0 = \text{current state}\\ \mathbf x_{k+1} = \text{step}(\mathbf x_{k}, \mathbf u_{k})\\ \text{actuation and state bounds} \end{aligned} \]
- Execute first action and repeat
Example (Foehn, Romero, and Scaramuzza 2021)
RL
- Use a simulator to train a policy
- Input: state
- Output: motor RPM, desired force/torque, or attitude setpoint
- For sim-to-real transfer use domain randomization
- Sample different inertia and physical parameters
- Modelling motor delays is a must
Example (Molchanov et al. 2019)
Example 2 (Lorentz et al. 2025)
Controller Comparison (Sun et al. 2022)
- Geometric controllers and NMPC have similar performance if reference is valid
- NMPC handles invalid trajectories better
- For both, more advanced attitude controllers are important
- Incremental nonlinear dynamic inversion (INDI) (uses IMU rather than model)
- QP-based control allocation
- RL has no (stability) guarantees, but works best in drone racing
Payload Transport
3 Crazyflies transporting a triangular payload with cables
Payload Transport
Hierarchical control structure multiple UAVs transporting a payload
Dual Quaternions
- Expressing a pose can also be done with dual quaternions (8 values), rather than positional part + quaternions (13 values)
- Controllers show (slightly) lower tracking errors (Wang and Yu 2013; Recalde et al. 2025)
Example: DQ-NMPC
Assignment 2
Task
Implement the Lee geometric controller for the Bitcraze Crazyflie 2.1 robot. Test and tune in your simulator (Assignment 1). Execute physical test flights with your controller and report the tracking errors.
Simulation
- The output of the controller is essentially \(\eta\); avoid using PWM conversion altogether
- For working gains (and other inspiration), see the Lee controller in the firmware
Testing steps:
- Start with a step response (i.e., slight change in position, zero for derivatives)
- Test with a circle trajectory in the \(xy\)-plane
- Add support for csv-file setpoints
- Tune the gains with the goal to minimize the cumulative position error on the figure-8 trajectory
Physical Flight
- Be careful with the units - the gyro is deg/s in the firmware
Steps:
- Prepare the bindings using `bindgen’
- Add your controller code, make sure to switch to
f32and switch from std to core - Try flying with cfclient, similar to the initial demo
- Use the uSD-card deck to log flight data
- Report the cumulative position error for the figure-8 trajectory (you can run it via an official example script)
Conclusion
Learned Today
- Multirotor controllers
- Linearized controllers, geometric controllers, nonlinear MPC, RL
- Details of the state-of-the-art geometric controller
- Assignment 2
Next 3.5 Weeks
- Discussion for Assignment 2.
- Presentations of simulation (not graded).
- Discussions and flight demos (2 weeks).
Questions
?