It's never not working.. it's an iterative process..

I've got some datasets with encoder values of "cleaning" sessions. I used GNU octave to make plots using the same algorithm the roomba uses. Only this time "post-process".

This way I can mess around with values to see if I can improve the accuracy of the system. 

The octave code (M-file):

clear();
#DISTANCEPERCOUNT = pi * 72.0 / 508.8
#DISTANCEPERCOUNT = 0.48
DISTANCEPERCOUNT = 0.44456499814949904317867595046408
#WHEELDISTANCE = 235.0
WHEELDISTANCE = 235.0
FILE = '**\FullClean.csv'

fid=fopen(FILE,'r');
nlines = fskipl (fid, Inf)
frewind (fid);
A=fscanf(fid,'%d,%d',[2 nlines+1]);

encoderRPosPrev = A(1);
encoderLPosPrev = A(2);
theta = 0;
Xcoo = 0;
Ycoo = 0;
Xrecord(1) = 0;
Yrecord(1) = 0;

for record = A
  encoderRight=record(2);
  encoderLeft=record(1);

  encoderRDelta = encoderRight-encoderRPosPrev;
  if (encoderRDelta > 32768)
    encoderRDelta = (encoderRight - 32768) - (encoderRPosPrev+32768);
  elseif (encoderRDelta < -32768)
    encoderRDelta = (encoderRight + 32768) - (encoderRPosPrev-32768);
  endif

  encoderLDelta = encoderLeft-encoderLPosPrev;
  if (encoderLDelta > 32768)
    encoderLDelta = (encoderLeft - 32768) - (encoderLPosPrev+32768);
  elseif (encoderLDelta < -32768)
    encoderLDelta = (encoderLeft + 32768) - (encoderLPosPrev-32768);
  endif
  SR = DISTANCEPERCOUNT * encoderRDelta;
  SL = DISTANCEPERCOUNT * encoderLDelta;
  meanDistance = (SL + SR)/2;
  encoderRPosPrev = encoderRight;
  encoderLPosPrev = encoderLeft;
  theta += atan((SR - SL) / WHEELDISTANCE);
  if(theta > 6.28)
    theta -= 6.28;
  elseif(theta < -6.28)
    theta += 6.28;
  endif
  Xcoo += meanDistance * cos(theta);
  Ycoo += meanDistance * sin(theta);
  Xrecord(end+1) = Xcoo;
  Yrecord(end+1) = Ycoo;
endfor
Xplot = Xrecord';
Yplot = Yrecord';
scatter(-Yplot,Xplot,1);

It's not the prettiest, but it works. I had the most difficult time implementing the int16 wraparound. In the end I decided to kinda brute force it.

And some results:

Wheel Distance: 220mm / DistancePerCount: 0.44mm

Wheel Distance: 235mm / DistancePerCount: 0.44mm

Wheel Distance: 250mm / DistancePerCount: 0.44mm

Wheel Distance: 250mm / DistancePerCount: 0.44mm

As you can see, I have some control over the results, but I don't know if it will be enough.

Maybe the whole system is just too noisy to get good results. I think I'll make some driving scripts to get better defined datasets. Drive in a square for starters. The first leg overlapping with the last leg so theta should be equal. This way, I have a closing error and a result to minimise. 

I really want to see what I can get out of the wheel encoders before integrating the IMU. 

Well, I'm having fun :)