Self localization for 3 wheeled omnidirectional robot

Question

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

Answer 1

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
}

Please pardon my pseudo-code shorthand.

And of course, all variables should be initialized to zero before the loop. (except the time variables which you might want to initialize to the current time. And the encoders which might need to be initialized to the encoder count if you have absolute instead of incremental encoders.)