%   
% Written by:
% -- 
% John L. Weatherwax                2006-08-28
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

close all; clear; clc;

Phi = [ 0.9, 0.1, 0 ; 0, 0.8, 0.1; 0, 0, 0.75 ];
n   = size(Phi,1);

H = [ 1, 0, 0; 0, 1, 0 ];
Q = eye(n);

r = size(H,1);
R = eye(r);

P = [20, 10, 3; 10, 6, 2; 3, 2, 1 ];

% Compute the updated P(+) matrix using sequential filtering: 
%
K  = zeros(n,r);
Pp = P; % initial condition for P(+)
for ri=1:r, % for each row of H
  hrow    = H(ri,:);
  num     = Pp * hrow'; % a column
  den     = hrow * Pp * hrow' + R(ri,ri); % a scalar 
  K(:,ri) = num/den; % put this column into a matrix 
  Pp      = ( eye(n) - K(:,ri) * hrow ) * Pp;
end

% print everything: 
%
K

Pp