oem.cc

Go to the documentation of this file.

#include "arts.h"
#include <iostream>
#include "lin_alg.h"
#include "logic.h"
#include "math.h"
//#include "oem.h"
#include "stdlib.h"
#include "arts_omp.h"

using std::ostream;
using std::endl;
using std::setw;
using std::scientific;
using std::fixed;


//------------------------------------------------------------------------------------
//
//   Functions for displaying progress of inversions
//
//------------------------------------------------------------------------------------

void separator( ostream& stream,
                Index length )
{
    for (Index i = 0; i < length; i++)
        stream << "-";
    stream << endl;
}


void log_init_li( ostream& stream )
{
    stream << endl;
    stream << "Starting linear OEM inversion:" << endl << endl;
    separator( stream, 52 );
    stream << setw(6) << "Step";
    stream << setw(15) << "     Chi^2     ";
    stream << setw(15) << "     Chi^2_x   ";
    stream << setw(15) << "     Chi^2_y   ";
    stream << endl;
    separator( stream, 52 );
}


void log_step_li( ostream& stream,
                  Index step_number,
                  Numeric cost,
                  Numeric cost_x,
                  Numeric cost_y )
{
    stream << setw(5) << step_number << " ";
    stream << scientific;
    stream << setw(15) << cost;
    stream << setw(15) << cost_x;
    stream << setw(15) << cost_y;
    stream << endl;
}


void log_finalize_li( ostream& stream )
{
    separator( stream, 52 );
    stream << endl;
}


void log_init_gn( ostream& stream,
                  Numeric tol,
                  Index max_iter )
{
    stream << endl;
    stream << "Starting OEM inversion: " << endl << endl;
    stream << "\tMethod: Gauss-Newton" << endl;
    stream << "\tStop criterion: " << tol << endl;
    stream << "\tMax. iterations: " << max_iter << endl << endl;
    separator( stream, 67 );
    stream << setw(6) << "Step";
    stream << setw(15) << "     Chi^2     ";
    stream << setw(15) << "     Chi^2_x   ";
    stream << setw(15) << "     Chi^2_y   ";
    stream << setw(15) << "     d_i^2     ";
    stream << endl;
    separator( stream, 67 );
}


void log_step_gn( ostream& stream,
                  Index step_number,
                  Numeric cost,
                  Numeric cost_x,
                  Numeric cost_y,
                  Numeric di2 )
{
    stream << setw(5) << step_number << " ";
    stream << scientific;
    stream << setw(15) << cost;
    stream << setw(15) << cost_x;
    stream << setw(15) << cost_y;
    if (di2 < 0)
        stream << setw(15) << "";
    else
        stream << setw(15) << di2;
    stream << endl;
}


void log_finalize_gn( ostream& stream,
                      bool converged,
                      Index iter,
                      Index max_iter )
{
    separator( stream, 67 );
    stream << endl;
    if ( converged )
        stream << "\tConverged: YES";
    else if ( iter == max_iter )
    {
        stream << "\tConverged: NO" << endl;
        stream << "\tMaximum no. of iterations was reached.";
    }
    stream << endl << endl;
}


void log_init_lm( ostream& stream,
                  Numeric tol,
                  Index max_iter )
{
    stream << endl;
    stream << "Starting OEM inversion: " << endl  << endl;
    stream << "     Method: Levenberg-Marquardt " << endl;
    stream << "     Stop criterion: " << tol << endl;
    stream << "     Max. iterations: " << max_iter << endl << endl;
    separator( stream, 75 );
    stream << setw(6) << "Step";
    stream << setw(15) << "     Chi^2      ";
    stream << setw(15) << "     Chi^2_x    ";
    stream << setw(15) << "     Chi^2_y    ";
    stream << setw(9) << "  gamma  ";
    stream << setw(15) << "     d_i^2     ";
    stream << endl;
    separator( stream, 75 );
}

void log_gamma_step_lm( ostream& stream,
                        Numeric cost,
                        Numeric cost_x,
                        Numeric cost_y,
                        Numeric gamma )
{
    stream << setw(6) << "";
    stream << scientific;
    stream << setw(15) << cost;
    stream << setw(15) << cost_x;
    stream << setw(15) << cost_y;
    stream.unsetf(ios_base::floatfield);
    stream << setw(9) << gamma;
    stream << endl;
}


void log_step_lm( ostream& stream,
                  Index step_number,
                  Numeric cost,
                  Numeric cost_x,
                  Numeric cost_y,
                  Numeric gamma,
                  Numeric di2 )
{
    stream << setw(6) << step_number;
    stream << scientific;
    stream << setw(15) << cost;
    stream << setw(15) << cost_x;
    stream << setw(15) << cost_y;
    stream.unsetf(ios_base::floatfield);
    stream << setw(9) << gamma;
    stream << scientific;
    stream << setw(15) << di2;
    stream << endl;
}


