%Linearized 4th order model and analysis of eigenvalues
%Equations based on Schwab et al 2004
%Run bicycleparameters first
%kja 040611
%Parameters of a bicycle model
%kja 040613
%Basic data is given by 26 parameters

aminit;

%Acceleration of gravity [m/s^2]
g=9.81;

%Wheel base [m]
b=1.00;	

%Trail [m]
c=0.08;

%c=-0.5 %A large negative trail seems to help for rear-wheel steering!
%Wheel radii
Rrw=0.35;
Rfw=0.35;

%Head angle [radians]
lambda=pi*70/180;	

%Rear frame mass [kg], center of mass [m], and inertia tensor [kgm^2]
mrf=12;xrf=0.439;zrf=0.579;
Jxxrf=0.475656;Jxzrf=0.273996;Jyyrf=1.033092;Jzzrf=0.527436;
mrf=87;xrf=0.491586;zrf=1.028138;
Jxxrf=3.283666;Jxzrf=0.602765;Jyyrf=3.8795952;Jzzrf=0.565929;

%Front frame mass [kg], center of mass [m], and inertia tensor [kgm^2]
mff=2;xff=0.866;zff=0.676;
Jxxff=0.08;Jxzff=-0.02;Jyyff=0.07;Jzzff=0.02;

%Rear wheel mass [kg], center of mass [m], and inertia tensor [kgm^2]
mrw=1.5;Jxxrw=0.07;Jyyrw=0.14;

%Front wheel mass [kg], center of mass [m], and inertia tensor [kgm^2]
%mfw=3;Rfw=0.35;Jxxfw=0.14;Jyyfw=0.28;
mfw=1.5;Jxxfw=0.07;Jyyfw=0.14;

%Auxiliary variables
xrw=0;zrw=Rrw;xfw=b;zfw=Rfw;
Jzzrw=Jxxrw;Jzzfw=Jxxfw;
mt=mrf+mrw+mff+mfw;
xt=(mrf*xrf+mrw*xrw+mff*xff+mfw*xfw)/mt;
zt=(mrf*zrf+mrw*zrw+mff*zff+mfw*zfw)/mt;
Jxxt=Jxxrf+mrf*zrf^2+Jxxrw+mrw*zrw^2+Jxxff+mff*zff^2+Jxxfw+mfw*zfw^2;
Jxzt=Jxzrf+mrf*xrf*zrf+mrw*xrw*zrw+Jxzff+mff*xff*zff+mfw*xfw*zfw;
Jzzt=Jzzrf+mrf*xrf^2+Jzzrw+mrw*xrw^2+Jzzff+mff*xff^2+Jzzfw+mfw*xfw^2;
mf=mff+mfw;
xf=(mff*xff+mfw*xfw)/mf;zf=(mff*zff+mfw*zfw)/mf;
Jxxf=Jxxff+mff*(zff-zf)^2+Jxxfw+mfw*(zfw-zf)^2;
Jxzf=Jxzff+mff*(xff-xf)*(zff-zf)+mfw*(xfw-xf)*(zfw-zf);
Jzzf=Jzzff+mff*(xff-xf)^2+Jzzfw+mfw*(xfw-xf)^2;
d=(xf-b-c)*sin(lambda)+zf*cos(lambda);
Fll=mf*d^2+Jxxf*cos(lambda)^2+2*Jxzf*sin(lambda)*cos(lambda)+Jzzf*sin(lambda)^2;
Flx=mf*d*zf+Jxxf*cos(lambda)+Jxzf*sin(lambda);
Flz=mf*d*xf+Jxzf*cos(lambda)+Jzzf*sin(lambda);
gamma=c*sin(lambda)/b;
Sr=Jyyrw/Rrw;Sf=Jyyfw/Rfw;St=Sr+Sf;Su=mf*d+gamma*mt*xt;
%Matrices for linearized fourth order model
M=[Jxxt -Flx-gamma*Jxzt;-Flx-gamma*Jxzt Fll+2*gamma*Flz+gamma^2*Jzzt];
K0=[-mt*g*zt g*Su;g*Su  -g*Su*cos(lambda)];
K1=[0 -(St+mt*zt)*sin(lambda)/b;0 (Su+Sf*cos(lambda))*sin(lambda)/b];
c12=gamma*St+Sf*sin(lambda)+Jxzt*sin(lambda)/b+gamma*mt*zt;
c22=Flz*sin(lambda)/b+gamma*(Su+Jzzt*sin(lambda)/b);
C0=[0 -c12;(gamma*St+Sf*sin(lambda)) c22];
one=diag([1 1]);null=zeros(2,2);

%Plot eigenvalues as function of velocity
figure(1); clf; subplot(221); hold on;

% Shade the stability region (bounds are printed out below)
patch([6.1 6.1 10.5 10.5 6.1], [-15 15 15 -15 -15], [0.9 0.9 0.9], 'EdgeColor', 'none');
plot([-15 15], [15 15], 'k', 'LineWidth', AM_thin_linewidth);
plot([-15 15], [-15 -15], 'k', 'LineWidth', AM_thin_linewidth);

% Initialize vectors to keep track of segments
stable = 0;
for i=1:4
  ee{i} = []; vv{i} = []; esign(i) = 0;
end

for V=-15:0.1:15
  C = C0*V;
  K = K0+K1*V^2;
  A = [one null;null M];B=[null one;-K -C];
  D = sort(real(eig(B,A)));

  % See if we have had an overall change in stability
  if (max(esign) * stable < 0) 
    % Sign changed; plot a vertical line
%    Not needed due to shading
%    plot([V V], [-15 15], 'k--', 'LineWidth', AM_ref_linewidth);
    fprintf(1, 'Change of sign at V = %g\n', V);
  end
  stable = max(esign);

  % Now go through each eigenvalue and evaluate
  for i=1:4
    % Figure out whether there has been a change in sign
    if (length(ee{i}) > 0 && sign(real(D(i))) ~= esign(i))
      % Eigenvalue has changed sign; plot it and reset
      if (esign(i) < 0) ph = plot(vv{i}, ee{i}, 'b-');
      else ph = plot(vv{i}, ee{i}, 'r--'); end
      set(ph, 'LineWidth', AM_data_linewidth);
      ee{i} = []; vv{i} = []; 
    end
    esign(i) = sign(real(D(i)));

    % Append this point to our list
    ee{i} = [ee{i}; D(i)]; vv{i} = [vv{i}; V];
  end
  
end

% Plot what's left of the ligns
for i=1:4
  if (esign(i) < 0) ph = plot (vv{i}, ee{i}, 'b-'); 
  else ph = plot(vv{i}, ee{i}, 'r--'); end;
  set(ph, 'LineWidth', AM_data_linewidth);
end

plot([-15 15], [0 0], 'k-', 'LineWidth', AM_thin_linewidth);
plot([0 0], [-15 15], 'k-', 'LineWidth', AM_thin_linewidth);

% Resize and label the axes
amaxis([-15 15 -10 10])
xlabel('x'); ylabel('y');
amaxis([-15 15 -15 15])
axis square; box on;

% Add some labels for stable and unstable regions
amtext(-11, 9, 'Unstable');
amtext(8.2, 6, 'Stable', 'Rotation', 90);
amtext(12.9, 5, 'Unstable', 'Rotation', 90);

amprint('bike-stabplot.eps');
