{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# PID Control"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Imagine that you prepared some milk to feed your baby, but the milk's temperature is only 25°C and it should be 37°C. The ambient temperature in your room is 20°C and this will cool down your milk. Luckily, you have a device which can heat up your milk . The device uses electricity to produce heat and you can manipulate the power $u$. You are also able to monitor the milk's temperature $T$. What you are facing is a *control problem*.\n",
"\n",
"Before we explore a solution, let us write down some code to simulate the scenario. We will model the temperature dynamics via Newton's law of cooling"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$$\n",
" \\frac{dT}{dt}=\\alpha (T_a - T(t)) + \\beta u(t)\n",
"$$ (milk_ode)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here, $d/dt$ is the time derivative, $T_a$ is the ambient temperature and $u$ the power of the heating device. $\\alpha$ and $\\beta$ are constants. We are not going to try to estimate realistic values for $\\alpha$ and $\\beta$ for our problem. This is just a toy example. Using the equation above, let us write some simulation code. "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"from IPython import display\n",
"display.set_matplotlib_formats('svg')"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"#define constants\n",
"alpha, beta = 1, 40\n",
"T_ambient, T_desired, T_start = 20, 37, 25\n",
"\n",
"# system update by discretization of the differential eq.\n",
"def next_temp(u, T, dt):\n",
" return T+alpha*(T_ambient-T)*dt + beta * u *dt\n",
"\n",
"def simulate_temp(controller, num_steps=20):\n",
" dt = 0.1 # Every time interval dt we set a new control value\n",
" T = T_start\n",
" T_list = [T]\n",
" for k in range(num_steps):\n",
" # ask controller for u value\n",
" u = controller.get_control(T,dt)\n",
" # device only allows to set u between 0 and 1:\n",
" u = np.clip(u, 0, 1)\n",
" # simulate what the temperature will be after time interval dt\n",
" T = next_temp(u, T, dt)\n",
" T_list.append(T)\n",
" time = dt*np.arange(num_steps+1)\n",
" plt.plot(time, np.repeat(T_desired, num_steps+1), ls=\"--\")\n",
" plt.plot(time, T_list)\n",
" plt.xlabel(\"time\"); plt.ylabel(\"Temperature\");"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The `simulate_temp` function needs a `controller` object as an input. This `controller` has a function `get_control()`, which looks at the current temperature `T` and the time `dt` that has elapsed since the last control command was issued. It tells us to which power $u$ we should set our heating device.\n",
"\n",
"First let us create a very silly controller: It will always set $u$ to zero. Hence, we are not heating the milk and it will cool down to the ambient temperature:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class SillyController:\n",
" def get_control(self,T,dt):\n",
" return 0\n",
"silly_controller = SillyController()\n",
"simulate_temp(silly_controller, num_steps=30)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Indeed, if we do nothing, the milk cools down. But now let us try to create a proper controller. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## P Controller\n",
"The idea of the P controller or proportional controller is simple: Imagine that you are turning a knob on the heating device that sets the value of $u$. You look at the difference between the desired temperature and the current temperature, the so-called error. If the error is large and positive (desired temperature > current temperature) you choose $u$ to be large and positive. This will heat up the milk and the error will go down. The more the error goes down, the more you would turn the $u$-knob towards zero. If the error is negative, i.e., the current milk temperature is too high (dangerous for the baby!), you would like to cool the milk down by setting $u$ to a negative value. Sadly, this is something you cannot do with your electrical heater [^foot_note_negative_u]).\n",
"\n",
"[^foot_note_negative_u]: You can imagine other control problems, where the control input $u$ does not need to be positive. For example you might want to control the torque on the wheels of a car in order to park at a specific position. If you have driven too far, you can set a negative torque and drive backwards.\n",
"\n",
"In summary, you choose $u$ to be proportional to the error. We can implement the P controller as follows"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class PController:\n",
" def __init__(self, Kp, set_point):\n",
" self.Kp = Kp\n",
" self.set_point = set_point\n",
" \n",
" def get_control(self, measurement, dt):\n",
" error = self.set_point - measurement\n",
" return self.Kp * error"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The `PController` computes the difference between a `set_point` (desired temperature) and a `measurement` (current temperature). This difference is called the `error`. The control value is just a positive constant `Kp` multiplied with the `error`. The constant `Kp` is known as the *proportional gain*, and finding a good value for `Kp` is called *tuning the controller*. Note that the `PController` knows nothing about heat or Newton's law of cooling. It does not rely on a model of how the control value $u$ influences the world. \n",
"\n",
"The mathematical formula for the P controller is "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$$u(t) = K_p e(t)$$"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here, $e$ denotes the error. Now, let us apply the `PController` and see what happens to the milk:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"p_controller = PController(Kp=0.2, set_point=T_desired)\n",
"simulate_temp(p_controller)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"As you can see, the milk's temperature goes up, but not quite to the value we wanted. There is still a gap between the actual temperature and the desired temperature. This is known as *steady state error* and is a typcial problem with a `PController`. \n",
"\n",
"In a real system it can be hard to understand why there is a *steady state error*, because we might not even have a model of the system. For our simulated milk system however, we can understand it: Assume that the actual temperature is equal to the desired temperature. Then the error `T_desired-T` is zero and hence `u=K_d * (T_desired-T)` is zero. This means no heat is added and the milk cools down below `T==T_desired`. A steady state is reached when the heat generated by the error is equal to the heat that is lost to the room. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now we could increase the `set_point` to \"desired temperature plus a bit more\". This would increase the steady state temperature and make it come closer to our desired temperature. But there exists a much better solution! The proportional integral or PI controller! \n",
"\n",
"## PI Controller\n",
"\n",
"The formula for the PI controller is"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$$u(t) = K_p e(t) + K_I \\int_0^t e(t) dt$$"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The control variable now depends on the time integral of the error. Imagine the situation from before: We have a P controller, which comes close to the desired temperature but there is still a gap. If we now switch to a PI controller, the integral term $K_I \\int_0^t e(t) dt$ grows over time and it will close the gap. The integral term is looking into the past and saying \"Hey, we have been below the `set_point` for quite some while now! We should increase the heat!\". Let us implement the PI controller and see how it solves the steady-state error problem:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class PIController:\n",
" def __init__(self, Kp, Ki, set_point):\n",
" self.Kp = Kp\n",
" self.Ki = Ki\n",
" self.set_point = set_point\n",
" self.int_term = 0\n",
" \n",
" def get_control(self, measurement, dt):\n",
" error = self.set_point - measurement\n",
" self.int_term += error*self.Ki*dt\n",
" return self.Kp * error + self.int_term"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"pi_controller = PIController(Kp=0.2, Ki = 0.15, set_point=T_desired)\n",
"simulate_temp(pi_controller)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We are pretty happy with our milk now. For this problem, the PI controller is a nice solution. There are other problems however, where it might not be enough to look into the past with the integral term. We also need to look into the future. This is what the derivative term in the PID controller does. \n",
"\n",
"## PID Controller\n",
"The formula of the PID controller is"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$$u(t) = K_p e(t) + K_I \\int_0^t e(t) dt + K_d \\frac{de}{dt}$$"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"and the implementation looks like this"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"class PIDController:\n",
" def __init__(self, Kp, Ki, Kd, set_point):\n",
" self.Kp = Kp\n",
" self.Ki = Ki\n",
" self.Kd = Kd\n",
" self.set_point = set_point\n",
" self.int_term = 0\n",
" self.derivative_term = 0\n",
" self.last_error = None\n",
" \n",
" def get_control(self, measurement, dt):\n",
" error = self.set_point - measurement\n",
" self.int_term += error*self.Ki*dt\n",
" if self.last_error is not None:\n",
" self.derivative_term = (error-self.last_error)/dt*self.Kd\n",
" self.last_error = error\n",
" return self.Kp * error + self.int_term + self.derivative_term"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"For our milk problem, we do not gain much using the PID controller. Below you will find an interactive simulation of the milk system, where you can play with the parameters $K_p, K_i, K_d$ using sliders. You should observe that the derivative term is actually not helping much here."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"hide-input"
]
},
"outputs": [],
"source": [
"# Import JavaScript simulation from another github repo\n",
"# JavaScript code was manually generated based on the python code from this chapter\n",
"from IPython.display import IFrame\n",
"IFrame('https://thomasfermi.github.io/PID-Playground-Temperature/', width=700, height=600)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"# create sliders for interactive simulation via python\n",
"# Does not work inside Jupyter Book; only in notebook environment\n",
"from ipywidgets import interact, FloatSlider\n",
"import ipywidgets as widgets\n",
"def run_pid_temp(Kp,Ki,Kd):\n",
" pid_controller = PIDController(Kp, Ki, Kd, set_point=T_desired)\n",
" simulate_temp(pid_controller)\n",
"\n",
"Kp_slider = FloatSlider(min=0.0, max=0.8, step=0.05, readout_format='.4f')\n",
"Ki_slider = FloatSlider(min=0.0, max=0.2, step=0.01, readout_format='.4f')\n",
"Kd_slider = FloatSlider(min=-3e-3, max=3e-3, step=2e-4, readout_format='.4f')\n",
"interact(run_pid_temp, Kp=Kp_slider, Ki=Ki_slider, Kd=Kd_slider);"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"There are problems however, where the derivative term of the PID controller is very important. Imagine a drone flying at height $p$ above the ground. We want it to stay at a desired height of $p=p_d=50$ meters. We can control the drone's upwards acceleration $a$ (hence $u=a$) and have to take into account that there is a constant downwards acceleration $g$ due to gravity.\n",
"Let us have a look at a toy simulation of the drone. The state of the simulation consists of the drones position and velocity. The controller only measures the position and issues an acceleration command. Note that you do not need to understand the details of the simulation code"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"g = -9.81\n",
"p_desired = 50\n",
"\n",
"# system update\n",
"def next_state(a, state, dt):\n",
" aeff = a + g\n",
" position, velocity = state \n",
" # integration by velocity verlet algorithm\n",
" position += velocity*dt + 0.5*aeff*dt**2\n",
" velocity += aeff*dt\n",
" return (position,velocity)\n",
"\n",
"def simulate_drone(controller, num_steps = 1000):\n",
" dt = 0.02\n",
" state = (0,0)\n",
" state_list=[state]\n",
" for i in range(num_steps):\n",
" a = controller.get_control(state[0], dt)\n",
" # upwards acceleration is positive (>0)\n",
" # and limited by power supply (<100)\n",
" a = np.clip(a, 0, 100)\n",
" state = next_state(a, state, dt)\n",
" state_list.append(state)\n",
" s = np.array(state_list)\n",
" time = dt*np.arange(num_steps+1)\n",
" plt.plot(time, [p_desired]*(num_steps+1), ls=\"--\")\n",
" plt.plot(time, s[:,0])\n",
" plt.gca().set_ylim(-100,100)\n",
" plt.xlabel(\"time\"); plt.ylabel(\"drone height\");"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Using the interactive simulation below, try to find values of $K_p, K_i, K_d$ that make the drone hover at 50 meters. You will need to use all three terms of the PID controller here."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"hide-input"
]
},
"outputs": [],
"source": [
"# Import JavaScript simulation from another github repo\n",
"# JavaScript code was manually generated based on the python code from this chapter\n",
"from IPython.display import IFrame\n",
"IFrame('https://thomasfermi.github.io/PID-Playground-Drone/', width=700, height=600)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"# create sliders for interactive simulation via python\n",
"# Does not work inside Jupyter Book; only in notebook environment\n",
"def run_pid_drone(Kp,Ki,Kd):\n",
" pid_controller = PIDController(Kp, Ki, Kd, set_point=p_desired)\n",
" simulate_drone(pid_controller)\n",
"\n",
"Kp_slider = FloatSlider(min=0, max=10, step=0.1, readout_format='.4f')\n",
"Ki_slider = FloatSlider(min=0.0, max=0.5, step=0.05, readout_format='.4f')\n",
"Kd_slider = FloatSlider(min=-3, max=3, step=0.1, readout_format='.4f')\n",
"interact(run_pid_drone, Kp=Kp_slider, Ki=Ki_slider, Kd=Kd_slider);"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You should find parameters where the control works smoothly. When you do not use the derivative term you will find that the drone oscillates around the target position. But why? If the drone is too low, the controller will make it increase its upwards position such that it can reach the target position. But once it reaches that position, it still has some upwards momentum and will hence continue to move upwards. This overshoot due to inertia can be tackled by the derivative term, which is"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$$u(t) = K_d \\frac{de}{dt} = K_d \\frac{d}{dt}\\bigg(p_d-p(t)\\bigg) = K_d \\left(0-\\frac{dp}{dt}\\right) = -K_d \\frac{dp}{dt}$$"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Assuming $K_d>0$ the formula above reveals: If the velocity $dp/dt$ is positive (upwards), the control $u$, which is the acceleration $a$, will be negative (downwards). Hence the derivative term acts against the problem of overshoot and oscillation.\n",
"\n",
"Within this section we always tuned the PID parameters $K_p, K_i, K_d$ manually. For the application within this book this is good enough. But if you want to learn about more methodical and even automatic tuning methods, check out the [Wikipedia Article on PID](https://en.wikipedia.org/wiki/PID_controller)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Before we finish our introduction to PID, we shortly mention its limitations. One of its advantages is also a disadvantage: its independence on a system model. PID solely reacts on the error. If you have no model, this is great, since you might be able to control something that you do not understand in detail. However, if you do have a model, it will not improve the capability of your PID controller. If you are not happy with the PID controller's performance, a model-based controller could be a better choice in this case. \n",
"There is also one problem with PID called *integral windup* . If the set point is far from the current value, the integral term can accumulate and get really large while the system moves towards the set point. This will lead to an overshoot. Luckily, this problem can be solved by slightly adjusting the implementation of the `PIDController`. For example, you could clip the integral term, which means not allowing it to grow beyond some user-specified threshold."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"```{tip}\n",
"This section gave you a gentle introduction to PID control using examples. For more details on PID, see the [Wikipedia Article](https://en.wikipedia.org/wiki/PID_controller) or this [youtube playlist by Brian Douglas](https://www.youtube.com/playlist?list=PLn8PRpmsu08pQBgjxYFXSsODEF3Jqmm-y). If you are interested in control theory in general, I can recommend [Steve Brunton's Control bootcamp on youtube](https://www.youtube.com/playlist?list=PLMrJAkhIeNNR20Mz-VpzgfQs5zrYi085m)\n",
"```"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Enough about baby milk and drones. You want to control a vehicle in the Carla simulator! We can use PID for the longitudinal control of the vehicle, i.e., to set the gas pedal properly. PID is not so well suited for lateral control, i.e., controlling the steering wheel. For that you will implement a method called pure pursuit. Once you understand what pure pursuit is, you will apply PID and pure pursuit inside Carla."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": []
}
],
"metadata": {
"celltoolbar": "Edit Metadata",
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.7.9"
}
},
"nbformat": 4,
"nbformat_minor": 4
}