%   
% 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 ];

hphtpr_inv = ( H * P * H' + R ) \ eye(r);
K = P * (H') * hphtpr_inv

% Compute the updated P(+) matrix using the Joseph form:
%
Pp = ( eye(n) - K * H ) * P * ( ( eye(n) - K * H )' ) + K * R * K'