% 20-stage RC Filter
% Lecture #30: Kalman Filter

V = zeros(20,1);
dV = 0*V;

dt = 0.01;
t = 0;
V0 = 0;

A = zeros(20,20);
for i=1:19
    A(i,i) = -50;
    A(i,i+1) = 25;
    A(i+1,i) = 25;
end
A(20,20) = -25;
B = zeros(20,1);
B(1) = 25;
C = zeros(1,20);
C(20) = 1;
D = 0;

% Observer

Ae = [-2,1,0,0;1,-2,1,0;0,1,-2,1;0,0,1,-1];
Be = [1;0;0;0];
Ce = [0,0,0,1];

% Kalman Filter
F = Be;
v = 2;   % input noise
w = 0.01;   % sensor noise

Q = (F*v) * (F*v)';
R = w*w;
H = lqr(Ae', Ce', Q, R)'
Xe = zeros(4,1);

Ref = 0;
n=0;
DATA = [];

while(t < 9.9)
   
   V0 = 2*sin(t);
   
   dV = A*V + B*(V0 + v*randn);
   y = V(20) + w*randn;
   
   dXe = Ae*Xe + Be*V0 + H*(y - Ce*Xe);
   
   V = V + dV * dt;
   Xe = Xe + dXe * dt;
   t = t + dt;

   n = mod(n+1, 5);
   if(n == 0)
   plot([0:20], [V0;V], 'b.-', [0:5:20],[V0;Xe],'m.-',t, -1, 'b+',19.8,Ref,'b+');
   ylim([-3,3]);
   xlim([0,20]);
   pause(0.01);
   end
   DATA = [DATA ; V([5,10,15,20])', Xe'];  
end

t = [1:length(DATA)]' * dt;
plot(t,DATA)