EOM 6DOF Simple Variable Mass ECEF Quaternion — Robotics/Equations Of Motion

EOM 6DOF Simple Variable Mass ECEF Quaternion

Robotics / Equations Of Motion

The six-degree-of-freedom equations of motion of a rigid body with a fuel tank between an empty and a full mass, over a rotating ellipsoidal planet, carried in the planet-fixed (ECEF) frame with the attitude as a quaternion. With the body velocity V_b relative to the planet, the body rates ω_b relative to the inertial frame, the planet rate ω_e and the planet-fixed position X_f:

• V_b' = F/m − V_b × (ω_b + DCM_bfω_e) − DCM_bf(ω_e × (ω_e × X_f)) – the Coriolis and centripetal terms of the rotating frame
• ω_b' = ((M − (dI/dt)ω_b) − ω_b × (Iω_b)) divided on the RIGHT by I, which applies I^−T
• I(m) = I_e(1 − λ) + I_fλ with λ the tank's fill fraction, and dI/dt = ((I_f − I_e)/(m_f − m_e))·dm/dt – the rate is NOT gated at the limits
• m' = dm/dt, zero while the mass sits at the limit the flow is heading for
• q' = ½Ω(ω_b − DCM_bfω_e)q – the attitude is relative to the PLANET, and the unit quaternion drives it
• X_f' = DCM_bf^TV_b

The geodetic latitude, longitude and altitude come from the planet-fixed position by thirty-two passes of Bowring's recursion, exactly as Robotics / Axes Transformations / ECEF Position To LLA runs it.

Ports

• F – the applied force in body axes, [3,1].
• M – the applied moment about the centre of gravity in body axes, [3,1].
• m_dot – dm/dt, the fuel flow, a scalar; negative drains the tank.
• V_ECEF – the velocity relative to the planet, in planet-fixed axes, DCM_bf^TV_b, [3,1].
• X_ECEF – the planet-fixed position X_f, [3,1].
• LLA – geodetic latitude and longitude in degrees and the altitude in the position's own unit, [3,1].
• Euler – the Euler angles [φ; θ; ψ] relative to north-east-down, in radians, read back from DCM_be, [3,1].
• DCM_bi – the direction cosine matrix from the inertial frame to body axes, [3,3].
• DCM_be – from north-east-down to body axes, [3,3].
• DCM_ef – from the planet-fixed frame to north-east-down, [3,3].
• Vb – the body velocity relative to the planet [u; v; w], [3,1].
• wb_rel – the body rates relative to north-east-down, [3,1].
• wb – the body rates relative to the inertial frame, [3,1].
• dwb – their derivative, [3,1].
• Ab – the body acceleration relative to the planet, V_b', [3,1].
• Fuel – the tank status, [1,1]: +1 at the full mass, −1 at the empty one and 0 between.

Parameters

• Units – Metric (MKS) (the default) or English (Velocity in ft/s). The two are the same arithmetic, so the choice only says which units the numbers are in.
• Initial Location [mu l h] – the geodetic latitude and longitude in degrees and the altitude, three values. Defaults to [0 0 0].
• Initial Velocity [U V W] – the initial body velocity relative to the planet, three values. Defaults to [0 0 0].
• Initial Euler Angles [phi theta psi] – relative to north-east-down, in radians, three values. Defaults to [0 0 0].
• Initial Body Rates [p q r] – relative to north-east-down, in rad/s, three values; the inertial rates the block carries are seeded from them and from the planet and transport rates at the initial state. Defaults to [0 0 0].
• Initial Mass – the tank's starting mass, a scalar > 0. Defaults to 1.
• Empty Mass – the lower limit. Defaults to 0.5.
• Full Mass – the upper limit. Defaults to 2.
• Empty Inertia – the inertia tensor at the empty mass, a [3,3]; applied TRANSPOSED, as the Simulink mask applies it. Defaults to eye(3).
• Full Inertia – the inertia tensor at the full mass, a [3,3]. Defaults to 2*eye(3).
• Equatorial Radius – R, the planet's equatorial radius. Defaults to the WGS84 value 6378137.
• Flattening – f, in [0, 1). Defaults to the WGS84 value 0.0033528106647474805.
• Planet Rotation Rate – ω_e in rad/s about the polar axis. Defaults to the WGS84 value 7.292115e-05.
• Celestial Longitude of Greenwich – the angle in degrees between the inertial frame's x axis and the prime meridian at t = 0; it grows at the rotation rate and positions DCM_bi. Defaults to 0.
• Integration Substeps – M, how many fourth-order Runge-Kutta steps a sample is integrated with on the discrete solver and in exported code, a whole number of 1 or more. Defaults to 100. No Simulink counterpart.
• Sampling Time (s) – zero or less inherits the solver's rate; a positive value runs the block at that period.

Code export

All ten targets: Python, MATLAB, Java, Rust, C, C++, VHDL, Verilog, SystemVerilog and PLC Structured Text (which has no atan2, so the angle read-back rebuilds it from ATAN). A core holds the fifteen states, publishes the twelve outputs from them and from the inputs, and integrates one sample with the inputs held and M Runge-Kutta substeps – with M = 100, the same computation as Simulink's fixed-step ode4 at one hundredth of the sample time. The planet constants, the two tensors and the interpolation slope are folded to constants; the inertia's inverse comes from the adjugate at every derivative evaluation, because the tensor follows the tank.

The three HDL targets are simulation-only real arithmetic, quantized at the port: a square root, an arctangent and a thirty-two pass recursion have no Q16.16 form. ⚠ Their ports also carry Q16.16, which holds nothing past about 32767, so a planet-fixed position in metres on an Earth-sized planet (6.4×10⁶) saturates there – the HDL cores are exercised on a small planet, and a full-size one belongs in the seven software targets.

Simulink bridge

Import and export, mapped to Aerospace Blockset's aerolib6dof2/Simple Variable Mass 6DOF ECEF (Quaternion) (the library name carries an embedded newline): Units → units (the two offered values 1:1), Initial Location [mu l h] → xg_0, Initial Velocity [U V W] → Vm_0, Initial Euler Angles [phi theta psi] → eul_0, Initial Body Rates [p q r] → pm_0, Initial Mass → mass_0, Empty Mass → mass_e, Full Mass → mass_f, Empty Inertia → inertia_e, Full Inertia → inertia_f, Equatorial Radius → R, Flattening → F, Planet Rotation Rate → w_E and Celestial Longitude of Greenwich → LPM0. Always emitted with mtype = Simple Variable, ptype = Custom (a block left on Earth (WGS84) accepts a written radius and discards it), angle_in = Internal and vre_flag and abi_flag off: each of those moves that block's port list, and this block has the one it is drawn with. The knots unit system is not offered, and an imported one is reported. "Sampling Time (s)" does not cross: the Simulink block is continuous and defines no SampleTime.

Notes

• Stateful, continuous and nonlinear: fifteen continuous states – the twelve of the rigid body, the celestial longitude of Greenwich (which only positions DCM_bi) and the tank's mass, which is clamped to its limits after every substep.
• The two acceleration outputs are functions of the inputs at the same instant, so the block HAS direct feedthrough and a loop closed around it is an algebraic loop.
• No gimbal lock: the quaternion kinematics are regular at every attitude. The Euler-angle OUTPUT is still a read-back, so φ and ψ wrap at ±π and θ is clamped to the arcsine's domain at ±90°.
• The latitude recursion runs a fixed thirty-two passes rather than to a tolerance: a loop that stops when it likes cannot be written identically in ten languages.