void log_finalize_lm( ostream& stream,
                      bool converged,
                      Numeric cost,
                      Numeric cost_x,
                      Numeric cost_y,
                      Numeric gamma,
                      Numeric gamma_max,
                      Index iter,
                      Index max_iter )
{
    separator( stream, 75 );
    stream << endl << "Finished Levenberg-Marquardt iterations:" << endl;
    if ( converged )
        stream << "\t Converged: YES" << endl;
    else if ( gamma == gamma_max )
    {
        stream << "\t Converged: NO" << endl;
        stream << "\t Iteration aborted because gamma > gamma_max.";
    }
    else if ( iter == max_iter )
    {
        stream << "\t Converged: NO" << endl;
        stream << "\t Iteration aborted because maximum no. of iterations was reached.";
    }
    stream << endl;

    stream << "\t Chi^2: " << cost << endl;
    stream << "\t Chi^2_x: " << cost_x << endl;
    stream << "\t Chi^2_y: " << cost_y << endl;
    stream << endl << endl << endl;
}

//------------------------------------------------------------------------------------
//
//   Calculation of the cost function
//
//------------------------------------------------------------------------------------


template <typename Se_t>
void oem_cost_y( Numeric& cost_y,
                 ConstVectorView y,
                 ConstVectorView yf,
                 Se_t SeInv,
                 const Numeric&  normfac )
{
  Vector dy = y; dy -= yf;
  Vector tmp( y.nelem() );
  mult( tmp, SeInv, dy );
  cost_y = dy * tmp;
  cost_y /= normfac;
}


void oem_cost_x( Numeric& cost_x,
                 ConstVectorView x,
                 ConstVectorView xa,
                 ConstMatrixView SxInv,
                 const Numeric&  normfac )
{

  Vector dx = x; dx -= xa;

  Vector tmp( x.nelem() );
  mult( tmp, SxInv, dx );
  cost_x = dx * tmp;
  cost_x /= normfac;
}

//------------------------------------------------------------------------------------
//
//  Algebra Helper Functions
//
//------------------------------------------------------------------------------------


void mult_outer( MatrixView A,
                 ConstMatrixView B,
                 ConstVectorView b )
{
    Index n;

    n = b.nelem();

    // A,B must be n x n.
    assert( is_size( A, n, n ) );
    assert( is_size( B, n, n ) );

    for ( Index i = 0; i < n; i++)
    {
        for ( Index j = 0; j < n; j++)
        {
            A(i,j) = B(i,j) * b[i] * b[j];
        }
    }
}


void scale_columns( MatrixView A,
                    ConstMatrixView B,
                    ConstVectorView b )
{
    Index m,n;

    m = A.nrows();
    n = A.ncols();

    // A,B must be n x n.
    assert( is_size( B, m, n ) );
    assert( is_size( b, n ) );

    for ( Index i = 0; i < n; i++)
    {
        for ( Index j = 0; j < n; j++)
        {
            A(i,j) = B(i,j) * b[j];
        }
    }
}


void scale_rows( MatrixView A,
                 ConstMatrixView B,
                 ConstVectorView b )
{
    Index m,n;

    m = A.nrows();
    n = A.ncols();

    // A,B must be n x n.
    assert( is_size( B, m, n ) );
    assert( is_size( b, n ) );

    for ( Index i = 0; i < n; i++)
    {
        for ( Index j = 0; j < n; j++)
        {
            A(i,j) = B(i,j) * b[i];
        }
    }
}

//------------------------------------------------------------------------------------
//
//  Linear OEM Class
//
//------------------------------------------------------------------------------------


LinearOEM::LinearOEM( ConstMatrixView J_,
                      ConstMatrixView SeInv_,
                      ConstVectorView xa_,
                      ConstMatrixView SxInv_ )
    : J(J_), SeInv(SeInv_), SxInv(SxInv_), x_norm()
{
    n = J.ncols();
    m = J.nrows();

    assert( is_size( SeInv_, m, m ) );
    assert( is_size( SxInv_, n, n ) );
    assert( is_size( xa_, n ) );

    xa = xa_;

    matrices_set = false;
    gain_set = false;
    x_norm_set = false;
    form = NFORM;

    // Allocate memory for matrices.
    LU = Matrix( n, n );
    indx = ArrayOfIndex( n );
    tmp_nn_1 = Matrix( n, n );
    tmp_nm_1 = Matrix( n, m );
    tmp_n_1 = Vector( n );

}


void LinearOEM::set_x_norm( ConstVectorView x_norm_ )
{
    x_norm = x_norm_;
    x_norm_set = true;
    matrices_set = false;
    gain_set = false;

}


ConstVectorView LinearOEM::get_x_norm()
{
    return x_norm;
}


