Dynamics

Dynamical system models for parameter estimation benchmarks.

Quadrotor

Full 6-DOF quadrotor dynamics with inertial parameter management.

This module implements a rigid-body quadrotor model with:

• Separate ground-truth and estimated inertial parameters (mass, center of mass, full 3x3 inertia tensor).

• Newton-Euler data matrix/force vector for parameter estimation (A theta = b).

• Payload attach/detach via parallel-axis theorem.

• Aerodynamic added-inertia and parameter drift utilities.

• RK4 integration and autograd-based linearisation.

class online_estimators.dynamics.quadrotor.QuadrotorDynamics(r_off=None)

Bases: object

Rigid-body quadrotor with separate true/estimated inertial parameters.

The model uses a quaternion attitude representation q = [q_w, q_x, q_y, q_z] and a 13-dimensional state vector:

x = [r(3), q(4), v(3), omega(3)]

where r is the world-frame position, q the body->world quaternion, v the world-frame linear velocity, and omega the body-frame angular velocity.

Parameters:

r_off (array-like or None) – Initial body-frame COM offset (3-vector). Defaults to zero.

nx

State dimension (12 after quaternion reduction).

Type:

int

nu

Control dimension (4 motors).

Type:

int

dt

Default integration time-step (1/freq).

Type:

float

freq

Control frequency in Hz.

Type:

float

T = array([[ 1.,  0.,  0.,  0.],        [ 0., -1.,  0.,  0.],        [ 0.,  0., -1.,  0.],        [ 0.,  0.,  0., -1.]])

H = array([[0., 0., 0.],        [1., 0., 0.],        [0., 1., 0.],        [0., 0., 1.]])

set_true_inertial_params(theta)

Set ground-truth inertial parameters.

Parameters:

theta (np.ndarray, shape (10,)) – [m, m*cx, m*cy, m*cz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz]

Return type:

None

get_true_inertial_params()

Return the 10-vector of ground-truth inertial parameters.

Returns:

[m, m*cx, m*cy, m*cz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz]

Return type:

np.ndarray, shape (10,)

set_estimated_inertial_params(theta)

Update the controller’s believed inertial parameters.

Parameters:

theta (np.ndarray, shape (10,))

Return type:

None

get_estimated_inertial_params()

Return the 10-vector of estimated inertial parameters.

Return type:

np.ndarray, shape (10,)

get_hover_thrust_est()

Per-motor hover thrust from estimated mass.

Return type:

ndarray

get_hover_thrust_true()

Per-motor hover thrust from true mass.

Return type:

ndarray

dynamics_true(x, u, wind_vec=None)

Continuous-time dynamics using ground-truth parameters.

Return type:

ndarray

Parameters:

• x (ndarray)

• u (ndarray)

dynamics_rk4_true(x, u, dt=None, wind_vec=None)

RK4 step using ground-truth parameters.

Return type:

ndarray

Parameters:

• x (ndarray)

• u (ndarray)

dynamics_est(x, u, wind_vec=None)

Continuous-time dynamics using estimated parameters.

Return type:

ndarray

Parameters:

• x (ndarray)

• u (ndarray)

dynamics_rk4_est(x, u, dt=None, wind_vec=None)

RK4 step using estimated parameters.

Return type:

ndarray

Parameters:

• x (ndarray)

• u (ndarray)

get_linearized_true(x_ref, u_ref)

Linearise true dynamics about (x_ref, u_ref).

Returns:

• A (np.ndarray, shape (12, 12))

• B (np.ndarray, shape (12, 4))

Parameters:

• x_ref (ndarray)

• u_ref (ndarray)

get_linearized_est(x_ref, u_ref)

Linearise estimated dynamics about (x_ref, u_ref).

Returns:

• A (np.ndarray, shape (12, 12))

• B (np.ndarray, shape (12, 4))

Parameters:

• x_ref (ndarray)

• u_ref (ndarray)

get_data_matrix(x, dx)

Build the 6x10 Newton-Euler data matrix A.

Constructs the matrix such that A theta = w where theta = [m, mcx, mcy, mcz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz].

Forces and torques are expressed in the body frame.

Parameters:

• x (np.ndarray, shape (13,)) – State [r, q, v, omega].

• dx (np.ndarray, shape (13,)) – State derivative [rdot, q_dot, v_dot_world, alpha_body].

Return type:

np.ndarray, shape (6, 10)

get_force_vector(x_curr, dx, u_curr=None)

Build the 6x1 wrench vector b (body frame).

The wrench is purely gravito-inertial (excludes motor forces), matching get_data_matrix() so that A theta = b.

Parameters:

• x_curr (np.ndarray, shape (13,))

• dx (np.ndarray, shape (13,))

• u_curr (np.ndarray or None) – Unused; kept for API compatibility.

Return type:

np.ndarray, shape (6,)

attach_payload(m_p, delta_r_p)

Attach a point-mass payload via the parallel-axis theorem.

Parameters:

• m_p (float) – Payload mass.

• delta_r_p (array-like, shape (3,)) – Body-frame position of the payload relative to the vehicle origin.

Return type:

None

detach_payload()

Detach (drop) the most recently attached payload.

Raises:

• RuntimeError – If no payloads are attached.

• ValueError – If removing the payload would yield non-positive mass.

Return type:

None

static closest_spd(theta, epsilon=1e-06)

Project the inertia sub-matrix of theta to the nearest SPD matrix.

Parameters:

• theta (np.ndarray, shape (10,))

• epsilon (float) – Eigenvalue floor.

Return type:

np.ndarray, shape (10,)

aero_added_inertia(wind_vec)

Apply aerodynamic added-inertia perturbation from a wind vector.

Parameters:

wind_vec (np.ndarray, shape (3,))

Return type:

None

add_drift(mass_std=0.0, inertia_std=0.0, com_std=0.0)

Add Gaussian drift to true inertial parameters.

Parameters:

• mass_std (float) – Standard deviation for mass drift.

• inertia_std (float) – Standard deviation for inertia-tensor element drift.

• com_std (float) – Standard deviation for center-of-mass drift.

Return type:

None

static hat(v)

Skew-symmetric matrix from 3-vector: hat(u) v == u x v.

Parameters:

v (np.ndarray, shape (3,))

Return type:

np.ndarray, shape (3, 3)

static L(q)

Left-quaternion multiplication matrix: L(q) p == q (x) p.

Parameters:

q (np.ndarray, shape (4,))

Return type:

np.ndarray, shape (4, 4)

classmethod qtoQ(q)

Quaternion to 3x3 rotation matrix (body -> world).

Parameters:

q (np.ndarray, shape (4,))

Return type:

np.ndarray, shape (3, 3)

classmethod G(q)

Quaternion kinematic Jacobian G(q) = L(q) H.

Parameters:

q (np.ndarray, shape (4,))

Return type:

np.ndarray, shape (4, 3)

static rptoq(phi)

Rodrigues parameters (3-vector) -> unit quaternion.

Parameters:

phi (np.ndarray, shape (3,))

Return type:

np.ndarray, shape (4,)

static qtorp(q)

Unit quaternion -> Rodrigues parameters.

Parameters:

q (np.ndarray, shape (4,))

Return type:

np.ndarray, shape (3,)

classmethod E(q)

Block-diagonal embedding for reduced-state linearisation.

Parameters:

q (np.ndarray, shape (4,))

Return type:

np.ndarray, shape (13, 12)

Double Pendulum

Double pendulum dynamics with RK4 integration.

Implements the equations of motion for a planar double pendulum with autograd-based Jacobian computation for linearisation.

class online_estimators.dynamics.double_pendulum.DoublePendulum(g=9.81, m1=1.0, m2=1.0, l1=1.0, l2=1.0, dt=0.01)

Bases: object

Planar double pendulum with autograd-based linearisation.

State vector: x = [theta_1, omega_1, theta_2, omega_2]

Parameters:

• g (float) – Gravitational acceleration (m/s^2).

• m1 (float) – Link masses (kg).

• m2 (float) – Link masses (kg).

• l1 (float) – Link lengths (m).

• l2 (float) – Link lengths (m).

• dt (float) – Integration time-step (s).

Examples

>>> from online_estimators.dynamics import DoublePendulum
>>> dp = DoublePendulum()
>>> x0 = np.array([0.1, 0.0, 0.2, 0.0])
>>> u = np.array([0.0])
>>> x1 = dp.step(x0, u)

continuous_dynamics(state, m1, m2, l1, l2, g, u)

Continuous-time equations of motion.

Parameters:

• state (np.ndarray, shape (4,)) – [theta_1, omega_1, theta_2, omega_2]

• m1 (float) – Physical parameters.

• m2 (float) – Physical parameters.

• l1 (float) – Physical parameters.

• l2 (float) – Physical parameters.

• g (float) – Physical parameters.

• u (np.ndarray, shape (1,)) – Torque applied to the second link.

Returns:

[omega_1, alpha_1, omega_2, alpha_2]

Return type:

np.ndarray, shape (4,)

step(x, u)

Advance the state by one time-step using RK4.

Parameters:

• x (np.ndarray, shape (4,))

• u (np.ndarray, shape (1,))

Return type:

np.ndarray, shape (4,)

linearize(x_ref, u_ref)

Linearise the discrete dynamics about (x_ref, u_ref).

Returns:

• A (np.ndarray, shape (4, 4))

• B (np.ndarray, shape (4, 1))

Parameters:

• x_ref (ndarray)

• u_ref (ndarray)