function [xhatplus,Pplus,xhatminus,Pminus] = sect_4_4_kfilter_z(Phi, H, Q, Psi, M, 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.
% 
%-----

[n,N] = size(z);

% Storage for our results: 
%
xhatminus = zeros(2,N); % holds \hat{x}_1(-), \hat{x}_2(-), \cdots, \hat{x}_N(-) in the columns
Pminus = zeros(2,2,N);
xhatplus = zeros(2,N-1); % holds \hat{x}_1(+), hat{x}_2(+), \cdots \hat{x}_{N-1}(+) (one fewer spot needed) in the columns
Pplus = zeros(2,2,N-1);

% Seed the storage above with the correct initial conditions
%
xhatminus(:,1) = [0;0];
Pminus(:,:,1) = zeros(2,2); 

for k=2:N,
  zeta_km1 = z(:,k) - Psi * z(:,k-1);
  D_km1 = H * Phi - Psi * H;
  DPDTInv = ( D_km1 * Pminus(:,:,k-1) * D_km1' + R ) \ eye(n);
  K_km1 = Pminus(:,:,k-1) * D_km1' * DPDTInv; 
  C_km1 = M * DPDTInv;
  xhatplus(:,k-1) = xhatminus(:,k-1) + K_km1 * ( zeta_km1 - D_km1 * xhatminus(:,k-1) );
  xhatminus(:,k) = Phi * xhatplus(:,k-1) + C_km1 * ( zeta_km1 - D_km1 * xhatplus(:,k-1) );
  Pplus(:,:,k-1) = ( eye(n) - K_km1 * D_km1 ) * Pminus(:,:,k-1) * (( eye(n) - K_km1 * D_km1 ).') + K_km1 * R * K_km1.';
  Pminus(:,:,k) = Phi * Pplus(:,:,k-1) * Phi.' + Q - C_km1 * M.' - Phi * K_km1 * M - M.' * K_km1.' * Phi.';
end