Mellinger Controller

The Mellinger controller is a geometric tracking controller originally developed for aggressive quadrotor maneuvers [1]. This implementation is based on the Crazyflie firmware version.

Controller Functions

state2attitude

drone_controllers.mellinger.control.state2attitude(pos, quat, vel, ang_vel, cmd, ctrl_errors=None, ctrl_info=None, ctrl_freq=100, *, mass, kp, kd, ki, gravity_vec, mass_thrust, int_err_max, thrust_max, pwm_max)

Compute the positional part of the mellinger controller.

All controllers are implemented as pure functions. Therefore, integral errors have to be passed as an argument and returned as well.

Parameters:

Name	Type	Description	Default
pos	Array	Drone position with shape (..., 3).	required
quat	Array	Drone orientation as xyzw quaternion with shape (..., 4).	required
vel	Array	Drone velocity with shape (..., 3).	required
ang_vel	Array	Drone angular drone velocity in rad/s with shape (..., 3).	required
cmd	Array	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].	required
ctrl_errors	tuple[Array, ...] | None	Tuple of integral errors. For state2attitude, the tuple contains a single array (..., 3) for the position integral error or is None.	None
ctrl_info	tuple[Array, ...] | None	Tuple of arrays with additional data. Not used in state2attitude.	None
ctrl_freq	float	Control frequency in Hz	100
mass	float	Drone mass used for calculations in the controller in kg.	required
kp	Array	Proportional gain for the position controller with shape (3,).	required
kd	Array	Derivative gain for the position controller with shape (3,).	required
ki	Array	Integral gain for the position controller with shape (3,).	required
gravity_vec	Array	Gravity vector with shape (3,). We assume gravity to be in the negative z direction. E.g., [0, 0, -9.81].	required
mass_thrust	float	Conversion factor from thrust to PWM.	required
int_err_max	Array	Range of the integral error with shape (3,). i_range in the firmware.	required
thrust_max	float	Maximum thrust in N.	required
pwm_max	float	Maximum PWM value.	required

Returns:

Type	Description
Array	The RPY collective thrust command [rad, rad, rad, N], and the integral error of the position
Array	controller.

The position control component of the Mellinger controller. Converts desired position, velocity, and acceleration into attitude commands.

Example:

from drone_controllers import parametrize
from drone_controllers.mellinger import state2attitude

controller = parametrize(state2attitude, "cf2x_L250")

rpyt, pos_err_i = controller(pos, quat, vel, ang_vel, cmd)

attitude2force_torque

drone_controllers.mellinger.control.attitude2force_torque(pos, quat, vel, ang_vel, cmd, ctrl_errors=None, ctrl_info=None, ctrl_freq=500, *, kR, kw, ki_m, kd_omega, int_err_max, torque_pwm_max, thrust_max, pwm_min, pwm_max, L, KM, KF, mixing_matrix)

Compute the attitude to desired force-torque part of the Mellinger controller.

Note

We omit the axis flip in the firmware as it has only been introduced to make the controller compatible with the new frame of the Crazyflie 2.1.

Parameters:

Name	Type	Description	Default
pos	Array	Drone position with shape (..., 3).	required
quat	Array	Drone orientation as xyzw quaternion with shape (..., 4).	required
vel	Array	Drone velocity with shape (..., 3).	required
ang_vel	Array	Drone angular drone velocity in rad/s with shape (..., 3).	required
cmd	Array	Commanded attitude (roll, pitch, yaw) and total thrust [rad, rad, rad, N].	required
ctrl_errors	tuple[Array, ...] | None	Tuple of integral errors. For attitude2force_torque, the tuple contains a single array (..., 3) for the angular velocity integral error or is None.	None
ctrl_info	tuple[Array, ...] | None	Tuple of arrays with additional data. Not used in attitude2force_torque.	None
ctrl_freq	int	Control frequency in Hz	500
kR	Array	Proportional gain for the rotation error with shape (3,).	required
kw	Array	Proportional gain for the angular velocity error with shape (3,).	required
ki_m	Array	Integral gain for the rotation error with shape (3,).	required
kd_omega	Array	Derivative gain for the angular velocity error with shape (3,).	required
int_err_max	Array	Range of the integral error with shape (3,). i_range in the firmware.	required
torque_pwm_max	Array	Maximum torque in PWM.	required
thrust_max	float	Maximum thrust in N.	required
pwm_min	float	Minimum PWM value.	required
pwm_max	float	Maximum PWM value.	required
ang_vel_des		Desired angular velocity in rad/s.	required
prev_ang_vel		Previous angular velocity in rad/s.	required
prev_ang_vel_des		Previous angular velocity command in rad/s.	required
L	float	Distance from the center of the quadrotor to the center of the rotor in m.	required
KM	float	Torque constant (Nm/RPM).	required
KF	float	Force constant (N/RPM).	required
mixing_matrix	Array	Mixing matrix for the motor forces with shape (4, 3).	required

Returns:

Type	Description
tuple[Array, Array, Array]	4 Motor forces [N], i_error_m

The attitude control component that converts attitude commands into desired forces and torques.

Example:

from drone_controllers import parametrize
from drone_controllers.mellinger import attitude2force_torque

controller = parametrize(attitude2force_torque, "cf2x_L250")

force, torque, att_err_i = controller(pos, quat, vel, ang_vel, rpyt_cmd)

force_torque2rotor_vel

drone_controllers.mellinger.control.force_torque2rotor_vel(force, torque, *, thrust_min, thrust_max, L, KM, KF, mixing_matrix)

Convert desired collective thrust and torques to rotor speeds.

The firmware calculates PWMs for each motor, compensates for the battery voltage, and then applies the modified PWMs to the motors. We assume perfect battery compensation here, skip the PWM interface except for clipping, and instead return desired motor forces.

Note

The equivalent function in the crazyflie firmware is power_distribution from power_distribution_quadrotor.c.

Warning

This function assumes an X rotor configuration.

Parameters:

Name	Type	Description	Default
force	Array	Desired thrust in SI units with shape (...,).	required
torque	Array	Desired torque in SI units with shape (..., 3).	required
thrust_min	float	Minimum thrust in N.	required
thrust_max	float	Maximum thrust in N.	required
L	float	Distance from the center of the quadrotor to the center of the rotor in m.	required
KM	float	Torque constant (Nm/RPM).	required
KF	float	Force constant (N/RPM).	required
mixing_matrix	Array	Mixing matrix for the motor forces with shape (4, 3).	required

Returns:

Type	Description
Array	The desired motor forces in SI units with shape (..., 4).

Converts desired forces and torques into individual rotor velocities using the quadrotor mixing matrix.

Example:

from drone_controllers import parametrize
from drone_controllers.mellinger import force_torque2rotor_vel

controller = parametrize(force_torque2rotor_vel, "cf2x_L250")

rotor_speeds = controller(force, torque)

Parameter Classes

StateParams

drone_controllers.mellinger.params.StateParams

Parameters for the Mellinger state controller.

load(drone_model) staticmethod

Load the parameters from the config file.

Parameters for the position control loop.

AttitudeParams

drone_controllers.mellinger.params.AttitudeParams

Parameters for the Mellinger attitude controller.

load(drone_model) staticmethod

Load the parameters from the config file.

Parameters for the attitude control loop.

ForceTorqueParams

drone_controllers.mellinger.params.ForceTorqueParams

Parameters for the Mellinger force torque controller.

load(drone_model) staticmethod

Load the parameters from the config file.

Parameters for the force/torque to rotor speed conversion.

Complete Controller Pipeline

Here's how to use all three components together:

import numpy as np
from drone_controllers import parametrize
from drone_controllers.mellinger import (
    state2attitude, 
    attitude2force_torque, 
    force_torque2rotor_vel
)

# Parametrize all three controller components
state_ctrl = parametrize(state2attitude, "cf2x_L250")
attitude_ctrl = parametrize(attitude2force_torque, "cf2x_L250")
rotor_ctrl = parametrize(force_torque2rotor_vel, "cf2x_L250")

# Define state
pos = np.array([0.0, 0.0, 1.0])
quat = np.array([0.0, 0.0, 0.0, 1.0])  # [x, y, z, w]
vel = np.array([0.0, 0.0, 0.0])
ang_vel = np.array([0.0, 0.0, 0.0])

# Define command: [x, y, z, vx, vy, vz, ax, ay, az, yaw, r_rate, p_rate, y_rate]
cmd = np.array([1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

# Run the complete pipeline
rpyt, pos_err_i = state_ctrl(pos, quat, vel, ang_vel, cmd)
force, torque, att_err_i = attitude_ctrl(pos, quat, vel, ang_vel, rpyt)
rotor_speeds = rotor_ctrl(force, torque)

print(f"Final rotor speeds: {rotor_speeds} rad/s")

Integral Error Handling

The Mellinger controller uses integral terms for robustness. You must pass integral errors between calls:

# Initialize
pos_err_i = None
att_err_i = None

for step in range(100):
    # Update state and command...

    # Pass previous integral errors
    ctrl_errors = (pos_err_i,) if pos_err_i is not None else None
    rpyt, pos_err_i = state_ctrl(pos, quat, vel, ang_vel, cmd, ctrl_errors=ctrl_errors)

    ctrl_errors = (att_err_i,) if att_err_i is not None else None  
    force, torque, att_err_i = attitude_ctrl(pos, quat, vel, ang_vel, rpyt, ctrl_errors=ctrl_errors)

    rotor_speeds = rotor_ctrl(force, torque)

Array API Compatibility

All Mellinger functions support the Python Array API and can be used with JAX, PyTorch, etc.:

import jax.numpy as jnp
from jax import jit

# Convert to JAX arrays
pos_jax = jnp.array([0.0, 0.0, 1.0])
quat_jax = jnp.array([0.0, 0.0, 0.0, 1.0])
# ... other arrays

# JIT compile the controller
jit_controller = jit(parametrize(state2attitude, "cf2x_L250"))

rpyt, pos_err_i = jit_controller(pos_jax, quat_jax, vel_jax, ang_vel_jax, cmd_jax)

References

[1] D. Mellinger and V. Kumar, "Minimum snap trajectory generation and control for quadrotors," 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 2011, pp. 2520-2525, doi: 10.1109/ICRA.2011.5980409.