function [xhatminus,pminus,xhatplus,pplus] = example_4_7_1_extended_kf(x_0, P_0, b, H_A, Q_A, R_A, z_k, t_k)
%
% Inputs: 
%
% x_0, P_0: the augmented initial state and covariance matrix
% b: a constant
% Q_A: the augmented process noise covariance matrix
% R_A: the augmented measurement noise covariance matrix
%
% 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
  % integrate our nonlinear equations from t_{k-1} < t < t_k: 
  xh = xhatplus(:,ki);
  ph = pplus(:,:,ki);
  y0 = [ xh(:); ph(:) ];
  [tout,yout] = ode45( @(t,x) example_4_7_1_extended_kf_fn( t, x, b, H_A, R_A, Q_A ), [t_k(ki),t_k(ki+1)], y0 );
  
  % state covariance extrapolation over t_{k-1} < t < t_{k}:
  % 
  ylast = yout(end,:)';
  xm    = ylast(1:3);
  pm    = reshape(ylast(4:end),3,3);
  xhatminus(:,ki) = xm;
  pminus(:,:,ki)  = pm;
  
  K_k = pm * H_A' * ( ( H_A * pm * H_A' + R_A ) \ eye(2) );
  
  % state covariance update due to measurement:
  %
  xhatplus(:,ki+1) = xm + K_k * ( z_k(:,ki) - H_A * xm );
  pplus(:,:,ki+1)  = ( eye(3) - K_k * H_A ) * pm;
  
end