function [xhatplus,Pplus,xhatminus,Pminus] = sect_4_3_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 = length(z);

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

% Seed the storage above with the correct initial conditions
%
xhatminus(1) = 0;
Pminus(1) = 0; 

for k=2:N,
  zeta_km1 = z(k) - Psi * z(k-1);
  D_km1 = H * Phi - Psi * H;
  K_km1 = Pminus(k-1) * D_km1 / ( D_km1 * Pminus(k-1) * D_km1 + R );
  C_km1 = M / ( D_km1 * Pminus(k-1) * D_km1 + R );
  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) = ( 1 - K_km1 * D_km1 ) * Pminus(k-1) * ( 1 - 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