function [xhatminus,pminus,xhatplus,pplus] = sect_6_1_extended_kf(k_1, k_2, k_3u, mu_q, var_q, R, z_k, t_k)
%
% 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;

% Compute our state estimate using the linear filter:
xhatplus = zeros(1,N+1);
pplus    = zeros(1,N+1);
xhatplus(1) = 0; % the initial condtion on the state
pplus(1)    = 0; 
xhatminus   = zeros(1,N);
pminus      = zeros(1,N);
for ki = 1:N
  
  % integrate our nonlinear equations from t_{k-1} < t < t_k: 
  y0          = [ xhatplus(ki), pplus(ki) ];
  [tout,yout] = ode45( @(t,x) sect_6_1_extended_kf_fn( t, x, k_1,k_2,k_3u,mu_q,var_q ), [t_k(ki),t_k(ki+1)], y0 );
  
  % state covariance extrapolation over t_{k-1} < t < t_{k}:
  % 
  xhatminus(ki) = yout(end,1);
  pminus(ki)    = yout(end,2);
  
  K_k = pminus(ki)/( pminus(ki) + R );
  
  % state covariance update due to measurement:
  %
  xhatplus(ki+1) = xhatminus(ki) + K_k * ( z_k(ki) - xhatminus(ki) );
  pplus(ki+1)    = ( 1 - K_k ) * pminus(ki); 
end