%% parametri:
%Vs       % velocità de sterzo
%Vt       % velocità tangenziale
%T        % angolo di posizione=theta
%P        % angolo di sterzo=phi

% condizioni iniziali:

Vs0=0;    % velocità di sterzo iniziale
Vt0=1.41; % velocità tangenziale iniziale = raiz(2)
x0=-0.02;
y0=0;
th0=pi/4; % angolo de posizione iniziale = pi/4
phi0=0;     % angolo di sterzo iniziale


L=1      % longitudine tra assi del veicolo

% target 
x0t=0;
y0t=0;
th0t=pi/3;
phi0t=0;

% Equazioni:
A=[0 0 -Vt0*sin(th0t) 0
   0 0 Vt0*cos(th0t) 0
   0 0 0 Vt0/(L*(cos(phi0t))^2)
   0 0 0 0];

B=[cos(th0t) 0
   sin(th0t) 0
   tan(phi0t)/L 0
   0 1];

C=[1 0 0 0; 0 1 0 0];

F=-place(A,B,2*[-1,-1.1,-1.2,-1.3]);

eig(A+B*F);


K1=place(A',C',2*[-1 -1.1 -1.2 -1.3]')';

eig(A-K1*C)


Aoss=A-K1*C;
Boss=[B K1];
Coss=eye(4);
Doss=zeros(4,4);
X0oss=[0 0 pi/3 0];

