-------------------------------------------------------------------------------
   Finds the cost function

   x     = x0 + alpha*d;
   J     = 0.5 rho'*W*rho + 0.5*(x-xA)'*S0*(x-xA)

-------------------------------------------------------------------------------
   Form:
   J = CostF( alpha, x0, d, S0, xA )
-------------------------------------------------------------------------------

   ------
   Inputs
   ------
   alpha                  Current value of alpha
   x0                     Current value of x
   d                      Alpha*d is the next increment to x
   S0                     A priori state covariance matrix
   xA                     A priori state

   -------
   Outputs
   -------
   J                      The cost function.
 
-------------------------------------------------------------------------------