function [f,g] = calcfg_inc(tstep,h,x,y,z,x_d,y_d,z_d,x0_p,y0_p,z0_p,...
                            a,r,b,D,freq)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Purpose: Calculate cost function and its gradient
%           
%
%  (c) 2002  Data Assimilation Research Centre
%
%  Written by Amos Lawless
%
%  List of main variables
%    a:          sigma coefficient in equations
%    r:          rho coefficient in equations
%    b:          beta coefficient in equations
%    D:          Observation weighting matrix
%    h:          Time step for numerical scheme
%    freq:       Frequency of observations
%    tstep:      Number of time steps to perform
%    [x,y,z]:    Forward trajectory
%    [x_d,y_d,z_d]: Innovations
%    [x0_p,y0_p,z0_p]: Initial perturbations
%
%  Output:
%    [f,g]: Cost function and gradient
%   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
f=0.0d0;
x_p=zeros(tstep,1); % Perturbations
y_p=zeros(tstep,1); % Perturbations
z_p=zeros(tstep,1); % Perturbations
%
x_p(1)=x0_p;
y_p(1)=y0_p;
z_p(1)=z0_p;
%
% Calculate cost function
[x_p,y_p,z_p]=modeuler_tl(tstep,h,0,z,y,x,z_p,y_p,x_p,a,r,b);
for i=1:freq:tstep
  f = f + 0.5 * ( (x_d(i)+x_p(i))*(x_d(i)+x_p(i))*D(i) ...
        + (y_d(i)+y_p(i))*(y_d(i)+y_p(i))*D(i) ...
        + (z_d(i)+z_p(i))*(z_d(i)+z_p(i))*D(i) );
end
%
% Calculate gradient of cost function
x_hat=zeros(tstep,1);
y_hat=zeros(tstep,1);
z_hat=zeros(tstep,1);
%
x_hat(tstep) = (x_d(tstep)+x_p(tstep))*D(tstep);
y_hat(tstep) = (y_d(tstep)+y_p(tstep))*D(tstep);
z_hat(tstep) = (z_d(tstep)+z_p(tstep))*D(tstep);
%
for i=tstep-1:-1:1
  [x1_hat,y1_hat,z1_hat] = modeuler_adj(1,h,0,z(i),y(i),x(i),...
               x_hat(i+1),y_hat(i+1),z_hat(i+1),a,r,b);
  x_hat(i) = x1_hat(1) + (x_d(i)+x_p(i))*D(i);
  y_hat(i) = y1_hat(1) + (y_d(i)+y_p(i))*D(i);
  z_hat(i) = z1_hat(1) + (z_d(i)+z_p(i))*D(i);
end
g=[x_hat(1),y_hat(1),z_hat(1)];