# Self localization for 3 wheeled omnidirectional robot

I am trying to self localize a 3 wheeled omni directional robots using just motor encoders I followed the odometry method mentioned in equations 5,8,9 of https://bibliotecadigital.ipb.pt/bitstream/10198/1897/1/C2_002_Gon%C3%A7alves.pdf

But I cant seem to implement the equations in code, It'll be helpful if anyone help me out with the basic structure of the code or flowchart

I agree that the paper's notation is a little confusing. I think the confusing part is that they use $$K$$ for time-step number, and $$T$$ for delta time. So equation (5) simply says: your new global angle $$\theta$$ (the angle at time $$K$$), equals your last angle (at time $$K-1$$) plus your angular velocity $$w$$ times the delta time $$T$$.

I think this how it should go:

loop()
{
Current_Encoders = get_encoder_counts()
Delta_Encoders = Current_Encoders - Last_Encoders
Last_Encoders = Current_Encoders

now = get_time()
T = now - last_time
last_time = now

// time detla * encoder delta = instantaneous wheel velocities
V_1, V_2, V_3 = T * Delta_Encoders

// get instantaneous local velocities
// Note: G is a constant matrix, so this can be turned into 3 equations
V, V_n, w = G * (V_1, V_2, V_3)

// get new global theta
theta = theta + w * T

// get global velocities
V_x = V * cos(theta) - V_n * sin(theta)
V_y = V * sin(theta) + V_n * cos(theta)

// get new global positions
x = x + V_x * T
y = y + V_y * T
}