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

EOM 6DOF Custom Variable Mass Wind Quaternion

Robotics / Equations Of Motion

The six-degree-of-freedom equations of motion of a rigid body whose mass, inertia tensor and inertia rate arrive on wires, 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], and m, dI/dt and I read off the inputs every sample:

• V' = F_x/m
• α' = q − (p·cos α + r·sin α)·tan β + F_z/(m·V·cos β)
• β' = p·sin α − r·cos α + F_y/(m·V)
• ω' = 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 – the mass, a scalar [1,1] > 0.
• I_dot – the rate of change of the inertia tensor, [3,3].
• I – the inertia tensor, an invertible [3,3]. The block does not check that I_dot is its derivative.
• 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 and m 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, I_dot and I 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.

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].
• 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 thirteen states, publishes the ten 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 inertia is 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/Custom 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. Always emitted with mtype = Custom 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. mass_0, inertia and the empty/full values are not read at this mass type and are not offered. "Sampling Time (s)" does not cross: the Simulink block is continuous and defines no SampleTime.

Notes

• Stateful, continuous and nonlinear: thirteen 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.
• 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°.