Canonical RHS excerpt from the registered callable used for this benchmark cell. Expand it to verify the state equations; it is not a standalone runnable fixture.
Show canonical RHS excerpt
def rhs_ekf_drone_predict(t: float, y: np.ndarray) -> np.ndarray:
vx, vy, vz = y[3], y[4], y[5]
phi, theta, psi = y[6], y[7], y[8]
p, q, r = y[9], y[10], y[11]
cp, sp = np.cos(phi), np.sin(phi)
ct, st = np.cos(theta), np.sin(theta)
cy, sy = np.cos(psi), np.sin(psi)
a_thrust = _T_HOVER / _M
fx = a_thrust * (cp * st * cy + sp * sy) - _DRAG * vx
fy = a_thrust * (cp * st * sy - sp * cy) - _DRAG * vy
fz = a_thrust * cp * ct - _G - _DRAG * vz
th_c = np.clip(theta, -_THETA_CLIP, _THETA_CLIP)
tan_th = np.tan(th_c)
ct_c = np.cos(th_c)
sec_th = 1.0 / ct_c if abs(ct_c) > 1e-12 else 1e12 * np.sign(ct_c)
d = np.empty(12)
d[0] = vx
d[1] = vy
d[2] = vz
d[3] = fx
d[4] = fy
d[5] = fz
d[6] = p + q * sp * tan_th + r * cp * tan_th
d[7] = q * cp - r * sp
d[8] = (q * sp + r * cp) * sec_th
d[9] = (_IY - _IZ) / _IX * q * r
d[10] = (_IZ - _IX) / _IY * p * r
d[11] = (_IX - _IY) / _IZ * p * q
return d- Parameters
- _DRAG = 0.1
- _G = 9.81
- _IX = 0.029
- _IY = 0.029
- _IZ = 0.055
- _M = 1.5
- _THETA_CLIP = 1.48
- _T_HOVER = 14.715
- Initial condition
- y(0) = [0, 0, 10, 1, 0, 0, 0, 0.05, 0, 0, 0, 0]
- Horizon
- t ∈ [0, 30]
Canonical RHS excerpt captured from the same registered callable used for the published benchmark. Frozen closure values are summarized below; helper imports and solver settings are intentionally omitted.