Webots E-Puck localization VERY incaccurate (python)

Question

I need to make a robot which follows a line, then heads off the line when close to the goal and then rejoins the line. but the localization program which has been provided has proven itself to be inaccurate.

"""Lab3 controller."""


#  from controller import Robot, Motor, DistanceSensor
from controller import Robot, DistanceSensor, Motor
import numpy as np
# create the Robot instance.
robot = Robot()

# define max speed
MAX_SPEED = 6.28

# get the time step of the current world.
TIMESTEP = 32 # ms
delta_t = TIMESTEP/1000.0 #s

runtime = 0
totalS = 0

# Robot pose
# Adjust the initial values to match the initial robot pose in your simulation
x = 0    # position in x [m]
y = 0.45    # position in y [m]
phi = -0  # orientation [rad]

# Robot velocity and acceleration
dx = 0.0   # speed in x [m/s]
dy = 0.0   # speed in y [m/s]
ddx = 0.0  # acceleration in x [m/s^2]
ddy = 0.0  # acceleration in y [m/s^2]

# Robot wheel speeds
wl = 0.0    # angular speed of the left wheel [rad/s]
wr = 0.0    # angular speed of the right wheel [rad/s]

# Robot linear and angular speeds
u = 0.0    # linear speed [m/s]
w = 0.0    # angular speed [rad/s]

# e-puck Physical parameters for the kinematics model (constants)
R = 0.0205    # radius of the wheels: 20.5mm [m]
D = 0.0565    # distance between the wheels: 52mm [m]
A = 0.05    # distance from the center of the wheels to the point of interest [m]

# initz line sensors
gs = []
gsNames = ['gs0', 'gs1', 'gs2']
for i in range(3):
    gs.append(robot.getDevice(gsNames[i]))
    gs[i].enable(TIMESTEP)


# encoders
encoder = []
encoderNames = ['left wheel sensor', 'right wheel sensor']
for i in range(2):
    encoder.append(robot.getDevice(encoderNames[i]))
    encoder[i].enable(TIMESTEP)

oldEncoderValues = []

# initz motors    
leftMotor = robot.getDevice('left wheel motor')
rightMotor = robot.getDevice('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(0.0)

# Robot Localization functions

def get_wheels_speed(encoderValues, oldEncoderValues, delta_t):
    """Computes speed of the wheels based on encoder readings"""
    #Encoder values indicate the angular position of the wheel in radians
    wl = (encoderValues[0] - oldEncoderValues[0])/delta_t
    wr = (encoderValues[1] - oldEncoderValues[1])/delta_t

    return wl, wr


def get_robot_speeds(wl, wr, r, d):
    """Computes robot linear and angular speeds"""
    u = r/2.0 * (wr + wl)
    w = r/d * (wr - wl)

    return u, w


def get_robot_pose(u, w, x_old, y_old, phi_old, delta_t):
    """Updates robot pose based on heading and linear and angular speeds"""
    delta_phi = w * delta_t
    phi = phi_old + delta_phi
    phi_avg = (phi_old + phi)/2   
    if phi >= np.pi:
        phi = phi - 2*np.pi
    elif phi < -np.pi:
        phi = phi + 2*np.pi
    
    delta_x = u * np.cos(phi_avg) * delta_t
    delta_y = u * np.sin(phi_avg) * delta_t
    x = x_old + delta_x
    y = y_old + delta_y

    return x, y, phi


#------------ Main Loop ------------------------------------------------------------------------------------
while True:

    # start simulation step
    robot.step(TIMESTEP)

    # define seconds since start simulation, every step is 0.032 seconds
    totalS = round(totalS + 0.032, 4)
    print(totalS)

    # Read the line sensors:
    gsValues = []
    for i in range(3):
        gsValues.append(gs[i].getValue())

    # Process line sensor data here.
    line_right = round(gsValues[0])
    line_center = round(gsValues[1])
    line_left = round(gsValues[2])
# Data gathering -----------------------------------------------------------------------------


    # Read the encoders
    encoderValues = []
    for i in range(2):
        encoderValues.append(encoder[i].getValue())    # [rad]
        
    # Update old encoder values if not done before
    if len(oldEncoderValues) < 2:
        for i in range(2):
            oldEncoderValues.append(encoder[i].getValue())   

#-- Localization -------------------------------------------------------------------------------------------

    # Compute speed of the wheels
    [wl, wr] = get_wheels_speed(encoderValues, oldEncoderValues, delta_t)
    
    # Compute robot linear and angular speeds
    [u, w] = get_robot_speeds(wl, wr, R, D)
    
    # Compute new robot pose
    [x, y, phi] = get_robot_pose(u, w, x, y, phi, delta_t)

    # update old encoder values for the next cycle
    oldEncoderValues = encoderValues

    print(f'Sim time: {totalS}  Pose: x={x:.2f} m, y={y:.2f} m, phi={phi:.4f} rad.')


#-- Line Following -----------------------------------------------------------------------------------------

        # fullspeed ahead
        fullspeed = 0
        if line_left < 500 and line_right < 500:
            leftMotor.setVelocity(MAX_SPEED)
            rightMotor.setVelocity(MAX_SPEED)
            fullspeed = 1
            print('fullspeed')
        # turn Right
        right = 0
        if line_right > 500 and line_left < 500:
            leftMotor.setVelocity(MAX_SPEED)
            rightMotor.setVelocity(MAX_SPEED/2)
            right = 1
            print('right')
        # turn Left
        left = 0
        if line_right < 500 and line_left > 500:
            leftMotor.setVelocity(MAX_SPEED/2)
            rightMotor.setVelocity(MAX_SPEED)
            left = 1
            print('left')
        # if there is no line turn right (most probable as it is a right turning oval)
        if fullspeed == 0 and right == 0 and left == 0: 
            leftMotor.setVelocity(MAX_SPEED)
            rightMotor.setVelocity(2)
            print('houston, we have a problem')

I tried to add a gps on the turret slot, but this only resulted in more head ache as i couldn't understand the python documentation and the video i found was outdated according to the IDE. If you have any suggestions on how to improve i would like to hear them. I already fixed the global and local variables in the function in my own snippet, but i tried to keep this snippet as close to the original as possible.

The problem only happens when the robot leaves the line, but i also don't know why that is.