%param delta_encoders soll ein Array mit 1:linkerEncoderWert
%und 2:rechterEncoderWert sein
function [pos_prime phi_prime] = getLocPos(delta_encoders)

%TODO delta_encoders in Rad-Umfang umrechnen

b_R = delta_encoders(1)
b_L = delta_encoders(2)

b = 0.5 * (b_R + b_L)

%TODO Richtigen Wert für l einsetzen
%TODO Wenn b_R == b_L ist Nenner == 0, wie behandeln?
l = 10
r = 0.5 * l * ( (b_R + b_L)/(b_R - b_L) )

phi_prime = b / r
x_prime = r * sin(phi_prime)
y_prime = r * (1 - cos(phi_prime))

pos_prime(1) = x_prime
pos_prime(2) = y_prime

end

