%**************************************************************************
%*                                                                        *
%*  Rozszerzony Filtr Kalmana - odtwarzanie predkosci silnika klatkowego  *
%*             (zakladamy znajomosc momentu bezwladnosci)                 *                                                   *
%*                                                                        *
%*                                       Bartlomiej Ufnalski, 09.03.2017  *
%*                                                                        *
%**************************************************************************
%
% Kolejnosc uruchamiania plikow:
% 1. run_me_DRFOC_for_EKF_end_ELO.slx
% 2. EKF_speed_known_J.m --> algorytm rozszerzonego filtru Kalmana (EKF)
%
% For some details please consult http://ufnalski.edu.pl/dissertation/
%
% Parametry silnika Pn = 1.5 kW
a0=1/(Ls*Lr-Lm*Lm);
a1=1-a0*Rs*Lr*Ts;
a2=a0*Lm*Lm*Ts;
a3=a0*Lm*Ts;
a4=Lm*Ts;
a5=a0*Lr*Ts;
a6=3*pb*Lm*Ts/2/J_tot/Lr;
talr=Lr/Rr;
clear x_e x
x_e(:,1)=[0 0 0 0 0]';
P=diag([0.01 0.01 0.01 0.01 10]);
clear uu
uu(1,:)=usa.signals.values';
uu(2,:)=usb.signals.values';
x(:,1)=[0 0 0 0 0]';
% Macierz kowariancji szumu systemowego dla wektora zmiennych stanu
%                                      [isa isb psira psirb omega]
Q=diag([system_is_noise_PSD system_is_noise_PSD system_psir_noise_PSD system_psir_noise_PSD system_omega_noise_PSD*1e5]/Ts)*Ts^2;
% Macierz kowariancji szumu pomiarowego dla wektora [isa isb]
R=diag([noise_psd_I noise_psd_I]/Ts);
%
% ROZSZERZONY FILTR KALMANA
for k=2:round(numel(usa.signals.values)),
    % Measurement
    zz(1,k)=isa.signals.values(k);
    zz(2,k)=isb.signals.values(k);
    % Discrete Kalman filter time update equations - PREDICT
    x_e(1,k)=(a1-a2/talr)*x_e(1,k-1)+a3/talr*x_e(3,k-1)+a3*x_e(4,k-1)*x_e(5,k-1)+a5*uu(1,k-1);%isa
    x_e(2,k)=(a1-a2/talr)*x_e(2,k-1)+a3/talr*x_e(4,k-1)-a3*x_e(3,k-1)*x_e(5,k-1)+a5*uu(2,k-1);%isb
    x_e(3,k)=a4/talr*x_e(1,k-1)+(1-Ts/talr)*x_e(3,k-1)-Ts*x_e(5,k-1)*x_e(4,k-1);%psira
    x_e(4,k)=a4/talr*x_e(2,k-1)+(1-Ts/talr)*x_e(4,k-1)+Ts*x_e(5,k-1)*x_e(3,k-1);%psirb
    % zakladam znajomosci momentu bezwladnosci, dlatego wyzej zakladam mala wariancje prekosci wirnika w Q
    % przyjmujac var(omega)=system_omega_noise_PSD/Ts filtr nie rozsypuje sie --> porownaj z ekf_silnik.m
    x_e(5,k)=x_e(5,k-1)+a6*(x_e(2,k-1)*x_e(3,k-1)-x_e(1,k-1)*x_e(4,k-1));% omega; znajomosc momentu bezwladnosci
    %
    A=[ (a1-a2/talr)    0               a3/talr         a3*x_e(5,k-1)   a3*x_e(4,k-1);
        0               (a1-a2/talr)    -a3*x_e(5,k-1)  a3/talr         -a3*x_e(3,k-1);
        a4/talr         0               (1-Ts/talr)     -Ts*x_e(5,k-1)  -Ts*x_e(4,k-1);
        0               a4/talr         Ts*x_e(5,k-1)   (1-Ts/talr)     Ts*x_e(3,k-1);
        -a6*x_e(4,k-1)  a6*x_e(3,k-1)   a6*x_e(2,k-1)   -a6*x_e(1,k-1)  1];
    W=eye(5,5);
    P=A*P*A'+W*Q*W';
    % Discrete Kalman filter measurement update equation - CORRECT
    H=[ 1 0 ;
        0 1 ;
        0 0 ;
        0 0 ;
        0 0 ]';
    V=[1 0; 0 1];
    K=P*H'*(H*P*H'+V*R*V')^(-1); % Wzmocnienie Kalmana
    z_e(1,k)=x_e(1,k);
    z_e(2,k)=x_e(2,k);
    x_e(:,k)=x_e(:,k)+K*(zz(:,k)-z_e(:,k));
    P=(eye(5)-K*H)*P;
end

% Wykresy
try close(1), catch wiad, end;
figure(1);
plot(omega.time,omega.signals.values,omega.time,x_e(5,:)/pb)
ylabel('\omega_{est} [rad/s]')
xlabel('time [s]')
grid
legend('rotor speed','estimated speed (EKF, inertia known)','Location','best');
% The end