(Q2.2) Suppose that the system is controlled with a PD controller. Fix the value of Kp and plot in the same axes the (x, y) trajectory of the system for t ∈ [0, 50] for different values of Kd. Comment briefly on how Kd affects the closed-loop dynamics

lanekeepingQ2part2.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=1, x=0, y=0, pose_init=0, disturbance= 0):

        self.__length = length
        self.__velocity = velocity
        self.__x = x
        self.__y = y
        self.__pose = pose_init
        self.__disturbance = disturbance

    def move(self, steering_angle, dt):

        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]

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

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

    def y(self):
        return self.__y

    def x(self):
        return self.__x

    def theta(self):
        return self.__pose


class PidController:

    def __init__(self, kp,kd,ts):
        """
        :param kp: proportional gain
        :param ki: integral gain
        :param kd:
        :param ts:
        """
        self.__kp = kp
        self.__ts = ts
        self.__kd = kd / ts
        self.__previous_error = None
        self.__error_sum = 0.

    def control(self, y, set_point=0):
        """Documentation goes here

        :param y:
        :param set_point:
        :return:
        """
        error = set_point - y  # compute the control error
        steering_action = self.__kp * error  # P controller

        #D
        if self.__previous_error is not None:
            error_diff = error - self.__previous_error
            steering_action += self.__kd * error_diff
        #I
        # TODO: Do this as an exercise. Introduce the I component
        # here (don't forget to update the sum of errors).

        self.__previous_error = error
        return steering_action

sampling_rate = 40                                  # Sampling rate in Hz
t_final = 50
x_initial=0
y_initial=0.3
theta_initial = np.deg2rad(0)
disturbance_initial = np.deg2rad(1)
sampling_period = 1 / sampling_rate
ticks = sampling_rate*t_final

car_1 = Car(y=y_initial, x=x_initial, pose_init=theta_initial, disturbance=disturbance_initial)
pid_1 = PidController(kp=0.8, kd=0.2 , ts=sampling_period)
y_cache = np.array([car_1.y()], dtype=float)
x_cache = np.array([car_1.x()], dtype=float)

for i in range(ticks):
    u = pid_1.control(car_1.y())
    car_1.move(u, sampling_period)
    y_cache = np.append(y_cache, car_1.y())
    x_cache = np.append(x_cache, car_1.x())

car = Car(y=y_initial, x=x_initial, pose_init=theta_initial, disturbance=disturbance_initial)
pid = PidController(kp=0.8,kd=0.4, ts=sampling_period)
y_cache_2 = np.array([car.y()], dtype=float)
x_cache_2 = np.array([car.x()], dtype=float)

for i in range(ticks):
    u = pid.control(car.y())
    car.move(u, sampling_period)
    y_cache_2 = np.append(y_cache_2, car.y())
    x_cache_2 = np.append(x_cache_2, car.x())

car = Car(y=y_initial, x=x_initial, pose_init=theta_initial, disturbance=disturbance_initial)
pid = PidController(kp=0.8,kd=0.8, ts=sampling_period)
y_cache_3 = np.array([car.y()], dtype=float)
x_cache_3 = np.array([car.x()], dtype=float)

for i in range(ticks):
    u = pid.control(car.y())
    car.move(u, sampling_period)
    y_cache_3 = np.append(y_cache_3, car.y())
    x_cache_3 = np.append(x_cache_3, car.x())

plt.plot(x_cache, y_cache, label="K$_d$ = 0.02")
plt.plot(x_cache_2, y_cache_2, label="K$_d$ = 0.20")
plt.plot(x_cache_3, y_cache_3, label="K$_d$ = 0.80")
plt.grid()
plt.xlabel('X - Trajectory (m)')
plt.ylabel('Y - Trajectory (m)')
plt.legend()
plt.show()