function [xhatminus,pminus,xhatplus,pplus] = sect_7_prob_1_extended_kf(x_0, P_0, Phi_truth, H_A, Lambda_A, Q_A, R_A, z, ts)
%
% 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(ts)-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
  % propagate the state and uncertainty from t_{k-1} < t < t_k:
  %
  xh    = xhatplus(:,ki); % the nonlinear evolution of the state: 
  xm    = zeros(n,1);
  xm(1) = Phi_truth(1,1) * xh(1) + Phi_truth(1,2) * xh(2);
  xm(2) = xh(3)          * xh(1) + Phi_truth(2,2) * xh(2);
  xm(3) = xh(3);

  Phi_k = zeros(n,n);
  Phi_k(1,1) = Phi_truth(1,1);
  Phi_k(1,2) = Phi_truth(1,2);
  Phi_k(2,1) = xh(3);
  Phi_k(2,2) = Phi_truth(2,2);
  Phi_k(3,3) = 1.0;
  Phi_k(2,3) = xh(1);
  
  ph = pplus(:,:,ki); % the uncertainty
  pm = Phi_k * ph * Phi_k' + Lambda_A * Q_A * Lambda_A';
  
  xhatminus(:,ki) = xm; % store
  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(:,ki) - H_A * xm );
  pplus(:,:,ki+1)  = ( eye(n) - K_k * H_A ) * pm;
end