function [s, x_estimate] = kalman(x,z,Q,R,F,G,H)
% Author - Saurabh Prasad
% Date last modified - October 30, 2005
%-------------------------------------------------------------------------
%Function to estimate the state vector, given the previous state estimate
%(x),the current observation (z), an estimate of the Process and 
%Measurement covariance matrices (Q and R respectively), and the state
%space model (F,G,H). The corresponding differential equations being:
%  x(k+1) = F*x(k) + v(k)
%  z(k)   = s(k)   + n(k)
% where s(k) is the clean signal estimate, given by H*x(k).
%-------------------------------------------------------------------------

p = length(x);
P_old = eye(p,p);

% The following initializations are for debugging purposes:
% p = 3;
% x = normrnd(0,1,100,1);
% F = [0.1 2 0.1; 0 1 0.2; 0 0 0.3];
% z = x(length(x));
% Q = [1.1 2 2.1; 2 1 0.2; 5 0 0.3];
% R = 2;% [1.1 2 2.1; 2 1 0.2; 5 0 0.3];
% G = zeros(1,p);
% G(1,p) = 1;
% G = G';
% H = G';

X_estimate = x(1:p);

% Predict the state vector and the error covariance matrix using
% previous estimate of state vector and the error covariance matrix
X_predict = F*X_estimate + (G*normrnd(0,1));
P_new = F*P_old*F' + Q;    

% Computing the Kalman Gain:          
K = P_new*H'*inv(H*P_new*H'+ R);

% Correcting the observation using the innovations process to generate
% the 'filtered' estimate
e = z-H*X_predict;
X_estimate = X_predict + (K*e);
P_old = (eye(p,p)-K*H)*P_new;    

% Filtering the observation, z 
s = H*X_estimate;

x_estimate = X_estimate;