%Caso sistema osservabile

A=[ 1 1 0 0 0;
    0 1 0 0 0;
    0 0 -1 1 0;
    0 0 0 -1 0;
    0 0 0 0 2]

B=[0; 1; 0; -1; 0]

C=[1 0 1 0 1]

Os=[C; C*A; C*A^2; C*A^3; C*A^4]

rank(Os)

%x=[0; 1; 0; -1; 0]

u(1)=0;
u(2)=-1;
u(3)=1;
u(4)=-1;
u(5)=0;

y(1)=0;
y(2)=0;
y(3)=4;
y(4)=2;
y(5)=6;

t=size(A);

q=zeros(size(C));
for j=0:1:t-1
    yf=0;
    for k=1:1:j
        yf=yf+C*A^(k-1)*B*u(j-k+1);
    end
    q(j+1)=y(j+1)-yf;
end

x_0=inv(Os)*transpose(q)

%x=[0; 1; 0; -1; 0]

fprintf('---------------------------------\n');

%Caso sistema osservabile con dati non compatibili

A=[ 1 1 0 0 0;
    0 1 0 0 0;
    0 0 -1 1 0;
    0 0 0 -1 0;
    0 0 0 0 2]

B=[0; 1; 0; -1; 0]

C=[1 0 1 0 1]

Os=[C; C*A; C*A^2; C*A^3; C*A^4; C*A^5]

rank(Os)

%x=[0; 1; 0; -1; 0]

u(1)=0;
u(2)=-1;
u(3)=1;
u(4)=-1;
u(5)=0;
u(6)=0;

y(1)=0;
y(2)=0;
y(3)=4;
y(4)=2;
y(5)=6;
y(6)=-1;

t=6;

q=zeros(size(C));
for j=0:1:t-1
    yf=0;
    for k=1:1:j
        yf=yf+C*A^(k-1)*B*u(j-k+1);
    end
    q(j+1)=y(j+1)-yf;
end

q=q';

x_0=inv(Os'*Os)*Os'*q

%x=[0; 1; 0; -1; 0]
min1=Os*x_0-q


fprintf('------------------------------------\n');
%Caso dati Compatibili min quadrati trova soluzione esatta

u(1)=0;
u(2)=-1;
u(3)=1;
u(4)=-1;
u(5)=0;
u(6)=0;

y(1)=0;
y(2)=0;
y(3)=4;
y(4)=2;
y(5)=6;
y(6)=8;

t=6;

q=zeros(size(C));
for j=0:1:t-1
    yf=0;
    for k=1:1:j
        yf=yf+C*A^(k-1)*B*u(j-k+1);
    end
    q(j+1)=y(j+1)-yf;
end

q=q';

x_0=inv(Os'*Os)*Os'*q

min2=Os*x_0-q
