(Q2.1) Suppose that the system is controlled with a P controller. Plot in the same axes the (x, y) trajectories of the system for t ∈ [0, 50] for different values of Kp (starting from some initial point). Comment on how Kp affects the closed-loop behaviour

lanekeepingQ2part1.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,ts):
        """Documentation goes here

        :param kp: proportional gain
        :param ki: integral gain
        :param kd:
        :param ts:
        """
        self.__kp = kp
        self.__ts = ts
    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

        # I component:
        # 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 = 2000

car = Car(y=y_initial, x=x_initial, pose_init=theta_initial, disturbance=disturbance_initial)
pid = PidController(kp=0.02, ts=sampling_period)
y_cache_1 = np.array([car.y()], dtype=float)
x_cache_1 = np.array([car.x()], dtype=float)

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

car = Car(y=y_initial, x=x_initial, pose_init=theta_initial, disturbance=disturbance_initial)
pid = PidController(kp=0.2, 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, 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_1,y_cache_1, label = "K$_p$ = 0.02")
plt.plot(x_cache_2,y_cache_2, label = "K$_p$ = 0.2")
plt.plot(x_cache_3,y_cache_3, label = "K$_p$ = 0.8")
plt.grid()
plt.xlabel('X - Trajectory (m)')
plt.ylabel('Y - Trajectory (m)')
plt.legend()
plt.show()