Используя формулу одометрии, чтобы выяснить и исправить путь робота

Question

Использование ev3dev и MicroPython.

Я пытаюсь реализовать формулу грубой одометрии, чтобы исправить траекторию движения робота по прямой линии.

Вот мой код Это работает, и все же я не уверен, что в течение нескольких секунд он теряет свою линию, и правильный не может выявить слабину.

#!/usr/bin/env pybricks-micropython 
from pybricks import ev3brick as brick 
from pybricks.parameters import Button, Port, Stop
from pybricks.ev3devices import Motor, GyroSensor
from pybricks.tools import wait
from pybricks.robotics import DriveBase

import math

leftMotor = Motor(Port.B)
rightMotor = Motor(Port.C)

leftMotor.reset_angle(0)
rightMotor.reset_angle(0)

gyro = GyroSensor(Port.S2)
gyro.reset_angle(0)

wheelDiam = 56
wheelDist = 114

speed = 50
angle = 0
change = 0

robot = DriveBase(leftMotor, rightMotor, wheelDiam, wheelDist)
fudge = 0.5
fudge2 = -0.6
calibration = (math.pi * wheelDiam) / 360

while True:
    robot.drive(speed,angle)
    drift = leftMotor.angle() - rightMotor.angle()
    leftCalc = leftMotor.angle() * calibration
    rightCalc = rightMotor.angle() * calibration
    drift2 = (leftCalc - rightCalc) / wheelDist
    superCalc = math.degrees(math.atan(drift2))
    angle = (superCalc * fudge2) 

    print("figures gyro",gyro.angle(),"angle",angle,"super",superCalc)

Я сделал какую-то фундаментальную ошибку в коде здесь? Это работает, но это не так.