Partnership for Integration of Computation into Undergraduate Physics

## 1. Visualizing the system Before we can simulate the PID controller, we need to understand the forces acting on the drone. To do this, we will begin by drawing a **free body diagram** to visualize these forces. ### 1 a) Free Body Diagram Sketch the system and draw a free body diagram of a drone hovering at a target height $h$. Since we are working in one dimension (vertical), horizontal forces can be ignored. The drone is affected by two main forces: gravity, pulling it downward, and thrust, pushing it upward. The thrust force is controlled via the drone’s "gas pedal", which adjusts how much upward force is applied to counteract gravity. Draw one free body diagram for a light drone, and one for a heavy drone. What is the difference between the two? ### 1b) Change of forces The free body diagrams above describe the drone when it is hovering steadily at the target height $h$, meaning there is a balance between the forces. But what happens when the drone is not at the target height? Are the forces still the same? ## 2. The P Controller We will start by implementing the proportional (P) component of the PID controller. A proportional controller adjusts its output based on how far the drone is from the target height. Meaning, the larger the difference, the stronger the response. In our case, the controller output is the thrust force applied to the drone, which we write as a function of time $u(t)$. The error is the difference between the target height and the current height of the drone: $$ u(t) = K_P e(t) $$ where $K_P$ is the proportional gain, $e(t)$ is the error at time $t$, and $u(t)$ is the resulting thrust force. A larger error means more thrust. In this section, we will see how a proportional controller alone affects the drone's motion, and why it is often not enough on its own. ### 2 a) Finding the acceleration To simulate the drone's motion, we need to calculate the vertical acceleration. This is done by finding the net force acting on the drone, which is the difference between the thrust force and the gravitational force. The gravitational force is constant and can be represented as $F_g = m \cdot g$, where $m$ is the mass of the drone and $g$ is the acceleration due to gravity (approximately 9.81 m/s²). Using Newton's second law $F=ma$, express the drone's acceleration $a(t)$ as a function of the thrust force $u(t)$ and the gravitational force $F_g$. ### 2 b) Implementing the acceleration function Using the expression for acceleration, implement the function `acceleration(thrust, m, g)` that calculates the vertical acceleration of the drone. ``` import numpy as np import matplotlib.pyplot as plt def acceleration(thrust, m, g): """ Calculates the net vertical acceleration of the drone. """ return ... ``` ### 2 c) The proportional component Now we have implemented a function for acceleration. But as you can see, the acceleration depends on the thrust force $u(t)$, which we have not yet defined. In this subsection, we want to implement two more functions: 1. `proportional_component(K_P, error)` that calculates the thrust force $u(t)$ based on the error $e(t)$ and the proportional gain $K_P$. 2. `error(target_height, current_height)` that calculates the error $e(t)$ as the difference between the target height and the current height of the drone. These are the last two pieces we need to implement the proportional component of the controller. ``` def calculate_error(target_height, current_height): """ Calculates the difference between the target height and the current height of the drone. """ return ... def proportional_component(K_P, error): return ... ``` ### 2 d) Simulate the P controller Now that we have all the pieces, we are ready to simulate the movement of the drone using the proportional controller. In the following code section, you will implement the Euler-Cromer method to numerically integrate the velocity and position of the drone. The Euler-Cromer method is an extension of the Euler method and is significantly more stable and reliable for simulating physical systems when the acceleration is known. The Euler-Cromer equations for this system are: $$ \begin{align*} v(t + \Delta t) &= v(t) + a(t) \Delta t\\ r(t + \Delta t) &= r(t) + v(t + \Delta t) \Delta t \end{align*} $$ where $v(t)$ is the velocity at time $t$, $r(t)$ is the position at time $t$, and $\Delta t$ is the time step. ``` ## initialize the system m = 1.0 # mass target_height = ... # desired position E.g 10.0 start_position = 0.0 # initial position start_velocity = 0.0 # initial velocity g = 9.81 # gravitational acceleration K_P = ... # proportional gain n = 500 T = np.linspace(0, 20, n) # time vector dt = T[1] - T[0] # time step position = np.zeros(n) # position velocity = np.zeros(n) # velocity accelerations = np.zeros(n) # acceleration position[0] = start_position # set initial position velocity[0] = start_velocity # set initial velocity # Euler cromer method for numerical integration for i in range(1, n): # Calculate the error between the target and the current position # Calculate the control input using the proportional term function # Calculate the acceleration # Update velocity and position using Euler-Cromer method ... # plot the results plt.figure(figsize=(10, 5)) plt.plot(T, position, label='Position', color='blue') plt.axhline(target_height, color='red', linestyle='--', label=f'Setpoint({target_height} m)') plt.title('P controller: Position vs Time') plt.xlabel('Time (s)') plt.ylabel('Position (m)') plt.legend() plt.grid() plt.show() ``` ## 3) PD controller We have just witnessed the limitations of a P controller. It only reacts to the current error, meaning the difference between the target height and the measurement position, without considering how the error is changing over time. This often leads to overshooting and oscillations in the system. To improve the controller, we introduce a derivative component, D, which responds to the rate of change of the error. This allows the controller to anticipate where the system is heading, not just where it currently is. In other words, the derivative component acts as anticipatory control, pushing back harder when the error is changing rapidly and backing off when things are stabilizing. It effectively adds damping, which helps smooth out the response and prevent overshooting. Mathematically, the controller signal is now proportional to both the error and its derivative: $$u(t) = K_p e(t) + K_D \frac{\partial e(t)}{\partial t}$$ ### 3 a) Understanding the derivative component Before we implement the derivative component, let us first try to understand how it affects the system. The derivative component is the rate of change of the error, which can be approximated by: $$\frac{\partial e(t)}{\partial t} \approx \frac{e(t) - e(t - \Delta t)}{\Delta t}$$ where $\Delta t$ is the time step, $e(t)$ is the error at time $t$, and $e(t - \Delta t)$ is the error at the previous time step. Now consider these situations: 1. What happens to the thrust force $u(t)$ when the drone is **below** the target height, but rising quickly? 2. What happens to the thrust force $u(t)$ when the drone is **above** the target height, and falling back towards it? 3. What happens with the thrust force $u(t)$ when the drone is at a **stable** height (not necessarily the target height)? *Hint: Think about the sign of the error and the sign of its derivative in each case* ### 3 b) Implementing the derivative component The next thing we need to do is to implement the derivative of the error into the simulation. A simple way of calculating the derivative is to use the difference between the current and previous error: $$\frac{\partial e(t)}{\partial t} = \frac{e(t) - e(t-1)}{\Delta t}$$ Fill in the function `derivative_component(K_D, error, previous_error, dt)` below. ``` def derivative_component(K_D, current_error, previous_error, dt): """ Derivative term for a PID controller: K_D · (d e(t))/(d t). """ return ... ``` ### 3 c) Simulating the PD controller Now that you have implemented the derivative component, you are ready to simulate the movement of the drone using the PD controller. Use the Euler Cromer method to numerically integrate the velocity and position of the drone. The PD controller will use the proportional and derivative components to calculate the thrust force. Also plot the result and observe how it changes compared to the P controller. ``` K_P = ... # proportional gain K_D = ... # derivative gain position = np.zeros(n) # position velocity = np.zeros(n) # velocity accelerations = np.zeros(n) # acceleration position[0] = start_position # set initial position velocity[0] = start_velocity # set initial velocity #Euler-Cromer method for numerical integration with PD control previous_error = ... # initialize previous error for i in range(1, n): # calculate error # Calculate the control input using the proportional term and the derivative term functions # Update velocity and position using Euler-Cromer method # update previous error for next iteration ... ``` Notice how the derivative term stabilizes the drone. ## 4) PID controller We have now seen the effects of the P and D terms in the controller. While the derivative term helps by anticipating changes in error and damping the system response, the PD controller is still not perfect—particularly when it comes to eliminating steady-state errors, where the drone hovers at a height that is not exactly the desired height To address this problem, we will introduce the integral term, I. The integral term accumulates the error over time, effectively "remembering" past errors. If there is any persistent difference between the target and the measurement, the integral term will continue to grow, pushing the controller to adjust the output until the error is eliminated. This cumulative effect is what allows the controller to eliminate residual or static errors that the proportional term alone cannot handle. As the error decreases, the contribution from the proportional term naturally weakens, but the integral term, having built up over time, continues to drive the output until the error is zero. Once the error reaches zero, the integral term stops growing, stabilizing the system. Together, the three terms combine into a PID controller, which is defined as follows: $$u(t) = K_P e(t) + K_I \int_0^t e(t) dt + K_D \frac{\partial e(t)}{\partial t} $$ ### 4 a) Implementing the integral component We want to implement the integral of the error into the simulation by using a simple numerical integration method, such as a Riemann sum: $$\int_0^t e(t) dt \approx \sum_{i=0}^{n} e(t_i) \Delta t$$ The Riemann sum will be accumulated in a variable, which needs to be initialized as zero, and then updated in each iteration of the simulation loop. Fill in the function `integration_component(K_I, Riemann_sum)` below. ``` def integration_component(K_I, Riemann_sum): """ Integral term for a PID controller: K_I · Sum( e(t)·dt) """ return ... ``` ### 4 b) Simulating the PID controller Now that you have implemented all the necessary functions for the PID controller, you should be able to simulate the drone’s movement using the PID controller combined with the Euler Cromer method. ``` # Initialize the system K_P = ... # proportional gain K_D = ... # derivative gain K_I = ... # integral gain position = np.zeros(n) # position velocity = np.zeros(n) # velocity accelerations = np.zeros(n) # acceleration position[0] = start_position # set initial position velocity[0] = start_velocity # set initial velocity ``` Hopefully, you have now seen a significant improvement in the drone’s stability. Your PID controller should be able to keep the drone steady at the target height. Notice how the integral term eliminates steady-state errors, which was apparent in the PD controller. Now, try adjusting the gains $K_P$, $K_I$, and $K_D$ to see how they affect the system’s response. You can also experiment with changing the target height and observe how the controller reacts to different desired altitudes. ## 5. Add random effects In the real world, there are often random effects that can affect the stability of the drone. For example, wind gusts can push the drone up or down, and we need to adjust the thrust accordingly. In this exercise, we will add a random effect to the simulation to see how the PID controller reacts to it. We have created a few example functions to simulate the effects listed below. Try implementing them in your simulation to see how the PID controller reacts. You can also create your own random effects and observe the controller’s response. ### Examples: - Wind gusts - Air resistance - Moving target position ### 5 a) Free body diagram with random effects Before implementing the effects, first draw a free body diagram of the drone including the random effects you want to explore. How do these effects change the forces acting on the drone, and how do they affect the thrust force that the PID controller needs to apply? ### 5 b) Implementing random effects Implement the random effects you want to explore in the simulation. You can use the example functions provided or create your own. Make sure to adjust the forces acting on the drone accordingly. ``` def drag_force(v, drag_coeff): """ Calculates the drag force acting on the drone. """ return -drag_coeff * v def wind_force(wind_amplitude): """ Randomly generates a wind """ return np.random.uniform(-wind_amplitude, wind_amplitude) ## Moving setpoint def moving_target(t): """ Example function of a moving target that changes over time. """ return 10.0 + 5.0 * np.sin(1.0 * t) * np.exp(-0.09 * t) #plot the moving setpoint moving_targets = moving_target(T) plt.figure(figsize=(10, 5)) plt.plot(T, moving_targets, label='Moving Setpoint', color='green') plt.title('Moving Setpoint vs Time') plt.xlabel('Time (s)') plt.ylabel('Setpoint (m)') plt.legend() plt.grid() plt.show() ```