syms Ux a b kp xla Cf Cr Iz m g Kug k
syms x1 x2 x3 x4

x = [x1; x2; x3; x4]
Gffw = a+b+Kug*Ux^2/g 
Gffw = a+b+Ux^2*m*b/((a+b)*Cf)-Ux^2*m*a/((a+b)*Cr)

% Kapania,no sideslip compensation 
A = [0 Ux 0 Ux;
     0 0 1 0;
     (-a*kp*Cf)/Iz    (-a*kp*xla*Cf)/Iz    -(a^2*Cf+b^2*Cr)/(Ux*Iz)  (b*Cr-a*Cf)/Iz;
     (-kp*Cf)/(m*Ux)  (-kp*xla*Cf)/(m*Ux)  (b*Cr-a*Cf)/(m*Ux^2)-1    (-(Cf+Cr))/(m*Ux)];

% kapania, with sideslip compensation (but doesn't seem correct)
A0 = [0  Ux 0 Ux;
      0  0  1 0;
      (-a*kp*Cf)/Iz   (-a*kp*xla*Cf)/Iz   (-a^2*Cf-b^2*Cr)/(Ux*Iz) (b*Cr-a*Cf*(1-a*kp*xla))/Iz;
      (-kp*Cf)/(m*Ux) (-kp*xla*Cf)/(m*Ux) (-a*Cf+b*Cr)/(m*Ux^2)-1  (-Cf*(1+kp*xla)+Cr)/(m*Ux)];

B = [0; -Ux; a*Cf*Gffw/Iz; Cf*Gffw/(m*Ux)];

disp('E')
ssE = solve(A*x+B*k);
solE = simplify(ssE.x1)
%subs(solE,[a b m Cf Cr],[1.04 1.42 1500 160000 180000])

disp('0')
ss0 = solve(A0*x+B*k);
sol0 = simplify(ss0.x1)
%subs(sol0,[a b m Cf Cr],[1.04 1.42 1500 160000 180000])

% mine, rear axle
Ara = [0  Ux  0                         Ux;
       0  0   1                         0;
       (-a*kp*Cf)/Iz   (-a*kp*xla*Cf)/Iz    -(a*(a+b)*Cf)/(Ux*Iz)   (b*Cr-a*Cf)/Iz;
       (-kp*Cf)/(m*Ux) (-kp*xla*Cf)/(m*Ux)  (-(a+b)*Cf)/(m*Ux^2)-1  (-(Cf+Cr))/(m*Ux)];
Bra = [0; -Ux; a*Cf*Gffw/Iz; Cf*Gffw/(m*Ux)];        
Ara(4,:) = Ara(4,:) - b/Ux*Ara(3,:);
Bra(4) = Bra(4) - b/Ux*Bra(3);

disp('RA')
ssra = solve(Ara*x+Bra*k);
simplify(ssra.x1)


%%


Ux = linspace(10,30);
ay = 3;
kp = .053;
xla = 14.2;

k = ay./Ux.^2;
eSS = -(k.*xla.*(1560.*Ux.^2 - 628776))./442800

eSS0 = -(k.*(1560.*Ux.^2 - 628776).*(115415040000.*kp.*xla + 32112000000))./(70848000000.*kp.*(339456.*kp.*xla + 68400));

figure(); hold on;
plot(Ux,eSS)
plot(Ux,eSS0)