Figure 5: Simulink diagram of the forward dynamics