%***************************************************************
%  This mfile implements the discrete Kalman filter (DKF)
%  The underlying state-model is assumed stationary
%  Author: Balu Santhanam
%  Date  : 04/13/2000
%  SYNOPSIS :
%  [X,K,MMSE] = dkf(sig,Phi,Q,R,C,X_0,P_0)
%  sig   : Observation process 
%  X     : DKF State estimate (pxlength(sig))
%  K     : Kalman gain matrix (pxlength(sig))
%  Phi   : State Transition Matrix (pxp)
%  C     : Output-state Gain matrix (1xp)
%  Q     : Covariance of Model Noise (pxp)
%  R     : Covariance of Measurement Noise 
%  X_0   : Initial state estimate 
%  P_0   : Initial error covariance    
%***************************************************************
function [X,K,MMSE] = dkf(sig,Phi,Q,R,C,X_0,P_0)
format long
N = length(sig); [p,q] = size(Phi); X = []; K = [];
% Error control
if nargin < 5
   error('Insufficient Input Information');
elseif nargin == 5
   X_0 = zeros(p,1);
   P_0 = 100*eye(p); 
end;
xmin = X_0; Pmin = P_0;
%  Kalman Filtering Algorithm
for i = 1:1:N
%   Compute optimal Kalman gain Kal
%   needed for updating the state estimate  
%   based on previous state estimate and 
%   present data
    Kal = Pmin*C.'*inv(C*Pmin*C.' + R);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   State update using new data
    xplus = xmin + Kal*(sig(i) - C*xmin);
%   State prediction from previous state
    xmin = Phi*xplus;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Error covariance update after new data
    Pplus = (eye(p,p) - Kal*C)*Pmin;
%   Error covariance of predicted state
    Pmin = Phi*Pplus*Phi.' + Q;
    MMSE(i) = 10*log10(abs(trace(Pplus)));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    X = [X, xplus];
    K = [K, Kal];
end