Index LinearOEM::compute( Vector &x,
                         ConstVectorView y,
                         ConstVectorView y0 )
{

    // Set up timers.
    Index t1, t2, t3;
    t1 = timer.add_timer( "State vector computation" );
    timer.mark( t1 );
    t2 = timer.add_timer( "Matrix multiplication");
    t3 = timer.add_timer( "Linear system solving");

    if (!(matrices_set || gain_set))
    {

        timer.mark( t2 );
        mult( tmp_nm_1, transpose(J), SeInv );
        mult( tmp_nn_1, tmp_nm_1, J);
        timer.mark( t2 );

        tmp_nn_1 += SxInv;

        if ( x_norm_set )
        {
            mult_outer( tmp_nn_1, tmp_nn_1, x_norm );
            scale_rows( tmp_nm_1, tmp_nm_1, x_norm );
        }

        timer.mark( t3 );
        ludcmp( LU, indx, tmp_nn_1 );
        timer.mark( t3 );

        matrices_set = true;
    }

    tmp_m_1 = y;
    tmp_m_1 -= y0;

    if (!gain_set)
    {
        mult( tmp_n_1, tmp_nm_1, tmp_m_1 );

        timer.mark( t3 );
        lubacksub( x, LU, tmp_n_1, indx);
        timer.mark( t3 );

        if (x_norm_set)
            x *= x_norm;
    }
    else
    {
        mult( x, G, tmp_m_1 );
    }

    x += xa;

    timer.mark( t1 );

    return 0;
}


Index LinearOEM::compute( Vector &x,
                          MatrixView G_,
                          ConstVectorView y,
                          ConstVectorView y0 )
{

    if (!gain_set)
        compute_gain_matrix();

    compute( x, y, y0 );
    G_ = G;

    return 0;
}

void LinearOEM::compute_gain_matrix()
{

    // Setup timers.
    Index t1, t2, t3;
    t1 = timer.add_timer( "Gain Matrix Computation" );
    t2 = timer.add_timer( "Matrix Matrix Mult.");
    t3 = timer.add_timer( "Linear System" );
    timer.mark( t1 );

    // Assure that G has the right size.
    G.resize( n, m );
    tmp_nn_2.resize( n, n );

    if (!(matrices_set))
    {
        timer.mark( t2 );
        mult( tmp_nm_1, transpose(J), SeInv );
        mult( tmp_nn_1, tmp_nm_1, J);
        timer.mark( t2 );

        tmp_nn_1 += SxInv;

        if ( x_norm_set )
        {
            mult_outer( tmp_nn_1, tmp_nn_1, x_norm );
            scale_rows( tmp_nm_1, tmp_nm_1, x_norm );
        }

        timer.mark( t3 );
        ludcmp( LU, indx, tmp_nn_1 );
        timer.mark( t3 );

        matrices_set = true;
    }

    // Invert J^T S_e^{-1} J using the already computed LU decomposition.
    tmp_n_1 = 0.0;

    timer.mark( t3 );
    for ( Index i = 0; i < n; i++ )
    {
        tmp_n_1[i] = 1.0;
        lubacksub( tmp_nn_2(joker,i), LU, tmp_n_1, indx);
        tmp_n_1[i] = 0.0;
    }
    timer.mark( t3 );

    timer.mark( t2 );
    mult( G, tmp_nn_2, tmp_nm_1 );
    timer.mark( t2 );

    if ( x_norm_set )
    {
        scale_rows( G, G, x_norm );
        matrices_set = false;
    }
    gain_set = true;

    timer.mark( t1 );
}


void LinearOEM::compute_averaging_kernel( MatrixView A )
{

    assert( n == A.ncols() );
    assert( n == A.nrows() );

    // If not already computed, compute Gain matrix, which is
    // required for the computation of the averaging kernel.
    if (!gain_set)
        compute_gain_matrix( );

    // If the Gain matrix has already been computed, G
    // is set and can be reused for the computation of A.

    mult( A, G, J );
}


Index LinearOEM::compute_fit( Vector &yf,
                            const Vector &x,
                            ForwardModel &F )
{
    try
    {
        F.evaluate( yf, x );
    }
    catch (int e)
    {
        err = 9;
        return err;
    }

    return 0;
}


Index LinearOEM::compute_fit( Vector &yf,
                            Numeric &cost_x,
                            Numeric &cost_y,
                            Vector &x,
                            ConstVectorView y,
                            ForwardModel &F )
{

    try
    {
        F.evaluate( yf, x );
    }
    catch (int e)
    {
        return 9;
    }

    oem_cost_y( cost_y, y, yf, SeInv, (Numeric) m);
    oem_cost_x( cost_x, x, xa, SxInv, (Numeric) m);

    return 0;
}

//------------------------------------------------------------------------------------
//
//  Linear OEM
//
//------------------------------------------------------------------------------------


