Question about PID controller

[Ramri-as]

Hey everyone,

I recently created a PID for lateral control. I want to check whether it can correct the agent yaw to the desired yaw. The result is not what I hoped for because the agent starts to drift away or worse rotate even though the difference between its yaw and desired yaw is only 0.001 something.

Here is the scenario,
-Agent: Tesla Model 3
-Spawn yaw: -90
-Desired yaw: -90
-Desired speed: 3 km/h
-Simulation time: 15 sec
-Step time: 0.05 sec
-Reverse gear = True

An agent is spawned on Town04 with a determined location and rotation. It has two controllers, both PID, for lateral and longitudinal. The PID takes the agent's current yaw and speed every time step and compares it with desired values. The output is PID then converted to control action throttle and steering.

Is there something wrong with my code or is there something I missed??
Can simple PID be used in CARLA??

Here is my code

class PIDController:
def init(self, K_P, K_I, K_D, dt):
self.K_P = K_P
self.K_I = K_I
self.K_D = K_D
self.dt = dt
self.integral = 0
self.prev_error = 0

def reset(self):
    self.integral = 0
    self.prev_error = 0

def run_step(self, target_value, current_value):
    error = target_value - current_value
    self.integral += error * self.dt
    derivative = (error - self.prev_error) / self.dt
    self.prev_error = error
    output = self.K_P * error + self.K_I * self.integral + self.K_D * derivative
    return output, error, self.integral, derivative

class VehiclePIDController:
def init(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75, max_brake=0.3, max_steering=0.8):
self.max_throttle = max_throttle
self.max_brake = max_brake
self.max_steering = max_steering
self.vehicle = vehicle
self.world = self.vehicle.get_world()
self.past_steering = self.vehicle.get_control().steer

    self.lon_controller = PIDController(**args_longitudinal)
    self.lat_controller = PIDController(**args_lateral)

def run_step(self, target_speed, target_yaw, dt):
    current_speed = self.get_speed()
    throttle, speed_error, speed_integral, speed_derivative = self.lon_controller.run_step(target_speed,
                                                                                           current_speed)
    throttle = np.clip(throttle, 0.0, self.max_throttle)

    current_yaw = self.vehicle.get_transform().rotation.yaw
    steering, yaw_error, yaw_integral, yaw_derivative = self.lat_controller.run_step(target_yaw, current_yaw)
    steering = np.clip(steering, -self.max_steering, self.max_steering)

    return throttle, steering, current_speed, current_yaw, speed_error, speed_integral, speed_derivative, yaw_error, yaw_integral, yaw_derivative

def get_speed(self):
    velocity = self.vehicle.get_velocity()
    return np.sqrt(velocity.x ** 2 + velocity.y ** 2)

def main():
client = carla.Client('localhost', 2000)
client.set_timeout(20.0)
world = client.load_world('Town04')

blueprint_library = world.get_blueprint_library()
vehicle_bp = blueprint_library.find('vehicle.tesla.model3')
spawn_point = carla.Transform(carla.Location(x=7.84, y=-193.93, z=0.25), carla.Rotation(yaw=-90.0))
vehicle = world.spawn_actor(vehicle_bp, spawn_point)

args_lateral = {'K_P': 0, 'K_I': 0, 'K_D': 0, 'dt': 0.05}
args_longitudinal = {'K_P': 1.2, 'K_I': 0.2, 'K_D': 0, 'dt': 0.05}

pid_controller = VehiclePIDController(vehicle, args_lateral, args_longitudinal)

target_speed = 3.0
target_yaw = -90.0

time_step = 0.05
time_data = []
steering_data = []
throttle_data = []
target_speed_data = []
actual_speed_data = []
target_yaw_data = []
actual_yaw_data = []

speed_error_data = []
speed_integral_data = []
speed_derivative_data = []
yaw_error_data = []
yaw_integral_data = []
yaw_derivative_data = []

# Set spectator view
spectator = world.get_spectator()
spectator.set_transform(
    carla.Transform(vehicle.get_transform().location + carla.Location(z=20), carla.Rotation(pitch=-90)))

# Delay before starting the main loop
time.sleep(1)

# Main loop
try:
    start_time = time.time()
    while True:
        current_time = time.time() - start_time

        throttle, steering, actual_speed, actual_yaw, speed_error, speed_integral, speed_derivative, yaw_error, yaw_integral, yaw_derivative = pid_controller.run_step(
            target_speed, target_yaw, time_step)

        # Apply control
        control = carla.VehicleControl(throttle=throttle, steer=steering, reverse=True)
        vehicle.apply_control(control)

        # Collect data
        time_data.append(current_time)
        steering_data.append(steering)
        throttle_data.append(throttle)
        target_speed_data.append(target_speed)
        actual_speed_data.append(actual_speed)
        target_yaw_data.append(target_yaw)
        actual_yaw_data.append(actual_yaw)

        speed_error_data.append(speed_error)
        speed_integral_data.append(speed_integral)
        speed_derivative_data.append(speed_derivative)
        yaw_error_data.append(yaw_error)
        yaw_integral_data.append(yaw_integral)
        yaw_derivative_data.append(yaw_derivative)

        # Time step
        time.sleep(time_step)

        # Run simulation for a total of 15 seconds
        if current_time > 15:
            break

finally:
    vehicle.destroy()

# Visualization
plt.figure()
plt.plot(time_data, steering_data, label='Steering Input')
plt.plot(time_data, throttle_data, label='Throttle Input')
plt.xlabel('Time (s)')
plt.ylabel('Input Value')
plt.legend()
plt.title('Steering Input & Throttle Input')
plt.show()

plt.figure()
plt.plot(time_data, target_speed_data, label='Target Speed')
plt.plot(time_data, actual_speed_data, label='Actual Speed')
plt.xlabel('Time (s)')
plt.ylabel('Speed (m/s)')
plt.legend()
plt.title('Target Speed & Actual Speed')
plt.show()

plt.figure()
plt.plot(time_data, target_yaw_data, label='Target yaw')
plt.plot(time_data, actual_yaw_data, label='Actual yaw')
plt.xlabel('Time (s)')
plt.ylabel('Yaw angle (rad)')
plt.legend()
plt.title('Target Yaw vs Actual Yaw')
plt.show()

plt.figure()
plt.plot(time_data, speed_error_data, label='Speed Error')
plt.plot(time_data, speed_integral_data, label='Speed Integral')
plt.plot(time_data, speed_derivative_data, label='Speed Derivative')
plt.xlabel('Time (s)')
plt.ylabel('Speed PID Values')
plt.legend()
plt.title('Speed PID Components')
plt.show()

plt.figure()
plt.plot(time_data, yaw_error_data, label='Yaw Error')
plt.plot(time_data, yaw_integral_data, label='Yaw Integral')
plt.plot(time_data, yaw_derivative_data, label='Yaw Derivative')
plt.xlabel('Time (s)')
plt.ylabel('Yaw PID Values')
plt.legend()
plt.title('Yaw PID Components')
plt.show()

if name == 'main':
main()