function [xhatplus,Pplus,xhatminus,Pminus] = sect_4_5_kfilter_z(Phi, H, Q, R, z)
%
% Written by:
% -- 
% John L. Weatherwax                2005-08-14
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

[r,N] = size(z);
[n,n] = size(Phi);

% Storage for our results: 
%
xhatplus = zeros(n,N+1); % holds \hat{x}_0(+), \hat{x}_1(+), hat{x}_2(+), \cdots \hat{x}_{N}(+) in columns 
Pplus = zeros(n,n,N+1);

xhatminus = zeros(n,N); % holds \hat{x}_1(-), \hat{x}_2(-), \cdots, \hat{x}_N(-) in the columns
Pminus = zeros(n,n,N);

% Seed the storage above with the correct initial conditions
%
xhatplus(:,1) = zeros(n,1);
Pplus(:,:,1) = zeros(n,n);

for k=1:N,
  xhatminus(:,k) = Phi * xhatplus(:,k);
  Pminus(:,:,k) = Phi * Pplus(:,:,k) * Phi.' + Q;
  HPHP = H * Pminus(:,:,k) * H' + R;
  K_k = Pminus(:,:,k) * H' * ( HPHP \ eye(r) ); 
  xhatplus(:,k+1) = xhatminus(:,k) + K_k * ( z(:,k) - H * xhatminus(:,k) );
  Pplus(:,:,k+1) = ( eye(n) - K_k * H ) * Pminus(:,:,k) * (( eye(n) - K_k * H ).') + K_k * R * K_k.';
end