EOM 6DOF ECEF Quaternion — Robotics/Equations Of Motion

EOM 6DOF ECEF Quaternion

Robotics / Equations Of Motion

The six-degree-of-freedom equations of motion of a rigid body of fixed 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 − ω_b × (Iω_b)) divided on the RIGHT by I, which applies I^−T
• 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].
• 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].

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].
• Mass – m, a scalar > 0. Defaults to 1.
• Inertia – the inertia tensor I, a [3,3] with a nonzero determinant; applied TRANSPOSED, as the Simulink mask applies it. Defaults to 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 fourteen 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 mass, the inertia's transposed inverse and the planet constants are folded to constants.

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/6DOF ECEF (Quaternion): 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, Mass → mass_0, Inertia → inertia, Equatorial Radius → R, Flattening → F, Planet Rotation Rate → w_E and Celestial Longitude of Greenwich → LPM0. Always emitted with mtype = Fixed, 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: fourteen continuous states, the last of them the celestial longitude of Greenwich, which only positions DCM_bi.
• 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.