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)[source]#

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 (ndarray | 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)[source]#

Set ground-truth inertial parameters.

Parameters:

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

Return type:

None

get_true_inertial_params()[source]#

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:

ndarray

set_estimated_inertial_params(theta)[source]#

Update the controller’s believed inertial parameters.

Parameters:

theta (ndarray)

Return type:

None

get_estimated_inertial_params()[source]#

Return the 10-vector of estimated inertial parameters.

Return type:

ndarray

get_hover_thrust_est()[source]#

Per-motor hover thrust from estimated mass.

Return type:

ndarray

get_hover_thrust_true()[source]#

Per-motor hover thrust from true mass.

Return type:

ndarray

dynamics_true(x, u, wind_vec=None)[source]#

Continuous-time dynamics using ground-truth parameters.

Return type:

ndarray

Parameters:
dynamics_rk4_true(x, u, dt=None, wind_vec=None)[source]#

RK4 step using ground-truth parameters.

Return type:

ndarray

Parameters:
dynamics_est(x, u, wind_vec=None)[source]#

Continuous-time dynamics using estimated parameters.

Return type:

ndarray

Parameters:
dynamics_rk4_est(x, u, dt=None, wind_vec=None)[source]#

RK4 step using estimated parameters.

Return type:

ndarray

Parameters:
get_linearized_true(x_ref, u_ref)[source]#

Linearise true dynamics about (x_ref, u_ref).

Returns:

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

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

Parameters:
get_linearized_est(x_ref, u_ref)[source]#

Linearise estimated dynamics about (x_ref, u_ref).

Returns:

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

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

Parameters:
get_data_matrix(x, dx)[source]#

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 (ndarray) – State [r, q, v, omega].

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

Return type:

ndarray

get_force_vector(x_curr, dx, u_curr=None)[source]#

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:
Return type:

ndarray

attach_payload(m_p, delta_r_p)[source]#

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

Parameters:

  • m_p (float) – Payload mass.

  • delta_r_p (ndarray) – Body-frame position of the payload relative to the vehicle origin.

Return type:

None

detach_payload()[source]#

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)[source]#

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

Parameters:
Return type:

ndarray

aero_added_inertia(wind_vec)[source]#

Apply aerodynamic added-inertia perturbation from a wind vector.

Parameters:

wind_vec (ndarray)

Return type:

None

add_drift(mass_std=0.0, inertia_std=0.0, com_std=0.0)[source]#

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)[source]#

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

Parameters:

v (ndarray)

Return type:

ndarray

static L(q)[source]#

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

Parameters:

q (ndarray)

Return type:

ndarray

classmethod qtoQ(q)[source]#

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

Parameters:

q (ndarray)

Return type:

ndarray

classmethod G(q)[source]#

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

Parameters:

q (ndarray)

Return type:

ndarray

static rptoq(phi)[source]#

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

Parameters:

phi (ndarray)

Return type:

ndarray

static qtorp(q)[source]#

Unit quaternion -> Rodrigues parameters.

Parameters:

q (ndarray)

Return type:

ndarray

classmethod E(q)[source]#

Block-diagonal embedding for reduced-state linearisation.

Parameters:

q (ndarray)

Return type:

ndarray

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)[source]#

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)[source]#

Continuous-time equations of motion.

Parameters:

  • state (ndarray) – [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 (ndarray) – Torque applied to the second link.

Returns:

[omega_1, alpha_1, omega_2, alpha_2]

Return type:

ndarray

step(x, u)[source]#

Advance the state by one time-step using RK4.

Parameters:
Return type:

ndarray

linearize(x_ref, u_ref)[source]#

Linearise the discrete dynamics about (x_ref, u_ref).

Returns:

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

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

Parameters: