% Using observer and SRCF on the double tank
%
close all
Ts=0.1; % sample time
N=600;  % the length of the simulation

% system matrixes
A=[0.9512  0;
     0.0476  0.9512];
B=[ 0.0975;
    0.0024];
Bw=[    0.0975  0;
        0.0024  0.0975];
C=[0 1];
Dw=[0 0.5];
D=0;
T=ss(A,B,C,D,Ts);

% noise covariance matrix 
Rprim=0.01;
R1=0.0125;
Q1=[ 0.01    0;
    0       0.01];
S1=[0 0.005]';
R=Dw*Q1*Dw' + Rprim;
S=Bw*Q1*Dw';
Q=Bw*Q1*Bw;

% input signal
u=[ ones(10/0.1,1); 0.2*ones(10/0.1,1)];    
u=[u;u;u];
t=[0:0.1:599*0.1]';

% the noise signals
vprim=sqrt(R-Dw*Q*Dw')*randn(600,1);
w=sqrt(0.01)*randn(600,2);
wsrc=(Bw*w')';
vsrc=(Dw*w'+vprim')';

% define a new system with input u1=[u w v]^T;
B1=[B Bw zeros(2,1)];
D1=[0 Dw 1];
T1=ss(A,B1,C,D1,Ts);

% inital state
x01=[1.5 1.5]';

% the input
u1=[u w vprim];

% simulation
[ynoisy,t1,xnoisy]=lsim(T1,u1,t,x01);


%%%%%%%%%%% Square Root Covariance Filter %%%%%%%%%%%%%%%%%%%%%%

X=S*(R^(-1/2))';
Qx=Q-S*R^(-1)*S';

% inital values for SRC Filter
xobs0=[0 0]';   % x(0|-1)
Sx=eye(2,2);    % S(0|-1)

% a structure to collect the results
Out=struct('Re',1,'Sx',eye(2,2),'G',zeros(2,1),'xobs',xobs0);

for k=2:N+1;
    M=[C*Out(k-1).Sx    R^(1/2) 0 0;
       A*Out(k-1).Sx    X       Qx^(1/2)];
    Mtr=M';           %use transpose
    
    [Tr,Rmtr] = qr(Mtr);
    
    %                                              -                    -
    Mf=M*Tr;                %the resulted matrix  | Re^1/2  0           0|
                            %                     |  G       S(k+1|k)   0|
                            %                      _                    _
                            
    
    Out(k).Re   =   Mf(1,1);            % Note Out.Re is Re^1/2
    Out(k).G    =   Mf(2:3,1);
    Out(k).Sx   =   Mf(2:3,2:3);
    
    Out(k).xobs  =   A*Out(k-1).xobs + B*u(k-1) + Out(k).G*Out(k).Re^(-1)*(ynoisy(k-1)-C*Out(k-1).xobs);
    
end
    
Asrcf = A - Out(N).G*Out(N).Re^(-1)*C;

%division of the results
for k=1:N;
    x1(k)   = Out(k+1).xobs(1);
    x2(k)   = Out(k+1).xobs(2);
    Re_hsr(k)   = Out(k+1).Re;           %Re_hs  associate to half sqare root R_hsr = Re^1/2
    Sx1(k)  = Out(k+1).Sx(1,1);
    Sx2(k)  = Out(k+1).Sx(2,2);
end

xobs=[x1; x2];

figure(2)

% standard deviation
Res=(ynoisy'-C*xobs);
sigma1=sqrt( (1/N)*sum(Res.*Res))
sigma2=sqrt( (1/N)*sum(Res(50:end).*Res(50:end)) )
Re_hsr(end)

set(gca,'FontSize',18)
XCsrcf=xcorr(ynoisy'-C*xobs);
plot(-floor(length(XCsrcf)/2):floor(length(XCsrcf)/2),XCsrcf,'LineWidth',2)
xlabel('Lag (\tau)')
axis([-100 100 -1 15])
hold on,
figure(1)
[Xps_srcf,om]=pwelch(ynoisy'-C*xobs,128,[],[],1/Ts);
semilogy(om,Xps_srcf,'LineWidth',2);
xlabel('\omega (Hz)')
set(gca,'FontSize',18)
hold on,

%%%%%%%%%%%%%%%%%%   Asymptotic observer    %%%%%%%%%%%%%%%%%%%%%

% inital state of the observer and the input
x0obs=[0 0]';
uobs=[u'; ynoisy'];

% definition of the observer
p=[0.7 0.8]';
K=place(T.a',T.c',p)';
Aobs=[A-K*C];
Bobs=[B K];
Cobs=eye(2,2);
Dobs=zeros(2,2);
Tobs=ss(Aobs,Bobs,Cobs,Dobs,Ts);

[yobs,tobs,xobs]=lsim(Tobs,uobs,t,x0obs);

figure(1)
[Xps,om]=pwelch(ynoisy-xobs*C',128,[],[],1/Ts);
semilogy(om,Xps,'LineWidth',2,'Color','r');
legend('Square Root KF','Asymptotic Obs')
hold off,
figure(2)
%subplot(212)
XC=xcorr(ynoisy-xobs*C');
plot(-floor(length(XC)/2):floor(length(XC)/2),XC,'LineWidth',2,'Color','r')
legend('Square Root KF','Asymptotic Obs')
hold off,

fprintf('The eigenvalues of the Asymptotic Obs\n')
eig(Aobs),

fprintf('The eigenvalues of the steady state SRCF\n')
eig(Asrcf),
