clear;
close all;

%% Data
load data_kf_dcmotor


%% Model 
% Inertia
J=0.01; % Kg m^2 /s^2

% Friction coefficient
b=0.1; % N m s

% Field gain
Km=0.01; % N m /A

% Circuit resistance
Ra=1; % Ohm

% Circuit inductance
La=1; % H

% Sampling time
Tc=0.01;

% System matrices
A=[1-Tc*b/J Tc*Km/J; -Tc*Km/La 1-Tc*Ra/La];
B=[0 Tc/La]';
C=[1 0];
G=B;

% Time horizon
N=length(Y);
Tend = (N-1)*Tc;
T=[0:Tc:Tend]';

% Covariances
Q=1; 
R=0.01;

%% 1) KF  
% Initialization
Xinit=[0.5 0.5]';
Pinit=eye(2);

Xhat=zeros(N,2);
Pdiag=zeros(N,2);
Pp=Pinit;
Xp=Xinit;

for k=1:N
    % correction
    K=Pp*C'*inv(C*Pp*C'+R);
    P=Pp-K*C*Pp;
    Xc=Xp+K*(Y(k)-C*Xp);

    % prediction
    Xp=A*Xc+B*U(k);
    Pp=A*P*A'+G*Q*G';
 
    Xhat(k,:)=Xc';  
    Pdiag(k,:)=diag(P)';
end

figure
subplot(2,1,1)
hold on
plot(T,180/pi*X(:,1));
plot(T,180/pi*Xhat(:,1),'r');
plot(T,Y*180/pi,'g');
title('angular velocity (deg/sec): true (blue), measured (green), estimated (red)')
subplot(2,1,2)
hold on
plot(T,X(:,2));
plot(T,Xhat(:,2),'r');
title('current (A): true (blue), estimated (red)')

figure
subplot(2,1,1)
hold on
plot(T,180/pi*(X(:,1)-Xhat(:,1)),'b');
plot(T,180/pi*3*sqrt(Pdiag(:,1)),'r',T,-180/pi*3*sqrt(Pdiag(:,1)),'r');
title('angular velocity estimation error (blue) and 3-sigma confidence intervals (red)')
subplot(2,1,2)
hold on
plot(T,X(:,2)-Xhat(:,2),'b');
plot(T,3*sqrt(Pdiag(:,2)),'r',T,-3*sqrt(Pdiag(:,2)),'r');
title('current estimation error (blue) and 3-sigma confidence intervals (red)')

%mean square error
RMSE=[norm(X(:,1)-Xhat(:,1)) norm(X(:,2)-Xhat(:,2))]/sqrt(N+1)

%% 2) KF with biased measurements

% KF with bias sensor estimate
% Initialization
Xhat=zeros(N,3);
Pdiag=zeros(N,3);
Pp=blkdiag(Pinit,(20/180*pi)^2);
Xp=[Xinit; 0];

Ae=blkdiag(A,1);
Be=[B;0];
Ce=[C 1];
Ge=[G zeros(2,1); 0 1];
sigmab=1e-10;
Qe=blkdiag(Q,sigmab);

for k=1:N
    % correction
    K=Pp*Ce'*inv(Ce*Pp*Ce'+R);
    P=Pp-K*Ce*Pp;
    Xc=Xp+K*(Ybias(k)-Ce*Xp);

    % prediction
    Xp=Ae*Xc+Be*U(k);
    Pp=Ae*P*Ae'+Ge*Qe*Ge';
 
    Xhat(k,:)=Xc';  
    Pdiag(k,:)=diag(P)';
end

% standard KF (without sensor estimate)
Xhatr=zeros(N,2);
Pdiagr=zeros(N,2);
Ppr=Pinit;
Xpr=Xinit;

for k=1:N
    % correction 
    K=Ppr*C'*inv(C*Ppr*C'+R);
    Pr=Ppr-K*C*Ppr;
    Xcr=Xpr+K*(Ybias(k)-C*Xpr);

    % prediction
    Xpr=A*Xcr+B*U(k);
    Ppr=A*Pr*A'+G*Q*G';
 
    Xhatr(k,:)=Xcr';  
    Pdiagr(k,:)=diag(Pr)';
end

figure
subplot(3,1,1)
hold on
plot(T,180/pi*X(:,1));
plot(T,180/pi*Xhat(:,1),'r');
plot(T,180/pi*Xhatr(:,1),'c');
%plot(T,Ynb*180/pi,'g');
title('angular velocity (deg/sec): true (blue), KF with bias estimate (red), standard KF (cyan)')
subplot(3,1,2)
hold on
plot(T,X(:,2));
plot(T,Xhat(:,2),'r');
plot(T,Xhatr(:,2),'c');
title('current (A): true (blue), KF with bias estimate (red), standard KF (cyan)')
subplot(3,1,3)
hold on
plot(T,bias*ones(size(T)));
plot(T,Xhat(:,3),'r');
title('bias: true (blue), estimated (red)')

figure
subplot(3,1,1)
hold on
plot(T,180/pi*(X(:,1)-Xhat(:,1)),'b');
plot(T,180/pi*3*sqrt(Pdiag(:,1)),'r',T,-180/pi*3*sqrt(Pdiag(:,1)),'r');
plot(T,180/pi*(X(:,1)-Xhatr(:,1)),'b--');
plot(T,180/pi*3*sqrt(Pdiagr(:,1)),'r--',T,-180/pi*3*sqrt(Pdiagr(:,1)),'r--');
title({'angular velocity estimation error (blue) and 3-sigma confidence intervals (red):'},{'KF with (solid) and without (dashed) bias estimate'})
subplot(3,1,2)
hold on
plot(T,X(:,2)-Xhat(:,2),'b');
plot(T,3*sqrt(Pdiag(:,2)),'r',T,-3*sqrt(Pdiag(:,2)),'r');
plot(T,X(:,2)-Xhatr(:,2),'b--');
plot(T,3*sqrt(Pdiagr(:,2)),'r--',T,-3*sqrt(Pdiagr(:,2)),'r--');
title({'current estimation error (blue) and 3-sigma confidence intervals (red):'},{'KF with (solid) and without (dashed) bias estimate'})
subplot(3,1,3)
hold on
plot(T,bias*ones(size(T))-Xhat(:,3),'b');
plot(T,3*sqrt(Pdiag(:,3)),'r',T,-3*sqrt(Pdiag(:,3)),'r');
title('bias estimation error (blue) and 3-sigma confidence intervals (red)')

%mean square error
RMSE=[norm(X(:,1)-Xhat(:,1)) norm(X(:,2)-Xhat(:,2))]/sqrt(N+1)
RMSEbiased=[norm(X(:,1)-Xhatr(:,1)) norm(X(:,2)-Xhatr(:,2))]/sqrt(N+1)


