function [L, phi0]= calibrate_bike( dist, phi, L, phi0, dpos)


X = zeros(2,1);

P.dist = dist;
P.phi = phi;
P.L = L;
P.phi0 = phi0;
P.dpos = dpos;

options = optimset('Diagnostics', 'on');
options = optimset(options, 'Display', 'iter');
options = optimset(options, 'tolx', 1e-4);
options = optimset(options, 'tolfun', 1e-4);
options = optimset(options, 'MaxIter', 100);
options = optimset(options, 'LevenbergMarquardt', 'on');

LB = [-L; -10/180*pi]; UB = [2*L; 10/180*pi];
[X,resn,res]=lsqnonlin(@bike_loop_error,X,LB,UB,options, P); 

X
L = L+X(1);
phi0 = phi0+X(2);

function err = bike_loop_error(X, P)

L = P.L+X(1);
phi0 = P.phi0 ;%+ X(2);

[x,y,theta] = odo2pos(P.dist,P.phi, L, phi0);

dpos = -[x(1)-x(end);y(1)-y(end);theta(1)-theta(end)];
err = dpos - P.dpos;

global CALL_COUNT
if isempty(CALL_COUNT), CALL_COUNT = 0; end
CALL_COUNT = CALL_COUNT+1;
if mod(CALL_COUNT, 3) == 1
    figure(1) 
    plot(x,y)
    drawnow;
end