This is a short post on how to implement the equations of a quadcopter that were shown in the last post and are based on [1]. I strongly recommend reading that post since I explained the notation and the state space equations in depth there and thus I gloss over them here to focus on the new material. The github repo where the code for this post can be found is here.
The most common way of simulating a system is using some form of Runge-Kutta method [2]. These typically fall into one of two buckets: varying step size or fixed step size. The fixed step size will choose a constant value for \(\Delta t\) and perform the integration for however many time steps are required. On the other hand, the varying step size approach will attempt to keep the error between approximations of the ODE, such as between 4th and 5th order, below a user defined threshold. If the error is found to be under the threshold then the algorithm will try to increase the step size and check to see if it keeps the solution below the error threshold. A common method for this is the Dormand-Prince method. See here if you want to learn more about the varying step size approaches.
The rest of this post will walk through the equations for simulating the quadcopter using a fixed step size method and provide an implementation. Then it will go onto doing the same thing with a method that uses a varying step size approach and uses the scipy solve_ivp() function to perform the forward integration given the model \(\dot X\).
Fixed Step Size
Fixed step size Runge-Kutta is easy to implement once we have the model so let’s implement that ourselves since we’ll need it in a later post that discusses control. From the last post we saw that our state \(X\) is, $$X = \begin{bmatrix} x \\ y \\ z\\ \dot x \\ \dot y \\ \dot z \\ \phi \\ \theta \\ \psi \\ \dot \phi \\ \dot \theta \\ \dot \psi \end{bmatrix} $$ which caused our state space ODE to be, $$\dot X = \begin{bmatrix} \dot x \\ \dot y \\ \dot z \\ \ddot x \\ \ddot y \\ \ddot z \\ \dot \phi \\ \dot \theta \\ \dot \psi \\ \ddot \phi \\ \ddot \theta \\ \ddot \psi \end{bmatrix}$$ Using the Newton-Euler equations as talked about in the last equation, we were able to solve for \(\dot X\) in our last post to be, $$\dot X = F(X, u) = \begin{bmatrix} \dot x \\ \dot y \\ \dot z \\ \dot \phi \\ \dot \theta \\ \dot \psi \\ \frac{f_t(C_\psi S_\theta C_\phi+ S_\psi S_\phi)}{m} \\ \frac{f_t(S_\psi S_\theta C_\phi - C_\psi S_\phi)}{m} \\ \frac{f_t C_\theta C_\phi}{m} - g \\ \tiny{q(\dot \phi C_\phi T_\theta + \frac{\dot \theta S_\phi}{C_\theta^2}) + r(-\dot \phi S_\phi C_\theta + \frac{\dot \theta C_\phi}{C_\theta^2}) + \frac{(I_y - I_z)}{I_x}q r + \frac{\tau_\phi}{I_x} + (\frac{(I_z - I_x)}{I_y}p r + \frac{\tau_\theta}{I_y})S_\phi T_\theta + (\frac{(I_x - I_y)}{I_z}p q + \frac{\tau_\psi}{I_z}) C_\phi T_\theta} \\ \tiny{-q \dot \phi S_\phi - \dot \phi C_\phi r + (\frac{(I_z - I_x)}{I_y}p r + \frac{\tau_\theta}{I_y}) C_\phi - (\frac{(I_z - I_x)}{I_y}p r + \frac{\tau_\theta}{I_y}) S_\phi} \\ \tiny{q(\frac{\dot \phi C_\phi}{C_\theta} + \frac{\dot \phi S_\phi T_\theta}{C_\theta}) + r(\frac{-\dot \phi S_\theta}{C_\theta} + \frac{\dot \theta C_\phi T_\theta}{C_\theta}) + \frac{(\frac{(I_z - I_x)}{I_y}p r + \frac{\tau_\theta}{I_y})S_\phi}{C_\theta} + \frac{(\frac{(I_x - I_y)}{I_z}p q + \frac{\tau_\psi}{I_z}) C_\phi}{C_\theta}} \end{bmatrix} \tag{1}$$ where \(C_x\) denotes \(\cos(x)\), \(S_x\) denotes \(\sin(x)\), \(T_x\) denotes \(\tan(x)\), \(I_x\) denotes the moment of inertia about the x-axis, \(I_y\) denotes the moment of inertia about the y-axis, and \(I_z\) denotes the moment of inertia about the z-axis, \(g = 9.81 m/s^2\) denotes gravity. Note that \(\dot X\) depends on our control input, $$ u = \begin{bmatrix}f_t \\ \tau_\phi \\ \tau_\theta \\ \tau_\psi \end{bmatrix} $$ where \(f_t\) is the total thrust, \(\tau_\phi\) denotes the input torque about the x-axis in the body frame,\(\tau_\theta\) denotes the input torque about the y-axis in the body frame, and \(\tau_\psi\) denotes the input torque about the z-axis in the body frame.
If these equations look strange or you'd like a bit more explanation of the notation, see the previous post. It also includes some intuition on why the equations are reasonable and some slightly simpler ways of writing them that requires doing some matrix multiplications to build them up.
In our case the fourth order Runge-Kutta equations are, $$ X_{n+1} = X_n + \frac{1}{6} \Delta t (k_1 + 2 k_2 + 2 k_3 + k_4) \tag{2} $$ where, $$ k_1 = F(X, u_n) $$ $$ k_2 = F(X + \Delta t \frac{k_1}{2} , u_n) $$ $$ k_3 = F(X + \Delta t \frac{k_2}{2} , u_n) $$ $$ k_4 = F(X + \Delta t k_3 , u_n) $$
where \(u_n\) denotes the control input at time \(n\). Note that these equations assume zero order hold [3]. In short this means that the system’s control input is assumed to be constant during each time step and will change instantly at each time step. This assumption is valid in our case but if you want to dig deeper into it take a look at [3] and see the following video by Brian Douglas.
Here is some pseudo code for the implementation, The runge_kutta() function takes as input the current state \(X\), the initial time \(t_0\), the final time \(t_f\), the time step dt, and a matrix \(u\) where each row is the control input at time step \(t = t_0 + dt*row\). Using this it returns a matrix of the entire trajectory where each row is the state \(X\) at time \(t = t_0 + dt*row\).
If you try to copy and paste this you'll notice that you'll get an error about a "missing function quad_model" since there is a call to a quad_model() function which I didn't include in the gist. You can find it in the repo or by following the link at the top of the gist.
Varying Step Size
The implementation of Runge-Kutta with a varying step size requires a bit more care but thanks to the scipy ecosystem we don’t have to implement it ourselves. Instead we can use scipy's solve_ivp() function which provides an implementation of a Runge-Kutta 45 method commonly known as Dormand-Prince. Here are the main parts of the code, This code relies on some of the code in the repo so copy and pasting it will give you an error related to a "missing function get_epsilon_dot_dot." The overall structure is straightforward: we create a quad_model function that solve_ivp can use then we pass quad_model, the initial time, the final time, the initial state, and the control inputs to solve_ivp. Note that we don't pass in a step \(\Delta t\) since it uses a varying step size approach. Internally solve_ivp is calling quad model and seeing the error between the fourth and fifth order approximations and varying the step size accordingly. By varying the step size and checking the error, varying step size solvers are (usually) able to integrate ODEs faster than fixed step size solvers.
Conclusion
Hopefully you now have a good idea for how simulators like JMavSim and Gazebo are able to simulate a quadcopter. There are always optimizations that can be done for accuracy, speed, or usability but this should give you a solid foundation to build upon if you want to dig into their respective code bases or do your own implementation. It’s also important to realize that the ideas in this post apply not only to quadcopters but to any system for which you have a state space equation.
In the next post I’ll be digging into state estimation in order to figure out how exactly we can measure all of the variables inside our state X with sensors onboard the quadcopter. See you there.
References
[1] Teppo Luukkonen. Modelling and control of quadcopter. Independent research project in applied mathematics, 2011. https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
[2] https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods