Basic_KF.m

function [x_out, time_tot] = Basic_KF(varargin)

% [x_kalman, time_tot] = Basic_KF(y, G, F, dyn_var, obs_var);
% 
% This function runs the standard Kalman Filter on the set of measurements
% y taken by the linear operators in G to infer the dynamic signal
% approximately generated by the dynamics F
% 
%   The inputs are:
% 
% y:       MxT matrix of measurement vectors at each time step
% G:       MxNxT array of measurement matrices at each iteration
% F:       MxMxT array of dynamics matrices for each iteration
% dyn_var: Scalar value of the innovations variance (or approximate)
% obs_var: Scalar variance of the measurement noise
% 
%    The outputs are:
% x_out:      NxT matrix with the estimates of the signal x
% time_tot:   total runtime
% 
%
% Code by Adam Charles, 
% Department of Electrical and Computer Engineering,
% Georgia Institute of Technology
% 
% Last updated August 14, 2012. 
% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Input Parsing
if nargin > 1
    y = varargin{1};
    G = varargin{2};
else
    error('Bad (too few) number of inputs!')
end
if nargin > 2
    F = varargin{3};
else
    F = eye(size(G, 2));
end

if nargin > 3
    dyn_var = varargin{4};
else
    dyn_var = 0.01;
end

if nargin > 4
    obs_var = varargin{5};
else
    obs_var = 0.01;
end

if nargin > 5
    error('Bad (too many) number of inputs!')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Initializations
y_dim = size(G, 1);
x_dim = size(F, 1);
num_iters = size(y, 2) - 1;

x_out = zeros(x_dim, num_iters+1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Run Algorithm
tic;
% Initial Point
x_out(:, 1) = pinv(G(:, :, 1))*y(:, 1);
P = zeros(x_dim);
R = obs_var*eye(y_dim);
for ii = 1:num_iters
    x_pred = F(:, :, ii+1)*x_out(:, ii); % Predict state
    temp = zeros(size(x_pred));
    temp(x_pred ~= 0) = dyn_var;
    Q = diag(temp);
    % Predict variance
    P_pred = F(:, :, ii+1)*P*(F(:, :, ii+1).') + Q;
    % Calcualte Kalman Gain
    K = P_pred*(G(:, :, ii+1).')*pinv(R + G(:, :, ii+1)*P_pred*(G(:, :, ii+1).'));
    % Update state estimation
    x_out(:, ii+1) = x_pred + K*(y(:, ii+1) - G(:, :, ii+1)*x_pred);
    % Update state covariance
    P = P_pred - K*G(:, :, ii+1)*P_pred;
end
time_tot = toc;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%