(Q2.3) Determine a triple of PID parameters so that the closed-loop system behaves “nicely,” i.e., it converges fast to the set point without a constant bias and does not exhibit oscillations. Using the triple of Kp, Kd and Ki you chose, produce two plots: (i) u(t) vs time, (ii) the (x, y) trajectory of the system, for t ∈ [0, 50]. Discuss brief…

lanekeepingQ2part3.py

import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt


class Car:

    def __init__(self, length=2.3, velocity=5., disturbance=0, x=0., y=0., pose=0.):
        """
        :param length: The length between the two axles of the car
        :param velocity: The velocity of the car (m/s)
        :param disturbance: The additive disturbance (rad)
        :param x: The x-position of the car (m)
        :param y: The y-position of the car (m)
        :param pose: The angle of the car from the y-setpoint (rad)
        """
        self.__length = length
        self.__velocity = velocity
        self.__disturbance = disturbance
        self.__x = x
        self.__y = y
        self.__pose = pose

    # Simulate the motion of the car from t = 0 to t = 0 + dt.
    def move(self, steering_angle, dt):
        """
        :param steering_angle: The steering angle of the car (rad)
        :param dt: dt is a time that is added to 0 s to produce the final time of the simulation (s)
        :return:
        """

        # Define the system dynamics as a function for equations 3.11a-c
        def bicycle_model(_t, z):
            x= z[0]
            y= z[1]
            theta = z[2]
            return [self.__velocity*np.cos(theta),
                    self.__velocity*np.sin(theta),
                    self.__velocity*np.tan(steering_angle+self.__disturbance)
                    /self.__length]

        z_initial = [self.__x, self.__y, self.__pose]   # Starting from z_initial = [self.x, self.y, self.pose]

        solution = solve_ivp(bicycle_model,
                             [0, dt],
                             z_initial)

        self.__x = solution.y[0,-1]
        self.__y = solution.y[1,-1]
        self.__pose = solution.y[2,-1]

    def x(self):
        return self.__x

    def y(self):
        return self.__y

    def theta(self):
        return self.__pose


class PidController:

    def __init__(self, kp, kd, ki, ts):
        """
        :param kp: The proportional gain
        :param kd: The derivative gain
        :param ki: The integral gain
        :param ts: The sampling time
        """
        self.__kp = kp
        self.__kd = kd/ts
        self.__ki = ki*ts
        self.__ts = ts
        self.__previous_error = None                    # None i.e. 'Not defined yet'
        self.__sum_errors = 0.0
        self.steering_action = 0.

    def control(self, y, y_set_point=0):
        """
        :param y: The y-position of the car
        :param y_set_point: The desired y-position of the car
        :return:
        """
        error = y_set_point - y                         # Calculates the control error
        steering_action = self.__kp*error                # P control

        if self.__previous_error is not None:
            steering_action += self.__kd*(error - self.__previous_error)  # D control

        steering_action += self.__ki*self.__sum_errors   # I Control

        self.__sum_errors += error
        self.__previous_error = error                   # Means that next time we need the previous error
        self.steering_action = steering_action            # For steering_cache
        return steering_action


# Initial Variables, Angles, Sampling rate and Ticks
sampling_rate = 40                                      # Sampling rate in Hz
t_final = 50                                            # t [0, 50]
x_initial = 0
y_initial = 0.3                                         # 0.3 m = 30 cm
theta_initial = np.deg2rad(0)                           # 0 ° in radians,
disturbance_initial = np.deg2rad(1)                     # 1 ° in radians, counter-clockwise direction therefore positive
sampling_period = 1/sampling_rate                       # Sampling period in s
ticks = sampling_rate*t_final                           # 40 Hz x 50 s = 2000

# Simulation of vehicle with kp = 0.9, kd = 0.5 and ki = 0.01
car = Car(x=x_initial, y=y_initial, pose=theta_initial, disturbance=disturbance_initial)
pid = PidController(kp=0.8, kd=0.8, ki=0.01, ts=sampling_period)
y_cache = np.array([car.y()], dtype=float)
x_cache = np.array([car.x()], dtype=float)
steering_cache = np.array([pid.steering_action])

for i in range(ticks):
    control_action = pid.control(car.y())
    car.move(control_action, sampling_period)
    y_cache = np.append(y_cache, car.y())
    x_cache = np.append(x_cache, car.x())
    steering_cache = np.append(steering_cache, [pid.steering_action])

car_2 = Car(x=x_initial, y=y_initial, pose=theta_initial, disturbance=disturbance_initial)
pid_2 = PidController(kp=0.8, kd=0.5, ki=0.1, ts=sampling_period)
y_cache_2 = np.array([car_2.y()], dtype=float)
x_cache_2 = np.array([car_2.x()], dtype=float)
steering_cache_2 = np.array([pid_2.steering_action])

for i in range(ticks):
    steering_action_2 = pid_2.control(car_2.y())
    car_2.move(steering_action_2, sampling_period)
    y_cache_2 = np.append(y_cache_2, car_2.y())
    x_cache_2 = np.append(x_cache_2, car_2.x())
    steering_cache_2 = np.append(steering_cache_2, [pid_2.steering_action])

car_3 = Car(x=x_initial, y=y_initial, pose=theta_initial, disturbance=disturbance_initial)
pid_3 = PidController(kp=0.8, kd=0.5, ki=0.7, ts=sampling_period)
y_cache_3 = np.array([car_3.y()], dtype=float)
x_cache_3 = np.array([car_3.x()], dtype=float)
steering_cache_3 = np.array([pid_3.steering_action])

for i in range(ticks):
    control_action_3 = pid_3.control(car_3.y())
    car_3.move(control_action_3, sampling_period)
    y_cache_3 = np.append(y_cache_3, car_3.y())
    x_cache_3 = np.append(x_cache_3, car_3.x())
    steering_cache_3 = np.append(steering_cache_3, [pid_3.steering_action])

plt.plot(x_cache, y_cache, label="K$_i$ = 0.01")
plt.plot(x_cache_2, y_cache_2, label="K$_i$ = 0.1")
plt.plot(x_cache_3, y_cache_3, label="K$_i$ = 0.7")
plt.grid()
plt.xlabel('X - Trajectory (m)')
plt.ylabel('Y - Trajectory (m)')
plt.legend()
plt.show()

t_span = sampling_period * np.arange(ticks + 1)
plt.plot(t_span, steering_cache, label="K$_i$ = 0.01")
plt.plot(t_span, steering_cache_2, label="K$_i$ = 0.1")
plt.plot(t_span, steering_cache_3, label="K$_i$ = 0.7")
plt.grid()
plt.xlabel('Time (s)')
plt.ylabel('Steering Angle (rad)')
plt.legend()
plt.show()