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

EOM 6DOF Simple Variable Mass Wind Quaternion

Robotics / Equations Of Motion

The six-degree-of-freedom equations of motion of a rigid body that burns fuel, with the translation carried in wind axes as the airspeed V, the angle of attack α and the sideslip angle β, and the attitude as a body quaternion q, so there is no gimbal lock. With the force F_w = [F_x; F_y; F_z] in wind axes, the moment M in body axes, the body rates ω = [p; q; r], the mass m a state driven by the mass flow rate and held between the empty and full masses, and the inertia tensor following it linearly between its empty and full values:

• V' = F_x/m
• α' = q − (p·cos α + r·sin α)·tan β + F_z/(m·V·cos β)
• β' = p·sin α − r·cos α + F_y/(m·V)
• I(m) = (1 − λ)·I_e + λ·I_f, λ = (m − m_e)/(m_f − m_e), and dI/dt = (I_f − I_e)/(m_f − m_e)·dm/dt – on the raw rate, even at a limit, as the Simulink block does it
• m' = dm/dt, except zero while the tank is empty and draining or full and filling
• ω' = I^−T(M − dI/dt·ω − ω × (Iω)) – the transpose of the inverse, as the Simulink block divides a row vector by I; the two are the same for a symmetric tensor
• q' = ½Ω(ω)·q/|q| – the kinematics are evaluated on the normalized quaternion
• X_e' = DCM_we^T·[V; 0; 0], with DCM_we = DCM_wb(α, β)·DCM_be(q)

Ports

• Fw – the applied force in wind axes, [3,1].
• M – the applied moment about the centre of gravity in body axes, [3,1].
• m_dot – the mass flow rate dm/dt (negative while burning), a scalar [1,1].
• Ve – the velocity in the flat-earth frame, DCM_we^T·V_w, [3,1].
• Xe – the position in the flat-earth frame, [3,1].
• Wind – the wind angles [μ; γ; χ] (bank, flight path and heading) in radians, read back from DCM_we: μ = atan2(D₂₃, D₃₃), γ = asin(−D₁₃), χ = atan2(D₁₂, D₁₁), [3,1].
• DCMwe – the direction cosine matrix from the flat-earth frame to wind axes, [3,3].
• Vw – the velocity in wind axes, [V; 0; 0], [3,1].
• alpha_beta – [α; β] in radians, [2,1].
• alpha_beta_dot – [α'; β'] in rad/s, [2,1]. Reads Fw at the same instant.
• wb – the body rates [p; q; r] in rad/s, [3,1].
• dwb – the body angular accelerations ω' in rad/s², [3,1]. Reads M at the same instant.
• Ab – the body-axes acceleration V_b × ω + DCM_wb^T·F_w/m, with V_b = DCM_wb^T·[V; 0; 0], [3,1]. Reads Fw at the same instant.
• Fuel – the fuel tank status: 1 full (m ≥ m_f), −1 empty (m ≤ m_e), 0 otherwise, [1,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 Position [Xe Ye Ze] – three values. Defaults to [0 0 0].
• Initial Airspeed and Angles [V alpha beta] – the initial airspeed, angle of attack and sideslip (radians), three values. V must be nonzero, since it divides, and β inside (−π/2, π/2). Defaults to [100 0 0]: the Simulink dialog's own default, [0 0 0], divides by zero at the first step.
• Initial Wind Angles [mu gamma chi] – the initial bank, flight-path and heading angles in radians, three values; the initial body quaternion is the half-angle product of the Euler angles of DCM_wb^T·DCM_we. Defaults to [0 0 0].
• Initial Body Rates [p q r] – in rad/s, three values. Defaults to [0 0 0].
• Initial Mass – m₀, from the empty to the full mass. Defaults to 1.
• Empty Mass – m_e, the lower limit, > 0. Defaults to 0.5.
• Full Mass – m_f, the upper limit, greater than the empty mass. Defaults to 2.
• Empty Inertia – I_e, the invertible [3,3] tensor at the empty mass. Defaults to eye(3).
• Full Inertia – I_f, the invertible [3,3] tensor at the full mass. Defaults to 2*eye(3).
• 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 wind-angle read-back rebuilds it from ATAN). A core holds the fourteen states, publishes the eleven outputs from them and the inputs of the same sample, 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 clamped to the tank at every stage and after every substep exactly as the Simulink block's limited integrator does it. The inertia is interpolated and inverted in closed form at every derivative evaluation.

The three HDL targets are simulation-only real arithmetic, quantized at the port: a tangent and a square root of a state have no Q16.16 form. The cores simulate correctly and are not offered as synthesizable.

Simulink bridge

Import and export, mapped to Aerospace Blockset's aerolib6dof2/Simple Variable Mass 6DOF Wind (Quaternion): Units → units (the two offered values 1:1), Initial Position [Xe Ye Ze] → xme_0, Initial Airspeed and Angles [V alpha beta] → Vm_0, Initial Wind Angles [mu gamma chi] → wind_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 and Full Inertia → inertia_f. Always emitted with mtype = Simple Variable, rep = Quaternion and vre_flag and abi_flag off: the mass type and the two flags each move that block's port list, the representation selects a different block, and this block has one port list. The knots unit system is not offered – it converts velocities inside the integration – and an imported one is reported. The dialog's inertia is not read at this mass type and is not offered. "Sampling Time (s)" does not cross: the Simulink block is continuous and defines no SampleTime.

Notes

• Stateful, continuous and nonlinear: fourteen continuous states. Unlike the body-axes equations of motion, the block has direct feedthrough: alpha_beta_dot, dwb and Ab read the inputs of the same instant, so a loop from those outputs back to Fw or M is an algebraic loop.
• V divides and β = ±90° is singular (tan β and 1/cos β): a run that takes the airspeed through zero or the sideslip to ±90° is outside the model, as it is in the Simulink block.
• At a limit the mass stops, but the inertia rate does not: the Simulink block feeds the raw dm/dt to it, and so does this one. On the continuous solver the mass is held by the zero rate alone and can overshoot a limit by one solver step; the discrete solver and every exported core also clamp it, as Simulink does.
• No gimbal lock in the state. The wind-angle OUTPUT is a read-back, so μ and χ wrap at ±π and γ is clamped to the arcsine's domain at ±90°.