Index oem_linear_nform( Vector& x,
                        Matrix& G,
                        Matrix& J,
                        Vector& yf,
                        Numeric& cost_y,
                        Numeric& cost_x,
                        ForwardModel &F,
                        ConstVectorView xa,
                        ConstVectorView x_norm,
                        ConstVectorView y,
                        ConstMatrixView SeInv,
                        ConstMatrixView SxInv,
                        const Numeric& cost_start,
                        const bool& verbose )
{
    const Index m = J.nrows();
    const Index n = J.ncols();

    // Check dimensions for consistency.
    assert( xa.nelem() == n );
    assert( x_norm.nelem() == 0  || x_norm.nelem() == n );
    assert( y.nelem() == m );
    assert( (SeInv.ncols() == m) && (SeInv.nrows() == m) );
    assert( (SxInv.ncols() == n) && (SxInv.nrows() == n) );

    LinearOEM oem( J, SeInv, xa, SxInv );

    if ( x_norm.nelem() == n )
        oem.set_x_norm( x_norm );

    // Initialize log output.
    if (verbose)
      {
        log_init_li( cout );
        log_step_li( cout, 0, cost_start, 0, cost_start );
      }

    oem.compute( x, G, y, yf );
    oem.compute_fit( yf, cost_x, cost_y, x, y, F );

    // Finalize log output.
    if (verbose)
      {
        log_step_li( cout, 1, cost_y+cost_x, cost_x, cost_y );
        log_finalize_li( cout );
      }

    // Return convergence status
    return oem.get_error();
}

//------------------------------------------------------------------------------------
//
// Non-linear OEM Class
//
//------------------------------------------------------------------------------------


template<typename Se_t, typename Sx_t>
NonLinearOEM<Se_t, Sx_t>::NonLinearOEM( const Se_t      &SeInv_,
                                        ConstVectorView  xa_,
                                        const Sx_t      &SxInv_,
                                        ForwardModel    &F_,
                                        OEMMethod        method_ )
    : SeInv(SeInv_), SxInv(SxInv_), xa(xa_), F(F_), method(method_)
{

    m = SeInv_.nrows();
    n = SxInv_.nrows();

    assert( xa_.nelem() == n );
    assert( (SeInv_.ncols() == m) && (SeInv_.nrows() == m) );
    assert( (SxInv_.ncols() == n) && (SxInv_.nrows() == n) );

    xa = xa_;

    // Allocate memory for internal matrices and vectors.
    J = Matrix(m,n);
    G = Matrix(n,m);
    tmp_nm_1 = Matrix(n,m);
    tmp_nn_1 = Matrix(n,n);
    tmp_nn_2 = Matrix(n,n);

    tmp_m_1 = Vector(m);
    tmp_n_1 = Vector(n);
    tmp_n_2 = Vector(n);
    dx = Vector(n);
    yi = Vector(m);

    // Initialize internal state variables.
    matrices_set = false;
    gain_set = false;
    x_norm_set = false;
    conv = false;

    // Default iteration parameters.
    tol = 1e-5;
    max_iter = 100;

    ga_start = 4.0;
    ga_max = 100.0;
    ga_decrease = 2.0;
    ga_increase = 3.0;
    ga_threshold = 1.0;

}


template<typename Se_t, typename Sx_t>
void NonLinearOEM<Se_t, Sx_t>::set_x_norm( ConstVectorView x_norm_ )
{
    x_norm = x_norm_;
    x_norm_set = true;
}


template<typename Se_t, typename Sx_t>
ConstVectorView NonLinearOEM<Se_t, Sx_t>::get_x_norm()
{
    return x_norm;
}


template<typename Se_t, typename Sx_t>
Index NonLinearOEM<Se_t, Sx_t>::compute( Vector &x,
                                         ConstVectorView y,
                                         bool verbose )
{

    if (method == GAUSS_NEWTON)
    {
        gauss_newton( x, y, verbose );
    }
    else if (method == LEVENBERG_MARQUARDT)
    {
        levenberg_marquardt( x, y, verbose );
    }

    if (verbose)
        cout << timer << endl;

    return err;
}


template<typename Se_t, typename Sx_t>
Index NonLinearOEM<Se_t, Sx_t>::compute( Vector          &x,
                                         MatrixView       G_,
                                         ConstVectorView  y,
                                         bool             verbose )
{

    if (method == GAUSS_NEWTON)
    {
        gauss_newton( x, y, verbose );
    }
    else if (method == LEVENBERG_MARQUARDT)
    {
        levenberg_marquardt( x, y, verbose );
    }

    compute_gain_matrix( x );
    G_ = G;

    if (verbose)
        cout << timer << endl;

    return err;
}


