function [xhatminus,pminus,xhatplus,pplus] = sect_6_1_linear_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
  deltaT = t_k(ki+1) - t_k(ki);  
  phi    = exp(-k_1*deltaT);
  Gamma  = (1 - exp(-k_1*deltaT))/k_1;
  q      = (var_q/(2*(-k_1)))*(exp(2*(-k_1)*deltaT) - 1); % the process noise covariance

  % state covariance extrapolation over t_{k-1} < t < t_{k}:
  % 
  xhatminus(ki) = phi*xhatplus(ki) + Gamma * ( k_3u + mu_q );
  pminus(ki)    = phi*pplus(ki)*phi + q;
  
  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