returnValue EllipsoidalIntegrator::init( const DifferentialEquation &rhs_, const int &N_ ){
if( rhs_.isODE( ) == BT_FALSE ) return ACADOERROR(RET_CANNOT_TREAT_DAE);
nx = rhs_.getDim();
Q.resize(nx,nx);
for( int i=0; i<nx*nx; i++ ) Q(i) = 0.0;
N = N_;
IntermediateState derivatives = rhs_.getODEexpansion( N );
IntermediateState gg = derivatives.getCol(0);
for( int i=0; i<N; i++ ) gg << derivatives.getCol(i+1)/acadoFactorial(i+1);
g << gg;
// FILE *file = fopen("g.c","w");
// file << g;
// fclose(file);
gr << derivatives.getCol(N+1)/acadoFactorial(N+1);
dg << gg.ADforward( VT_DIFFERENTIAL_STATE, rhs_.getComponents(), nx );
DifferentialState r(nx);
IntermediateState dgg = gg.ADforward( VT_DIFFERENTIAL_STATE, rhs_.getComponents(), r );
ddg << 0.5*dgg.ADforward( VT_DIFFERENTIAL_STATE, rhs_.getComponents(), r );
return SUCCESSFUL_RETURN;
}
EllipsoidalIntegrator::EllipsoidalIntegrator( const DifferentialEquation &rhs_, const int &N_ ):AlgorithmicBase(){
ASSERT( rhs_.isODE( ) == BT_TRUE );
setupOptions();
init( rhs_, N_ );
}