%**************************************************************************
%*                                                                        *
%*  Rozszerzony Filtr Kalmana - odtwarzanie predkosci silnika klatkowego  *
%*                                                                        *
%*                                       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
% przyjmujac var(omega)=system_omega_noise_PSD/Ts filtr nie rozsypuje sie --> porownaj z ekf_silnik.m
%
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
% The end