template<typename Se_t, typename Sx_t>
void NonLinearOEM<Se_t, Sx_t>::gauss_newton( Vector          &x,
                                             ConstVectorView  y,
                                             bool             verbose )
{

    Index t1, t2, t3, t4;
    t1 = timer.add_timer( "Gauss-Newton Iteration" );
    t2 = timer.add_timer( "Matrix Matrix Mult." );
    t3 = timer.add_timer( "Linear System" );
    t4 = timer.add_timer( "Jacobian Evaluation" );
    timer.mark( t1 );

    Numeric di2 = -1.0;

    cost_x = 0.0; cost_y = 0.0;
    iter = 0;
    conv = false;
    err = 1;

    // Initialize log output.
    if (verbose)
    {
          log_init_gn( cout, tol, max_iter );
    }

    // Start stop watch.

    // Set the starting vector.
    x = xa;

    while ( (!conv) && (iter < max_iter) )
    {
        // Compute Jacobian and y_i.

        timer.mark( t1 );
        timer.mark( t4 );
        try
        {
            F.evaluate_jacobian( yi, J, x);
        }
        catch (int e)
        {
            err = 9;
            return void();
        }
        timer.mark( t1 );
        timer.mark( t4 );


        timer.mark( t2 );
        mult( tmp_nm_1, transpose(J), SeInv );
        mult( tmp_nn_1, tmp_nm_1, J );
        timer.mark( t2 );

        tmp_nn_1 += SxInv;

        if (x_norm_set)
        {
            mult_outer( tmp_nn_1, tmp_nn_1, x_norm );
            scale_rows( tmp_nm_1, tmp_nm_1, x_norm );
        }

        // tm1 = K_i(x_i - x_a)

        tmp_n_1 = x;
        tmp_n_1 -= xa;
        mult( tmp_n_2, SxInv, tmp_n_1 );

        if (x_norm_set)
            tmp_n_2 *= x_norm;

        tmp_m_1 = y;
        tmp_m_1 -= yi;
        // tmp_n_1 = K_i^T * S_e^{-1} * (y - F(x_i))
        mult( tmp_n_1, tmp_nm_1, tmp_m_1 );

        // This vector is used to test for convergence later.
        // See eqn. (5.31).
        tmp_n_1 -= tmp_n_2;

        // Compute cost function and convergence criterion.
        oem_cost_x( cost_x, x, xa, SxInv, (Numeric) m);
        oem_cost_y( cost_y, y, yi, SeInv, (Numeric) m);

        // Print log.
        if ( verbose )
            log_step_gn( cout, iter, cost_x + cost_y, cost_x, cost_y, di2 );

        timer.mark( t3 );
        solve( dx, tmp_nn_1, tmp_n_1 );
        timer.mark( t3 );

        if (x_norm_set)
            dx *= x_norm;
        x += dx;
        iter++;

        // Compute convergence criterion.
        di2 = dx * tmp_n_1;

        // If converged, stop iteration.
        if ( di2 <= tol * (Numeric) n )
        {
            conv = true;
            err = 0;
        }
    }

    if ( iter == max_iter )
        err = 1;

    // Stop stop watch.

    // Finalize log output.
    if ( verbose )
        log_finalize_gn( cout, conv, iter, max_iter );

}


