%----- KALMANUV FILTR
randn('state',0);%reset generatoru nahodnych cisel

%----- Soustava
% Defining system
Ac = [0 1 0 0; 0 0 1 0; 0 0 0 1;-10e-11 -24010 -2539 -54];
Bc = [0 0 0 31940]';
Cc = [1 0 0 0];
Dc = 0;
Ts = 0.01;

[Ad, Bd, Cd, Dd] = ssdata(c2d(ss(Ac,Bc,Cc,Dc),Ts));
Ns = size(Ad,1);
Ny = size(Cd,1);

%----- Kovariancni matice
Qd = diag([0.01, 0.01, 0.1, 0.1]);
Rd = 0.05;
Pd = Qd;%pocatecni hodnota kovariancni matice

%----- Pocatecni podminky
xs = zeros(Ns,1);
xp = zeros(Ns,1);

%----- Inicializace historie
N = 300;
uh = [1*ones(1,N)];
xsh = zeros(Ns,N);
xph = zeros(Ns,N);
Lh = zeros(Ns,N);
Kh = zeros(Ns,N);
eh = zeros(1,N);
ysh = zeros(1,N);
yph = zeros(1,N);

%----- Simulace
for t = 1 : N
   v = chol(Qd)*randn(Ns,1);
   e = chol(Rd)*randn(1,1);
   u = uh(t);
   ys = Cd*xs + Dd*u + e;%vystup systemu ys(k)

%----- Datovy krok KF
	yp = Cd*xp + Dd*u;%odhad vystupu yp(k|k-1)
	ep = ys - yp;%chyba odhadu vystupu e(k|k-1)=ys(k)-yp(k|k-1)
	L = Pd*Cd'*inv(Cd*Pd*Cd'+Rd);%Kalmanovo zesileni
	Pd = Pd - L*Cd*Pd;%kovariancni matice P(k|k)
	xp = xp + L*ep;%odhad stavu x(k|k)
	yp = Cd*xp + Dd*u;%odhad vystupu y(k|k)
%----- Ulozeni dat
	eh(t) = ep; xsh(:,t) = xs; xph(:,t) = xp; ysh(t) = ys;
	yph(t) = yp; Lh(:,t) = L; Kh(:,t) = Ad*L;
	xs = Ad*xs + Bd*u + v;%stav systemu x(k)
%----- Casovy krok KF
	xp = Ad*xp + Bd*u;%odhad stavu x(k+1|k)
	Pd = Ad*Pd*Ad' + Qd;%kovariancni matice P(k+1|k)
end

%-----Frekcencni vlastnosti invariantniho KF
K = Ad*Lh(:,end);
Ak = Ad - K*Cd;
Bk = [Bd - K*Dd,K];
Ck = eye(Ns);
Dk = zeros(Ns,2);
[NumKU DenK] = ss2tf(Ak,Bk,Ck,Dk,1);%prenos U -> odhad X
[NumKY DenK] = ss2tf(Ak,Bk,Ck,Dk,2);%prenos Y -> odhad X
figure(Ns+3);
bode(tf(NumKU(1,:),DenK,Ts));
title('Prenos U -> odhad X 1');
figure(Ns+4);
bode(tf(NumKY(1,:),DenK,Ts));
title('Prenos Y -> odhad X 1');