%
%   This script illustrates estimating the state of a 
%    Brownian motion from discrete, noisy measurements
%    by a Kalman filter
%
%   Exercise 7 in Matssyst Gk 2002
%
clear all, format compact, figure(1)
randn('state',0);

%
%   A realization of the Brownian motion and some noisy measurements
%
T = 5;
n = 100;
dim = 2;
dt = 1/n;
tvec1 = 0:dt:T;
tvec2 = 0:T;
dx = sqrt(dt)*randn(dim,n*T);
x = cumsum(dx')';

if dim == 1
    sigma = .2
    psteady = 1/2 + sqrt(sigma^2+1/4);
    ksteady = psteady/(psteady+sigma^2);
else
    sigma = diag([1/10 1]);
end

%   Measurement
y = x(:,n:n:length(x)) + sigma*randn(dim,T);
x = [ zeros(dim,1) x ];

%
%   Implement Kalman Filter
%

A = eye(dim);
B = eye(dim);
C = eye(dim);
D = sigma*eye(dim);

Theta = D*D';
Xi = B*B';
P{1} = zeros(dim);
xhat = zeros(dim,1);

for k = 1:length(y)
 
    K(:,:,k) = A*P{k}*C'*inv(C*P{k}*C'+Theta);
 
    P{k+1} = A*(P{k}-P{k}*C'*inv(C*P{k}*C'+Theta)*C*P{k})*A'+Xi;
    
    xhat(:,k+1) = A*xhat(:,k) + K(:,:,k)*(y(:,k)-C*xhat(:,k));
    
end


if dim == 1
    subplot(2,1,1)
    if sigma == .5
        hold on
        plot(0:dt:T,x','k-')
        plot(1:T,y,'ko--')
        plot(0:T,xhat,'k*:')
        legend('True','Measurement','Estimate')
        title('Brownian motion estimate by Kalman filter')
    end
    subplot(2,1,2)
    plot(squeeze(K),'k'), hold on
    plot(ones(1,T)*ksteady,'k:')
    title('Kalman gain')
else
    hold on
    plot(x(1,:),x(2,:),'k-')
    plot(y(1,:),y(2,:),'ko--')
    plot(xhat(1,:),xhat(2,:),'k*:')
    legend('True','Measurement','Estimate')
    title('Brownian motion estimate by Kalman filter')
    axis equal
end

orient tall
myfontsize = 16;
myfontweight = 'bold';
changetextstyle(gcf,myfontsize,myfontweight);
