close all
clear all
clc

% System
A=[  0.96          0.5      0.27      0.28
    -0.125         0.96    -0.08     -0.07
    0             0        0.85      0.97
    0             0        0         0.99];
Bv=[0.5;0;0;1]*sqrt(15);
C=[0 2 0 0];

% Observability verification
O=obsv(A,C);
rank(O)

% Noise variances
V1=Bv*Bv';
V2=2000;

% Iitial state and output
[n,nn]=size(A);
x(:,1)=[0.1;0.2;-0.1;0.15];
y(1)=0.4;

% Dynamic predictor and filter. Initial conditions:
x_h(:,1)=zeros(n,1);
y_h(1)=C*x_h(:,1);
x_hh(:,1)=zeros(n,1);
P{1}=0.4*eye(n);

% Steady-state predictor and filter
sys1=ss(A,[zeros(n,1) eye(n)],C,[0 zeros(1,n)],1);
[KEST,Kr,Pr,Mr]=kalman(sys1,V1,V2,0);
x_hr(:,1)=zeros(n,1);
y_hr(1)=C*x_hr(:,1);
x_f(:,1)=zeros(n,1);

% Time evolution
N=2000;
for t=1:N
    
    % noise signals
    v1=Bv.*randn(n,1);
    v2=sqrt(V2)*randn;
    
    % system
    x(:,t+1)=A*x(:,t)+v1;
    y(t)=C*x(:,t)+v2;
    y_true(t)=C*x(:,t);
    
    % dynamic predictor
    % classical implementation
    K{t}=A*P{t}*C'*inv(C*P{t}*C'+V2);
    P{t+1}=A*P{t}*A'+V1-K{t}*[C*P{t}*C'+V2]*K{t}';
    y_h(t)=C*x_h(:,t);
    x_h(:,t+1)=A*x_h(:,t)+K{t}*(y(t)-y_h(t));
    %     predictor/corrector implementation
    K0=P{t}*C'*inv(C*P{t}*C'+V2);
    %     P0=(eye(n)-K0*C)*P{t};
    %     P{t+1}=A*P0*A'+V1;
    %     y_h(t)=C*x_h(:,t);
    %     xNN=x_h(:,t)+K0*(y(t)-y_h(t));
    %     x_h(:,t+1)=A*xNN;
    
    % dynamic filter
    % K0=P{t}*C'*inv(C*P{t}*C'+V2);
    x_hh(:,t)=x_h(:,t)+K0*(y(t)-y_h(t));
    
    % steady-state predictor
    y_hr(t)=C*x_hr(:,t);
    x_hr(:,t+1)=A*x_hr(:,t)+Kr*(y(t)-y_hr(t));
    
    % steady-state filter
    % x_f(:,t)=xNN;
    x_f(:,t)=x_hr(:,t)+Mr*(y(t)-y_hr(t));
    % y_f(t)=C*x_f(:,t);
    
end

N0=1;
for k=1:4
    
    figure,hold on
    plot(x(k,1:N))
    plot(x_h(k,1:N),'r')
    plot(x_hh(k,1:N),'c')
    plot(x_hr(k,1:N),'g')
    plot(x_f(k,1:N),'k')
    
    RMSE(k,1)=norm(x(k,N0+1:N)-x_h(k,N0+1:N))/sqrt(N-N0);
    RMSE(k,2)=norm(x(k,N0+1:N)-x_hh(k,N0+1:N))/sqrt(N-N0);
    RMSE(k,3)=norm(x(k,N0+1:N)-x_hr(k,N0+1:N))/sqrt(N-N0);
    RMSE(k,4)=norm(x(k,N0+1:N)-x_f(k,N0+1:N))/sqrt(N-N0);
    
end
RMSE