template<typename Se_t, typename Sx_t>
void NonLinearOEM<Se_t, Sx_t>::levenberg_marquardt( Vector          &x,
                                                    ConstVectorView  y,
                                                    bool             verbose )
{
    // Setup timers.
    Index t1, t2, t3, t4;
    t1 = timer.add_timer( "Levenberg-Marquardt Iteration" );
    t2 = timer.add_timer( "Matrix Matrix Mult." );
    t3 = timer.add_timer( "Linear System" );
    t4 = timer.add_timer( "Jacobian Evaluation" );
    timer.mark( t1 );

    Numeric cost_old, cost, di2;
    di2 = -1.0;

    if ( verbose )
        log_init_lm( cout, tol, max_iter );

    // Set starting vector.
    x = xa;

    conv = false;
    iter = 0;
    cost_x = 0.0;
    cost_y = 0.0;
    Numeric gamma = ga_start;

    while ( (!conv) && (iter < max_iter) && (gamma <= ga_max) )
    {
        // Compute Jacobian and y_i.
        timer.mark( t1 );
        timer.mark( t4 );
        try
        {
            F.evaluate_jacobian( yi, J, x);
        }
        catch (int e)
        {
            err = 9;
            return void();
        }
        timer.mark( t1 );
        timer.mark( t4 );

        timer.mark(t2);
        mult( tmp_nm_1, transpose(J), SeInv );
        mult( tmp_nn_1, tmp_nm_1, J );
        timer.mark(t2);

        tmp_n_1 = x;
        tmp_n_1 -= xa;
        mult( tmp_n_2, SxInv, tmp_n_1 );

        tmp_m_1 = y;
        tmp_m_1 -= yi;
        mult( tmp_n_1, tmp_nm_1, tmp_m_1 );

        tmp_n_1 -= tmp_n_2;
        tmp_n_2 = tmp_n_1;
        if (x_norm_set)
            tmp_n_1 *= x_norm;

        // Compute costs and print log message.
        oem_cost_x( cost_x, x, xa, SxInv, (Numeric) m);
        oem_cost_y( cost_y, y, yi, SeInv, (Numeric) m);
        cost_old = cost_y + cost_x;

        if ( verbose )
            log_step_gn( cout, iter, cost_x + cost_y, cost_x, cost_y, di2 );

        bool found_x = false;
        while ( !found_x )
        {
            tmp_nn_2 = SxInv;
            tmp_nn_2 *= ( 1.0 + gamma );
            tmp_nn_2 += tmp_nn_1;

            if ( x_norm_set )
            {
                mult_outer( tmp_nn_2, tmp_nn_2, x_norm );
            }

            // This vector is used to test for convergence later.
            // See eqn. (5.31).

            timer.mark( t3 );
            solve( dx, tmp_nn_2, tmp_n_1 );
            timer.mark( t3 );

            if (x_norm_set)
                dx *= x_norm;
            xnew = x;
            xnew += dx;

            // Evaluate cost function.


            timer.mark(t1);
            timer.mark(t4);
            try
            {
                F.evaluate( yi, xnew );
            }
            catch ( int e )
            {
                err = 9;
                return void();
            }
            timer.mark(t1);
            timer.mark(t4);


            oem_cost_x( cost_x, xnew, xa, SxInv, (Numeric) m);
            oem_cost_y( cost_y, y, yi, SeInv, (Numeric) m);
            cost = cost_x + cost_y;

            // If cost has decreased, keep new x and
            // scale gamma.
            if (cost < cost_old)
            {

                if ( gamma >= (ga_threshold * ga_decrease))
                    gamma /= ga_decrease;
                else
                    gamma = 0;

                x = xnew;
                cost_old = cost;
                found_x = true;

            }
            // Else try to increase gamma and look for a
            // new x.
            else
            {
                if ( gamma < ga_threshold )
                           gamma = ga_threshold;
                else
                {
                    if ( gamma < ga_max )
                    {
                        gamma *= ga_increase;
                        if (gamma > ga_max)
                            gamma = ga_max;
                    }
                    // Gamma exceeds maximum. Abort.
                    else
                    {
                        gamma = ga_max + 1.0;
                        break;
                    }
                }
            }
        }

        // Increase iteration counter and check for convergence.
        iter++;

        di2 = dx * tmp_n_2;
        di2 /= (Numeric) n;

        if ( di2 <= tol )
        {
            conv = true;
            err = 0;
       }

    }

    if ( iter == max_iter )
        err = 1;
    if ( gamma >= ga_max )
        err = 2;

    timer.mark( t1 );

    // Final log output.
    if ( verbose )
    {
        cost = cost_x + cost_y;
        log_finalize_lm( cout, conv, cost, cost_x, cost_y,
                         gamma, ga_max, iter, max_iter );
    }
}


template<typename Se_t, typename Sx_t>
Index NonLinearOEM<Se_t, Sx_t>::compute_fit( Vector                &yf,
                                                      const Vector &x )
{
    try
    {
        F.evaluate( yf, x );
    }
    catch (int e)
    {
        err = 9;
        return err;
    }
    return 0;

}


template<typename Se_t, typename Sx_t>
Index NonLinearOEM<Se_t, Sx_t>::compute_fit( Vector &yf,
                                             Numeric &cost_x_,
                                             Numeric &cost_y_,
                                             const Vector &x,
                                             ConstVectorView y )
{

    try
    {
        F.evaluate( yf, x );
    }
    catch (int e)
    {
        err = 9;
        return err;
    }

    oem_cost_y( cost_y_, y, yf, SeInv, (Numeric) m);
    oem_cost_x( cost_x_, x, xa, SxInv, (Numeric) m);

    return 0;

}


template<typename Se_t, typename Sx_t>
void NonLinearOEM<Se_t, Sx_t>::compute_gain_matrix( ConstVectorView x )
{
    Index t1, t2, t3, t4;
    t1 = timer.add_timer( "Gain Matrix Computation" );
    t2 = timer.add_timer( "Matrix Matrix Mult." );
    t3 = timer.add_timer( "Linear System" );
    t4 = timer.add_timer( "Jacobian Evaluation" );
    timer.mark( t1 );

    timer.mark( t4 );
    try
    {
        F.evaluate_jacobian( tmp_m_1, J, x);
    }
    catch (int e)
    {
        err = 9;
        return void();
    }
    timer.mark( t4 );

    timer.mark( t2 );
    mult( tmp_nm_1, transpose(J), SeInv );
    mult( tmp_nn_1, tmp_nm_1, J );
    timer.mark( t2 );

    tmp_nn_1 += SxInv;

    if (x_norm_set)
    {
        mult_outer( tmp_nn_1, tmp_nn_1, x_norm );
        scale_columns( tmp_nm_1, tmp_nm_1, x_norm );
    }

    timer.mark( t3 );
    inv( tmp_nn_2, tmp_nn_1 );
    timer.mark( t3 );

    timer.mark( t2 );
    mult( G, tmp_nn_2, tmp_nm_1 );
    timer.mark( t2 );

    timer.mark( t1 );
}


