function dq = exampleHelperStateDerivative(L1, L2, M, q, u)
%exampleHelperStateDerivative Calculate derivative for truck-trailer system
%%
% 
% The vehicle kinematics are an extension of the typical bicycle model. Begin 
% with a simple 1-body, 2-axle system (Figure 2). The system's state can be expressed 
% by the location of the rear-axle's center $\left(x,y\right)$ and the orientation 
% of the centerline $\left(\theta \right)\;$ with respect to the world frame. The 
% system evolves over time based on the steering angle $\left(\alpha \;\right)$ of 
% the virtual steering wheel, located a fixed distance $\left(L\right)$ from the 
% rear-axle, and a velocity $\left(V\right)$:
% 
% <<../helperImages/xxTruckTrailerSystem.png>>
% 
% *Figure 1:* _Simplified kinematic model of a front-steered two-axle vehicle 
% with no-slip assumptions_
% 
% <http://liu.diva-portal.org/smash/get/diva2:826978/FULLTEXT02.pdf _Section 
% 2.3: Constrained motion_>
% 
% For this system, we assume that there is no lateral slippage, $v_{\textrm{ort}} 
% =0$, so all motion is directed along the vehicle's center axis, $v=V$:
% 
% $$\begin{array}{l}\dot{X} =v\;\cos \left(\theta \;\right)\\\dot{Y} =v\;\sin 
% \left(\theta \;\right)\end{array}$$
% 
% The same no-slip assumption can be applied to the virtual steering wheel, 
% which produces a geometric relationship between rear and front axles:
% 
% $$R=\frac{L}{\tan \left(\alpha \right)}$$ 
% 
% For a given $\alpha \;$, the vehicle will follow an arc with a fixed radius, 
% and for small distances along this arc, $\left(\delta s\right)$, there exists 
% the relation $\delta s=R\;\delta \theta$. Combining this with the geometric 
% relation above, we can describe the evolution of $\theta$ and complete our equations 
% of motion:
% 
% $$\dot{\;\theta \;} =\frac{v}{L}\tan \left(\alpha \right)$$
% 
% To extend this to a two-body system, we add an additional link to our kinematic 
% chain. The first body in our chain is fully described by the model above. Our 
% second body can be described by a duplicate set of equations:
% 
% $$\dot{{\;X}_2 } =v_2 \;\cos \left(\theta_2 \right)$$
% 
% $$\dot{{\;Y}_2 } =v_2 \;\sin \left(\theta_2 \right)$$
% 
% Note here that our two bodies are rigidly connected with a revolute joint, 
% so any translation that occurs at body 1's hitch point must match the translation 
% experienced by body 2's hitch point:
% 
% <<../helperImages/xxNTrailer.png>>
% 
% *Figure 2:* _Model of N-trailer system_
% 
% <http://liu.diva-portal.org/smash/get/diva2:826978/FULLTEXT02.pdf _Section 
% 4.1: Recursive formula based on kinematic constraints_>
% 
% This constraint results in the following relationship between $x,y,\dot{\;x} 
% ,\dot{\;y}$:
% 
% $$\begin{array}{l}x_1 -x_2 =L_{2\;} \cos \left(\theta {\;}_2 \right)+M\;\cos 
% \left(\theta \right)\\y_1 -y_2 =L_{2\;} \sin \left(\theta {\;}_2 \right)+M\;\sin 
% \left(\theta \right)\end{array}$$
% 
% $$\begin{array}{l}{\dot{\;x} }_2 -{\dot{\;x} }_1 =L_{2\;} \sin \left(\theta 
% {\;}_2 \right)\dot{\;\theta_2 } +M\;\sin \left(\theta \right)\dot{\;\theta } 
% \\{\dot{\;y} }_1 -{\dot{\;y} }_2 =L_{2\;} \cos \left(\theta {\;}_2 \right)\dot{\;\theta_2 
% } +M\;\cos \left(\theta \right)\dot{\;\theta } \end{array}$$
% 
% Further substitutions provide a definition for $\dot{\;\theta_{2\;} }$ and 
% $v_2$:
% 
% $$\dot{\;{\;\theta }_2 \;} =\frac{v_1 }{L_2 }\sin \left(\beta \right)-\frac{M\;}{L_2 
% }\cos \left(\beta \right)\dot{\theta}$$
% 
% $$v_2 =v\;\cos \left(\beta \right)+M\;\sin \left(\beta \right)\dot{\theta}$$
% 
% $$\beta \;=\theta -\theta_2$$
% 
% $$\dot{\beta \;} \;=\dot{\theta} -\dot{\theta_2 }$$
% 
% where $\beta$ corresponds to the difference in orientation, or the "interior 
% angle", between the two links. For this purposes of this example, we append 
% the state vector with a direction flag, $v_{\textrm{sign}}$, and a property 
% which stores the total number of steps traveled from the start configuration, 
% $n_{\textrm{tot}}$, giving us the following state notation:
% 
% $$q=\left\lbrack x_2 \;y_2 \;\theta_2 \;\beta \;v_{\textrm{sign}} \;n_{\textrm{tot}} 
% \right\rbrack$$

% Copyright 2021 The MathWorks Inc.

    v1 = u(1); alpha = u(2);
    theta2 = q(3); beta = q(4);
    
    theta1 = theta2 + beta;
    dx1dt = v1*cos(theta1);
    dy1dt = v1*sin(theta1);
    dth1dt = v1/L1 * tan(alpha);
    dth2dt = v1 * sin(beta) / L2 - M * cos(beta)*dth1dt/L2;
    dx2dt = dx1dt + M * sin(theta1) * dth1dt + L2 * sin(theta2) * dth2dt;
    dy2dt = dy1dt - M * cos(theta1) * dth1dt - L2 * cos(theta2) * dth2dt;
    dbedt = dth1dt - dth2dt;
    ds = abs(u(1));
    dVSign = 0;
    dq = [dx2dt dy2dt dth2dt dbedt dVSign ds];
end