function [xhatminus,pminus,xhatplus,pplus] = sect_6_2_extended_kf(x_0, P_0, Phi, Gamma, u_k, Q, R, z_k, t_k, ref_pt_1, ref_pt_2)
%
% Inputs: 
%
% 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(t_k)-1; % the number of measurements (not including the initial t_0 point)
n = length(x_0); % the state dimension

% Compute our state estimate using the linear filter:
xhatplus      = zeros(n,N+1);
pplus         = zeros(n,n,N+1);
xhatplus(:,1) = x_0; % the initial condtion on the state
pplus(:,:,1)  = P_0;
xhatminus     = zeros(n,N);
pminus        = zeros(n,n,N);
for ki = 1:N
  % state covariance extrapolation over t_{k-1} < t < t_{k}:
  % 
  xhatminus(:,ki) = Phi * xhatplus(:,ki) + Gamma * u_k;
  pminus(:,:,ki)  = Phi * pplus(:,:,ki) * (Phi') + Q;
  
  [h_k,H_k] = sect_6_2_h_fn(xhatminus(:,ki), ref_pt_1, ref_pt_2);
  
  K_k = pminus(:,:,ki) * (H_k') * ( ( H_k * pminus(:,:,ki) * H_k' + R ) \ eye(n) );
  
  % state covariance update due to measurement:
  %
  xhatplus(:,ki+1) = xhatminus(:,ki) + K_k * ( z_k(:,ki) - h_k );
  pplus(:,:,ki+1)  = ( eye(n) - K_k * H_k ) * pminus(:,:,ki); 
end