template <typename Se_t, typename Sx_t>
void NonLinearOEM<Se_t,Sx_t>::compute_averaging_kernel( MatrixView A,
                                                        ConstVectorView x )
{
    assert( A.ncols() == n );
    assert( A.nrows() == n );

    // If not already computed, compute Gain matrix, which is
    // required for the computation of the averaging kernel.
    if (!gain_set)
        compute_gain_matrix( x );

    // If the Gain matrix has already been computed both G and J
    // are set and can be reused for the computation of A.

    mult( A, G, J );
}


template <typename Se_t, typename Sx_t>
Index oem_gauss_newton( Vector& x,
                        Matrix& G,
                        Matrix& J,
                        Vector& yf,
                        Numeric& cost_y,
                        Numeric& cost_x,
                        Index& iter,
                        ForwardModel &F,
                        ConstVectorView xa,
                        ConstVectorView x_norm,
                        ConstVectorView y,
                        const Se_t &SeInv,
                        const Sx_t &SxInv,
                        const Index max_iter,
                        const Numeric tol,
                        bool verbose )
{
    const Index m = J.nrows();
    const Index n = J.ncols();

    // Check dimensions for consistency.
    assert( xa.nelem() == n );
    assert( x_norm.nelem() == 0  || x_norm.nelem() == n );
    assert( y.nelem() == m );
    assert( (SeInv.ncols() == m) && (SeInv.nrows() == m) );
    assert( (SxInv.ncols() == n) && (SxInv.nrows() == n) );

    NonLinearOEM<Se_t, Sx_t>
        oem( SeInv, xa, SxInv, F, GAUSS_NEWTON);
    oem.tolerance( tol );
    oem.maximum_iterations( max_iter );

    if ( x_norm.nelem() == n )
        oem.set_x_norm( x_norm );

    oem.compute( x, G, y, verbose );
    oem.compute_fit( yf, cost_x, cost_y, x, y );

    J = oem.get_jacobian();


    iter = oem.iterations();

    return oem.get_error();
}




template <typename Se_t, typename Sx_t>
Index oem_levenberg_marquardt( Vector& x,
                               Matrix& G,
                               Matrix& J,
                               Vector& yf,
                               Numeric& cost_y,
                               Numeric& cost_x,
                               Index& iter,
                               ForwardModel &F,
                               ConstVectorView xa,
                               ConstVectorView x_norm,
                               ConstVectorView y,
                               const Se_t &SeInv,
                               const Sx_t &SxInv,
                               Index max_iter,
                               Numeric tol,
                               Numeric gamma_start,
                               Numeric gamma_decrease,
                               Numeric gamma_increase,
                               Numeric gamma_max,
                               Numeric gamma_threshold,
                               bool verbose )
{

    const Index m = J.nrows();
    const Index n = J.ncols();

    // Check dimensions for consistency.
    assert( xa.nelem() == n );
    assert( x_norm.nelem() == 0  || x_norm.nelem() == n );
    assert( y.nelem() == m );
    assert( (SeInv.ncols() == m) && (SeInv.nrows() == m) );
    assert( (SxInv.ncols() == n) && (SxInv.nrows() == n) );

    NonLinearOEM<Se_t, Sx_t>
        oem( SeInv, xa, SxInv, F, LEVENBERG_MARQUARDT);
    oem.tolerance( tol );
    oem.maximum_iterations( max_iter );
    oem.gamma_start( gamma_start );
    oem.gamma_decrease( gamma_decrease );
    oem.gamma_increase( gamma_increase );
    oem.gamma_max( gamma_max );
    oem.gamma_threshold( gamma_threshold );

    if ( x_norm.nelem() == n )
        oem.set_x_norm( x_norm );

    oem.compute( x, G, y, verbose );
    oem.compute_fit( yf, cost_x, cost_y, x, y );

    J = oem.get_jacobian();

    iter = oem.iterations();

    return oem.get_error();

}

//------------------------------------------------------------------------------------
//
//   OEM versions so far not used
//
//------------------------------------------------------------------------------------



