function [xhatplus,Pplus,xhatminus,Pminus] = sect_4_2_kfilter_z(Phi, H, Q, 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);

% Stores \hat{x}_k(+) and P_k(+) for k=0,1,2, \cdots N in that order
xhatplus = zeros(1,N+1);
Pplus = zeros(1,N+1);

% Stores \hat{x}_k(-) and P_k(-) for k=1,2, \cdots N in that order
xhatminus = zeros(1,N);
Pminus = zeros(1,N);

% Put in initial conditions:
xhatplus(1) = 0.;
Pplus(1) = 0.; 

for k=1:N,
  % propagation step: 
  xhatminus(k) = Phi * xhatplus(k);
  Pminus(k) = Phi * Pplus(k) * Phi + Q; 

  % Kalman gain
  K_k = ( Pminus(k) + M )/( Pminus(k) + 2 * M + R );

  % update state
  xhatplus(k+1) = xhatminus(k) + K_k * ( z(k) - H* xhatminus(k) );
  Pplus(k+1) = Pminus(k) - K_k * ( M + Pminus(k) );
end