% Dos masas de 1 kg y 1 kg, con muelles/amort por en medio...
% Entrada: Fuerza sobre masa 2
% Estados posición y velocidad masa 1, posición y velocidad masa 2.

k1=3;f1=.1;k12=400.1;f12=4.025; M2=1; M1=1;
A=[0 1 0 0; ...
    -(k1+k12)/M1 -(f1+f12)/M1 k12/M1 f12/M1; ...
    0 0 0 1;... 
    k12/M2 f12/M2 -k12/M2 -f12/M2]
eig(A)
B=[0; 0; 0;1/M2]
%B=[0 0;1 0;0 0;0 1];%2 fzas?
Csim=[1 0 0 0;0 0 1 0]
sys=ss(A,B,Csim,0);
tf(sys)
step(sys,100)
bode(sys,logspace(-1,1))

%
%% Controlabilidad

Contrm=ctrb(A,B)
Gramc=gram(sys,'c')

%

[V,Lam]=eig(Gramc); V
sqrt(diag(Lam)')

% Probar distintos valores k12, f12.
%% Observabilidad
% Vamos a discretizar, para también evaluar cómo influye la frecuencia con la que se toman las medidas. [Detalles teoría discretización en http://personales.upv.es/asala/videos/disc1.html y ejemplos de Matlab detallados en http://personales.upv.es/asala/videos/disc2.html ]

Periodo=.3;
Csens=[1000 0 0 0];
sys2=ss(A,B,Csens,0);
sys2d=c2d(sys2,Periodo,'zoh');
Obsm=obsv(sys2d.A,sys2d.C)
Gramo=gram(sys2d,'o')
[Vo,Lamo]=eig(Gramo);diag(Lamo)', Vo

% Calidad de estimado de x0 en función de su efecto en medidas futuras:

sqrt(1./diag(Lamo)')