%% Solution of Labsession KF

clear; close all; clc

load data_kf_cattle

% system matrices
A=[ a1 a2 a3 0  0  0;
    b1 0  0  0  0  0;
    0  b2 b3 0  0  0;
    c1 c2 c3 0  0  0;
    0  0  0  d1 0  0;
    0  0  0  0  d2 d3];
B=eye(6);
G=eye(6);
C= [ 1  1  1  0  0  0;
     0  0  0  1  1  1];
Q=sw2*eye(6);
R=sv2*eye(2);

N=length(t);


%%%%
% A
%%%%

% initialization
x0 = zeros(6,1);   % x(0|-1)
P0 = 100^2*eye(6); % P(0|-1)

xp=x0;
Pp=P0;

% recursion
for k=1:N,
        
    % correction
    K = Pp*C'*inv(C*Pp*C'+R);
    x = xp + K*(yA(k,:)'-C*xp);
    P = Pp*(eye(6)-C'*K');
    
    % prediction
    xp = A*x + B*uA(k,:)';
    Pp = A*P*A' + G*Q*G';
    
    XestA(k,:)=x';
    PkfA(k,:)=diag(P)';
    
end

errorsA=xtrueA-XestA;
errsumA=sum((errorsA.^2)');
MSE_A=sum(PkfA');

% state variables estimates
figure
subplot(321), plot(t,xtrueA(:,1),'g',t,XestA(:,1),'r--');ylabel('x1')
subplot(322), plot(t,xtrueA(:,2),'g',t,XestA(:,2),'r--');ylabel('x2')
subplot(323), plot(t,xtrueA(:,3),'g',t,XestA(:,3),'r--');ylabel('x3')
subplot(324), plot(t,xtrueA(:,4),'g',t,XestA(:,4),'r--');ylabel('x4')
subplot(325), plot(t,xtrueA(:,5),'g',t,XestA(:,5),'r--');ylabel('x5')
subplot(326), plot(t,xtrueA(:,6),'g',t,XestA(:,6),'r--');ylabel('x6')
sgtitle('A: true state (green); KF (red)');

% mean square error
figure, plot(t,sqrt(errsumA),'g',t,sqrt(MSE_A),'r') 
title('A: square error (green: true; red: expected)')

% confidence intervals
figure
subplot(321), plot(t,errorsA(:,1),'g',t,-3*sqrt(PkfA(:,1)),'b--',t,3*sqrt(PkfA(:,1)),'b--');ylabel('x1')
subplot(322), plot(t,errorsA(:,2),'g',t,-3*sqrt(PkfA(:,2)),'b--',t,3*sqrt(PkfA(:,2)),'b--');ylabel('x2')
subplot(323), plot(t,errorsA(:,3),'g',t,-3*sqrt(PkfA(:,3)),'b--',t,3*sqrt(PkfA(:,3)),'b--');ylabel('x3')
subplot(324), plot(t,errorsA(:,4),'g',t,-3*sqrt(PkfA(:,4)),'b--',t,3*sqrt(PkfA(:,4)),'b--');ylabel('x4')
subplot(325), plot(t,errorsA(:,5),'g',t,-3*sqrt(PkfA(:,5)),'b--',t,3*sqrt(PkfA(:,5)),'b--');ylabel('x5')
subplot(326), plot(t,errorsA(:,6),'g',t,-3*sqrt(PkfA(:,6)),'b--',t,3*sqrt(PkfA(:,6)),'b--');ylabel('x6')
sgtitle('A: estimation errors (green); confidence intervals (blue)')


%%%%
% B
%%%%

%
% asymptotic Kalman filter
%
[Kas,Pinf,Pinf2,KFeig]=dlqe(A,eye(6),C,Q,R);

% initialization
xp=x0;
Pp=P0;

% recursion
for k=1:N,
        
    % correction
    x = xp + Kas*(yA(k,:)'-C*xp);
    
    % prediction
    xp = A*x + B*uA(k,:)';
    
    XestB(k,:)=x';
end

% state variables estimates
figure
subplot(321), plot(t,xtrueA(:,1),'g',t,XestA(:,1),'r--',t,XestB(:,1),'b-');ylabel('x1')
subplot(322), plot(t,xtrueA(:,2),'g',t,XestA(:,2),'r--',t,XestB(:,2),'b-');ylabel('x2')
subplot(323), plot(t,xtrueA(:,3),'g',t,XestA(:,3),'r--',t,XestB(:,3),'b-');ylabel('x3')
subplot(324), plot(t,xtrueA(:,4),'g',t,XestA(:,4),'r--',t,XestB(:,4),'b-');ylabel('x4')
subplot(325), plot(t,xtrueA(:,5),'g',t,XestA(:,5),'r--',t,XestB(:,5),'b-');ylabel('x5')
subplot(326), plot(t,xtrueA(:,6),'g',t,XestA(:,6),'r--',t,XestB(:,6),'b-');ylabel('x6')
sgtitle('B: true state (green); KF (red); asymptotic KF (blue)');


%%%%
% C
%%%%

%
% Kalman filter with fixed R matrix
%
R=300^2*eye(2);

% initialization
x0C = [yC(1,1)/3 yC(1,1)/3 yC(1,1)/3 yC(1,2)/3 yC(1,2)/3 yC(1,2)/3 ]';   % x(0|-1)
P0C = 10^2*eye(6); % P(0|-1)

xp=x0C;
Pp=P0C;

% recursion
for k=1:N,
        
    % correction
    K = Pp*C'*inv(C*Pp*C'+R);
    x = xp + K*(yC(k,:)'-C*xp);
    P = Pp*(eye(6)-C'*K');
    
    % prediction
    xp = A*x + B*uA(k,:)';
    Pp = A*P*A' + G*Q*G';
    
    XestC_fixedR(k,:)=x';
    PkfC_fixedR(k,:)=diag(P)';
    
end

%
% Kalman filter with time-varying R matrix (true)
%

% initialization
xp=x0C;
Pp=P0C;

% recursion
for k=1:N,
        
    % correction
    sv(k)=300*0.95^(k-1);
    R=sv(k)^2*eye(2);
    K = Pp*C'*inv(C*Pp*C'+R);
    x = xp + K*(yC(k,:)'-C*xp);
    P = Pp*(eye(6)-C'*K');
    
    % prediction
    xp = A*x + B*uA(k,:)';
    Pp = A*P*A' + G*Q*G';
    
    XestC_trueR(k,:)=x';
    PkfC_trueR(k,:)=diag(P)';
    
end

errorsC_fR=xtrueA-XestC_fixedR;
errsumC_fR=sum((errorsC_fR.^2)');
MSE_C_fR=sum(PkfC_fixedR');

errorsC_tR=xtrueA-XestC_trueR;
errsumC_tR=sum((errorsC_tR.^2)');
MSE_C_tR=sum(PkfC_trueR');

figure
subplot(321), plot(t,xtrueA(:,1),'g',t,XestC_trueR(:,1),'r--',t,XestC_fixedR(:,1),'b--'); ylabel('x1')
subplot(322), plot(t,xtrueA(:,2),'g',t,XestC_trueR(:,2),'r--',t,XestC_fixedR(:,2),'b--'); ylabel('x2')
subplot(323), plot(t,xtrueA(:,3),'g',t,XestC_trueR(:,3),'r--',t,XestC_fixedR(:,3),'b--'); ylabel('x3')
subplot(324), plot(t,xtrueA(:,4),'g',t,XestC_trueR(:,4),'r--',t,XestC_fixedR(:,4),'b--'); ylabel('x4')
subplot(325), plot(t,xtrueA(:,5),'g',t,XestC_trueR(:,5),'r--',t,XestC_fixedR(:,5),'b--'); ylabel('x5')
subplot(326), plot(t,xtrueA(:,6),'g',t,XestC_trueR(:,6),'r--',t,XestC_fixedR(:,6),'b--'); ylabel('x6')
sgtitle('C: true state (green); KF true R (red); KF fixed R (blue)');

figure, plot(t,sqrt(errsumC_fR),'b',t,sqrt(errsumC_tR),'g',t,sqrt(MSE_C_fR),'b--',t,sqrt(MSE_C_tR),'--')
title('C: error comparison (blue: fixed R; green: true R)')

% confidence intervals
figure
subplot(321), plot(t,errorsC_fR(:,1),'b',t,-3*sqrt(PkfC_fixedR(:,1)),'b--',t,3*sqrt(PkfC_fixedR(:,1)),'b--',t,errorsC_tR(:,1),'g',t,-3*sqrt(PkfC_trueR(:,1)),'g--',t,3*sqrt(PkfC_trueR(:,1)),'g--');ylabel('x1')
subplot(322), plot(t,errorsC_fR(:,2),'b',t,-3*sqrt(PkfC_fixedR(:,2)),'b--',t,3*sqrt(PkfC_fixedR(:,2)),'b--',t,errorsC_tR(:,2),'g',t,-3*sqrt(PkfC_trueR(:,2)),'g--',t,3*sqrt(PkfC_trueR(:,2)),'g--');ylabel('x2')
subplot(323), plot(t,errorsC_fR(:,3),'b',t,-3*sqrt(PkfC_fixedR(:,3)),'b--',t,3*sqrt(PkfC_fixedR(:,3)),'b--',t,errorsC_tR(:,3),'g',t,-3*sqrt(PkfC_trueR(:,3)),'g--',t,3*sqrt(PkfC_trueR(:,3)),'g--');ylabel('x3')
subplot(324), plot(t,errorsC_fR(:,4),'b',t,-3*sqrt(PkfC_fixedR(:,4)),'b--',t,3*sqrt(PkfC_fixedR(:,4)),'b--',t,errorsC_tR(:,4),'g',t,-3*sqrt(PkfC_trueR(:,4)),'g--',t,3*sqrt(PkfC_trueR(:,4)),'g--');ylabel('x4')
subplot(325), plot(t,errorsC_fR(:,5),'b',t,-3*sqrt(PkfC_fixedR(:,5)),'b--',t,3*sqrt(PkfC_fixedR(:,5)),'b--',t,errorsC_tR(:,5),'g',t,-3*sqrt(PkfC_trueR(:,5)),'g--',t,3*sqrt(PkfC_trueR(:,5)),'g--');ylabel('x5')
subplot(326), plot(t,errorsC_fR(:,6),'b',t,-3*sqrt(PkfC_fixedR(:,6)),'b--',t,3*sqrt(PkfC_fixedR(:,6)),'b--',t,errorsC_tR(:,6),'g',t,-3*sqrt(PkfC_trueR(:,6)),'g--',t,3*sqrt(PkfC_trueR(:,6)),'g--');ylabel('x6')
sgtitle('C: estimation errors (blue: fixed R; green: true R)')


%%%%
% D
%%%%


%
% Kalman filter with constant A matrix 
%

R=sv2*eye(2);

% initialization
xp=x0;
Pp=P0;

% recursion
for k=1:N,
        
    % correction
    K = Pp*C'*inv(C*Pp*C'+R);
    x = xp + K*(yD(k,:)'-C*xp);
    P = Pp*(eye(6)-C'*K');
    
    % prediction
    xp = A*x + B*uD(k,:)';
    Pp = A*P*A' + G*Q*G';
    
    XestD(k,:)=x';
    PkfD(k,:)=diag(P)';
    
end

errorsD=xtrueD-XestD;
errsumD=sum((errorsD.^2)');
MSE_D=sum(PkfD');

%
% Kalman filter with time-varying A matrix (true)
%

% initialization
xp=x0;
Pp=P0;

% recursion
for k=1:N,
        
    % correction
    K = Pp*C'*inv(C*Pp*C'+R);
    x = xp + K*(yD(k,:)'-C*xp);
    P = Pp*(eye(6)-C'*K');
    
    % prediction
    A(1,2)=0.45+0.3*sin(2*pi/20*(k-1));

    xp = A*x + B*uD(k,:)';
    Pp = A*P*A' + G*Q*G';
    
    XestD_trueA(k,:)=x';
    PkfD_trueA(k,:)=diag(P)';
    
end

errorsD_tA=xtrueD-XestD_trueA;
errsumD_tA=sum((errorsD_tA.^2)');
MSE_D_tA=sum(PkfD_trueA');

figure
subplot(321), plot(t,xtrueD(:,1),'g',t,XestD_trueA(:,1),'r--',t,XestD(:,1),'b--'); ylabel('x1')
subplot(322), plot(t,xtrueD(:,2),'g',t,XestD_trueA(:,2),'r--',t,XestD(:,2),'b--'); ylabel('x2')
subplot(323), plot(t,xtrueD(:,3),'g',t,XestD_trueA(:,3),'r--',t,XestD(:,3),'b--'); ylabel('x3')
subplot(324), plot(t,xtrueD(:,4),'g',t,XestD_trueA(:,4),'r--',t,XestD(:,4),'b--'); ylabel('x4')
subplot(325), plot(t,xtrueD(:,5),'g',t,XestD_trueA(:,5),'r--',t,XestD(:,5),'b--'); ylabel('x5')
subplot(326), plot(t,xtrueD(:,6),'g',t,XestD_trueA(:,6),'r--',t,XestD(:,6),'b--'); ylabel('x6')
sgtitle('D: true state (green); KF true A (red); KF fixed A (blue)');

figure, plot(t,sqrt(errsumD),'b',t,sqrt(errsumD_tA),'g',t,sqrt(MSE_D),'b--',t,sqrt(MSE_D_tA),'g--')
title('D: error comparison (blue: fixed A; green: true A)')

% confidence intervals
figure
subplot(321), plot(t,errorsD(:,1),'b',t,-3*sqrt(PkfD(:,1)),'b--',t,3*sqrt(PkfD(:,1)),'b--',t,errorsD_tA(:,1),'g',t,-3*sqrt(PkfD_trueA(:,1)),'g--',t,3*sqrt(PkfD_trueA(:,1)),'g--');ylabel('x1')
subplot(322), plot(t,errorsD(:,2),'b',t,-3*sqrt(PkfD(:,2)),'b--',t,3*sqrt(PkfD(:,2)),'b--',t,errorsD_tA(:,2),'g',t,-3*sqrt(PkfD_trueA(:,2)),'g--',t,3*sqrt(PkfD_trueA(:,2)),'g--');ylabel('x2')
subplot(323), plot(t,errorsD(:,3),'b',t,-3*sqrt(PkfD(:,3)),'b--',t,3*sqrt(PkfD(:,3)),'b--',t,errorsD_tA(:,3),'g',t,-3*sqrt(PkfD_trueA(:,3)),'g--',t,3*sqrt(PkfD_trueA(:,3)),'g--');ylabel('x3')
subplot(324), plot(t,errorsD(:,4),'b',t,-3*sqrt(PkfD(:,4)),'b--',t,3*sqrt(PkfD(:,4)),'b--',t,errorsD_tA(:,4),'g',t,-3*sqrt(PkfD_trueA(:,4)),'g--',t,3*sqrt(PkfD_trueA(:,4)),'g--');ylabel('x4')
subplot(325), plot(t,errorsD(:,5),'b',t,-3*sqrt(PkfD(:,5)),'b--',t,3*sqrt(PkfD(:,5)),'b--',t,errorsD_tA(:,5),'g',t,-3*sqrt(PkfD_trueA(:,5)),'g--',t,3*sqrt(PkfD_trueA(:,5)),'g--');ylabel('x5')
subplot(326), plot(t,errorsD(:,6),'b',t,-3*sqrt(PkfD(:,6)),'b--',t,3*sqrt(PkfD(:,6)),'b--',t,errorsD_tA(:,6),'g',t,-3*sqrt(PkfD_trueA(:,6)),'g--',t,3*sqrt(PkfD_trueA(:,6)),'g--');ylabel('x6')
sgtitle('D: estimation errors (blue: fixed A; green: true A)')

