% kalman.m - Kalman filter simulation
% (from Robbie Jacobs example)

% state estimate noise variance
q2=0.1^2;
% observation noise variance
r2=0.5^2;

% P(x_0)
sigma2=0.1^2;
xhat=0;

% drift velocity
v=0.1;

% plot P(x_0)
x=-1:0.01:10;
Px=exp(-0.5*((x-xhat).^2/sigma2)); Px=Px/sum(Px);
h=plot(x,Px); axis([-1 10 0 max(Px)])
drawnow

t=1;

while (1)
    
    % prediction: P(x_t|y_1....y_t-1) 
    xhat=xhat+v;
    sigma2=sigma2+q2;
    
    % new observation?
    y=input('y=');
    if ~isempty(y)
        % Kalman gain
        K=sigma2/(sigma2+r2);
        % update:  P(x_t|y_1....y_t)
        xhat=xhat+K*(y-xhat);  
        sigma2=(1-K)*sigma2;
    end
    
    % plot P(x_t|y_1....y_t)
    Px=exp(-0.5*((x-xhat).^2/sigma2)); Px=Px/sum(Px);
    set(h,'YData',Px)
    drawnow

    t=t+1;
    
end
