Inverted pendulum simulation
Free cart
Equations of motion
$$
\left\{
\begin{array}{ll}
L\ddot\Theta = g sin\Theta + \ddot xcos\Theta \\
(m+M)\ddot x + m\ddot \Theta L cos\Theta - mL\dot \Theta^2sin\Theta = 0
\end{array}
\right.
$$
Numerical equations, Heun's method
$$
\left\{
\begin{array}{ll}
\tilde \Theta _{i+1} = \Theta _{i+1} + hZ_i \\
\tilde Z _{i+1} = Z_i + h\frac{g}{l}sin\Theta_i \\
\Theta _{i+1} = \Theta _{i+1} + \frac{h}{2}(Z_i + \tilde Z _{i+1}) \\
Z _{i+1} = Z_i + \frac{h}{2}\frac{g}{l}(sin\Theta_i + sin\tilde \Theta _{i+1}) \\
\end{array}
\right.
$$
$$
\left\{
\begin{array}{ll}
\tilde x_{i+1} = x_i + hY_i \\
\tilde Y_{i+1} = Y_i - h\beta sin\Theta_i(gcos\Theta_i - LZ^2_i) \\
x_{i+1} = x_i + \frac{h}{2}(Y_i + \tilde Y_{i+1}) \\
Y_{i+1} = Y_i - \frac{h}{2}\beta(sin\Theta_i(gcos\Theta_i - LZ^2_i) + sin\tilde \Theta_i(gcos\tilde \Theta_i - L\tilde Z^2_i))
\end{array}
\right.
$$
Where \(\beta = \frac{m}{m+M}\)