void oem_linear_mform( VectorView x,
                       MatrixView G,
                       ConstVectorView xa,
                       ConstVectorView yf,
                       ConstVectorView y,
                       ConstMatrixView K,
                       ConstMatrixView Se,
                       ConstMatrixView Sx )
{
    Index m = K.nrows();
    Index n = K.ncols();

    // Check dimensions for consistency.
    assert( x.nelem() == n );
    assert( xa.nelem() == n );
    assert( y.nelem() == m );

    assert( (Se.ncols() == m) && (Se.nrows() == m) );
    assert( (Sx.ncols() == n) && (Sx.nrows() == n) );

    // m-form (eq. (4.6)).
    Matrix tmp_nm(n,m);
    Matrix tmp_mm(m,m), tmp_mm2(m,m);
    Vector tmp_m(m);

    mult( tmp_nm, Sx, transpose(K) ); // tmp_nm = S_a * K^T
    mult( tmp_mm, K, tmp_nm);
    tmp_mm += Se;

    // Compute gain matrix.
    inv( tmp_mm2, tmp_mm );
    mult( G, tmp_nm, tmp_mm2 );

    tmp_m = y;
    tmp_m -= yf;
    mult( x, G, tmp_m );

    x += xa;
}




bool oem_gauss_newton_m_form( VectorView x,
                              ConstVectorView y,
                              ConstVectorView xa,
                              ForwardModel &K,
                              ConstMatrixView Se,
                              ConstMatrixView Sx,
                              Numeric tol,
                              Index max_iter )
{
    Index n( x.nelem() ), m( y.nelem() );
    Numeric di2;
    Matrix Ki(m,n), SxKiT(m, n), KiSxKiT(m, m);
    Vector xi(n), yi(m), tm(m), tm2(m), tn(n), yi_old(m);

    Index iter = 0;
    bool converged = false;

    // Set the starting vector.
    x = xa;

    while ((!converged) && (iter < max_iter))
    {

        // Compute Jacobian and y_i.
        K.evaluate_jacobian( yi, Ki, x );
        // If not in the first iteration, check for
        // convergence here.
        if (iter > 0)
        {
            yi_old -= yi;

            solve( tm, Se, yi_old );
            mult( tm2, KiSxKiT, tm );
            // TODO: Optimize using LU decomp.
            solve( tm, Se, tm2);
            di2 = yi_old * tm;
            if ( fabs( di2 ) < tol * (Numeric) n )
            {
                converged = true;
                break;
            }
        }

        mult( SxKiT, Sx, transpose(Ki) );
        mult( KiSxKiT, Ki, SxKiT );
        KiSxKiT += Se;

        // tm = K_i(x_i - x_a)
        tn = x;
        tn -= xa;
        mult( tm, Ki, tn );

        tm -= yi;
        tm += y;

        solve( tm2, KiSxKiT, tm );
        mult( x, SxKiT, tm2 );
        x += xa;

        // Increase iteration counter and store yi.
        iter++;
        yi_old = yi;
    }
    return converged;
}


bool oem_gauss_newton_n_form( VectorView x,
                              ConstVectorView y,
                              ConstVectorView xa,
                              ForwardModel &K,
                              ConstMatrixView Se,
                              ConstMatrixView Sx,
                              Numeric tol,
                              Index max_iter )
{
    Index n( x.nelem() ), m( y.nelem() );
    Numeric di2;
    Matrix Ki(m,n), KiTSeInvKi(n, n), KiTSeInv(m,n), SeInv(m,m), SxInv(n,n);
    Vector xi(n), dx(n), dx_old(n), yi(m), tm(m), tn(n), sInvDx(n);

    Index iter = 0;
    bool converged = false;

    // Required matrix inverses.
    inv(SeInv, Se);
    inv(SxInv, Sx);

    // Set the starting vector.
    x = xa;
    dx_old = 0;

    while ((!converged) && (iter < max_iter))
    {

        // Compute Jacobian and y_i.
        K.evaluate_jacobian( yi, Ki, x );
        mult( KiTSeInv, transpose( Ki ), SeInv );
        mult( KiTSeInvKi, KiTSeInv, Ki );
        KiTSeInvKi += SxInv;

        // tm = K_i(x_i - x_a)
        tn = x;
        tn -= xa;
        mult( tm, Ki, tn );

        tm -= yi;
        tm += y;

        mult( tn, KiTSeInv, tm );
        solve( dx, KiTSeInvKi, tn );

        x = xa;
        x += dx;

        // Check for convergence.
        tm = y;
        tm -= yi;
        mult( sInvDx, KiTSeInv, tm );
        mult( tn, SxInv, dx_old );
        sInvDx -= tn;

        dx -= dx_old;
        di2 = dx * sInvDx;

        if ( di2 <= tol * (Numeric) m )
        {
            converged = true;
        }

        // Increase iteration counter and store yi.
        iter++;
        dx_old = dx;
    }
    return converged;
}