Vector Rosen34(
	Fun           &F , 
	size_t         M , 
	const Scalar &ti , 
	const Scalar &tf , 
	const Vector &xi ,
	Vector       &e )
{
	CPPAD_ASSERT_FIRST_CALL_NOT_PARALLEL;

	// check numeric type specifications
	CheckNumericType<Scalar>();

	// check simple vector class specifications
	CheckSimpleVector<Scalar, Vector>();

	// Parameters for Shampine's Rosenbrock method
	// are static to avoid recalculation on each call and 
	// do not use Vector to avoid possible memory leak
	static Scalar a[3] = {
		Scalar(0),
		Scalar(1),
		Scalar(3)   / Scalar(5)
	};
	static Scalar b[2 * 2] = {
		Scalar(1),
		Scalar(0),
		Scalar(24)  / Scalar(25),
		Scalar(3)   / Scalar(25)
	};
	static Scalar ct[4] = {
		Scalar(1)   / Scalar(2),
		- Scalar(3) / Scalar(2),
		Scalar(121) / Scalar(50),
		Scalar(29)  / Scalar(250)
	};
	static Scalar cg[3 * 3] = {
		- Scalar(4),
		Scalar(0),
		Scalar(0),
		Scalar(186) / Scalar(25),
		Scalar(6)   / Scalar(5),
		Scalar(0),
		- Scalar(56) / Scalar(125),
		- Scalar(27) / Scalar(125),
		- Scalar(1)  / Scalar(5)
	};
	static Scalar d3[3] = {
		Scalar(97) / Scalar(108),
		Scalar(11) / Scalar(72),
		Scalar(25) / Scalar(216)
	};
	static Scalar d4[4] = {
		Scalar(19)  / Scalar(18),
		Scalar(1)   / Scalar(4),
		Scalar(25)  / Scalar(216),
		Scalar(125) / Scalar(216)
	};
	CPPAD_ASSERT_KNOWN(
		M >= 1,
		"Error in Rosen34: the number of steps is less than one"
	);
	CPPAD_ASSERT_KNOWN(
		e.size() == xi.size(),
		"Error in Rosen34: size of e not equal to size of xi"
	);
	size_t i, j, k, l, m;             // indices

	size_t  n    = xi.size();         // number of components in X(t)
	Scalar  ns   = Scalar(double(M)); // number of steps as Scalar object
	Scalar  h    = (tf - ti) / ns;    // step size 
	Scalar  zero = Scalar(0);         // some constants
	Scalar  one  = Scalar(1);
	Scalar  two  = Scalar(2);

	// permutation vectors needed for LU factorization routine
	CppAD::vector<size_t> ip(n), jp(n);

	// vectors used to store values returned by F
	Vector E(n * n), Eg(n), f_t(n);
	Vector g(n * 3), x3(n), x4(n), xf(n), ftmp(n), xtmp(n), nan_vec(n);

	// initialize e = 0, nan_vec = nan
	for(i = 0; i < n; i++)
	{	e[i]       = zero;
		nan_vec[i] = nan(zero);
	}

	xf = xi;           // initialize solution
	for(m = 0; m < M; m++)
	{	// time at beginning of this interval
		Scalar t = ti * (Scalar(int(M - m)) / ns) 
		         + tf * (Scalar(int(m)) / ns);

		// value of x at beginning of this interval
		x3 = x4 = xf;

		// evaluate partial derivatives at beginning of this interval
		F.Ode_ind(t, xf, f_t);
		F.Ode_dep(t, xf, E);    // E = f_x
		if( hasnan(f_t) || hasnan(E) )
		{	e = nan_vec;
			return nan_vec;
		}

		// E = I - f_x * h / 2
		for(i = 0; i < n; i++)
		{	for(j = 0; j < n; j++)
				E[i * n + j] = - E[i * n + j] * h / two;
			E[i * n + i] += one;
		}

		// LU factor the matrix E
# ifndef NDEBUG
		int sign = LuFactor(ip, jp, E);
# else
		LuFactor(ip, jp, E);
# endif
		CPPAD_ASSERT_KNOWN(
			sign != 0,
			"Error in Rosen34: I - f_x * h / 2 not invertible"
		);

		// loop over integration steps
		for(k = 0; k < 3; k++)
		{	// set location for next function evaluation
			xtmp = xf; 
			for(l = 0; l < k; l++)
			{	// loop over previous function evaluations
				Scalar bkl = b[(k-1)*2 + l];
				for(i = 0; i < n; i++)
				{	// loop over elements of x
					xtmp[i] += bkl * g[i*3 + l] * h;
				}
			}
			// ftmp = F(t + a[k] * h, xtmp)
			F.Ode(t + a[k] * h, xtmp, ftmp); 
			if( hasnan(ftmp) )
			{	e = nan_vec;
				return nan_vec;
			}

			// Form Eg for this integration step
			for(i = 0; i < n; i++)
				Eg[i] = ftmp[i] + ct[k] * f_t[i] * h;
			for(l = 0; l < k; l++)
			{	for(i = 0; i < n; i++)
					Eg[i] += cg[(k-1)*3 + l] * g[i*3 + l];
			}

			// Solve the equation E * g = Eg
			LuInvert(ip, jp, E, Eg);

			// save solution and advance x3, x4
			for(i = 0; i < n; i++)
			{	g[i*3 + k]  = Eg[i];
				x3[i]      += h * d3[k] * Eg[i];
				x4[i]      += h * d4[k] * Eg[i];
			}
		}
		// Form Eg for last update to x4 only
		for(i = 0; i < n; i++)
			Eg[i] = ftmp[i] + ct[3] * f_t[i] * h;
		for(l = 0; l < 3; l++)
		{	for(i = 0; i < n; i++)
				Eg[i] += cg[2*3 + l] * g[i*3 + l];
		}

		// Solve the equation E * g = Eg
		LuInvert(ip, jp, E, Eg);

		// advance x4 and accumulate error bound
		for(i = 0; i < n; i++)
		{	x4[i] += h * d4[3] * Eg[i];

			// cant use abs because cppad.hpp may not be included
			Scalar diff = x4[i] - x3[i];
			if( diff < zero )
				e[i] -= diff;
			else	e[i] += diff;
		}

		// advance xf for this step using x4
		xf = x4;
	}
	return xf;
}
Beispiel #2
0
int LuSolve(
	size_t             n      ,
	size_t             m      ,
	const FloatVector &A      ,
	const FloatVector &B      ,
	FloatVector       &X      ,
	Float        &logdet      )
{
	// check numeric type specifications
	CheckNumericType<Float>();

	// check simple vector class specifications
	CheckSimpleVector<Float, FloatVector>();

	size_t        p;       // index of pivot element (diagonal of L)
	int     signdet;       // sign of the determinant
	Float     pivot;       // pivot element

	// the value zero
	const Float zero(0);

	// pivot row and column order in the matrix
	std::vector<size_t> ip(n);
	std::vector<size_t> jp(n);

	// -------------------------------------------------------
	CPPAD_ASSERT_KNOWN(
		size_t(A.size()) == n * n,
		"Error in LuSolve: A must have size equal to n * n"
	);
	CPPAD_ASSERT_KNOWN(
		size_t(B.size()) == n * m,
		"Error in LuSolve: B must have size equal to n * m"
	);
	CPPAD_ASSERT_KNOWN(
		size_t(X.size()) == n * m,
		"Error in LuSolve: X must have size equal to n * m"
	);
	// -------------------------------------------------------

	// copy A so that it does not change
	FloatVector Lu(A);

	// copy B so that it does not change
	X = B;

	// Lu factor the matrix A
	signdet = LuFactor(ip, jp, Lu);

	// compute the log of the determinant
	logdet  = Float(0);
	for(p = 0; p < n; p++)
	{	// pivot using the max absolute element
		pivot   = Lu[ ip[p] * n + jp[p] ];

		// check for determinant equal to zero
		if( pivot == zero )
		{	// abort the mission
			logdet = Float(0);
			return   0;
		}

		// update the determinant
		if( LeqZero ( pivot ) )
		{	logdet += log( - pivot );
			signdet = - signdet;
		}
		else	logdet += log( pivot );

	}

	// solve the linear equations
	LuInvert(ip, jp, Lu, X);

	// return the sign factor for the determinant
	return signdet;
}
void OdeGear(
	Fun          &F  , 
	size_t        m  ,
	size_t        n  ,
	const Vector &T  , 
	Vector       &X  ,
	Vector       &e  ) 
{
	// temporary indices
	size_t i, j, k;

	typedef typename Vector::value_type Scalar;

	// check numeric type specifications
	CheckNumericType<Scalar>();

	// check simple vector class specifications
	CheckSimpleVector<Scalar, Vector>();

	CPPAD_ASSERT_KNOWN(
		m >= 1,
		"OdeGear: m is less than one"
	);
	CPPAD_ASSERT_KNOWN(
		n > 0,
		"OdeGear: n is equal to zero"
	);
	CPPAD_ASSERT_KNOWN(
		size_t(T.size()) >= (m+1),
		"OdeGear: size of T is not greater than or equal (m+1)"
	);
	CPPAD_ASSERT_KNOWN(
		size_t(X.size()) >= (m+1) * n,
		"OdeGear: size of X is not greater than or equal (m+1) * n"
	);
	for(j = 0; j < m; j++) CPPAD_ASSERT_KNOWN(
		T[j] < T[j+1],
		"OdeGear: the array T is not monotone increasing"
	);

	// some constants
	Scalar zero(0);
	Scalar one(1);

	// vectors required by method
	Vector alpha(m + 1);
	Vector beta(m + 1);
	Vector f(n);
	Vector f_x(n * n);
	Vector x_m0(n);
	Vector x_m(n);
	Vector b(n);
	Vector A(n * n);

	// compute alpha[m] 
	alpha[m] = zero;
	for(k = 0; k < m; k++)
		alpha[m] += one / (T[m] - T[k]);

	// compute beta[m-1]
	beta[m-1] = one / (T[m-1] - T[m]);
	for(k = 0; k < m-1; k++)
		beta[m-1] += one / (T[m-1] - T[k]);


	// compute other components of alpha 
	for(j = 0; j < m; j++)
	{	// compute alpha[j]
		alpha[j] = one / (T[j] - T[m]);
		for(k = 0; k < m; k++)
		{	if( k != j )
			{	alpha[j] *= (T[m] - T[k]);
				alpha[j] /= (T[j] - T[k]);
			}
		}
	}

	// compute other components of beta 
	for(j = 0; j <= m; j++)
	{	if( j != m-1 )
		{	// compute beta[j]
			beta[j] = one / (T[j] - T[m-1]);
			for(k = 0; k <= m; k++)
			{	if( k != j && k != m-1 )
				{	beta[j] *= (T[m-1] - T[k]);
					beta[j] /= (T[j] - T[k]);
				}
			}
		}
	}

	// evaluate f(T[m-1], x_{m-1} )
	for(i = 0; i < n; i++)
		x_m[i] = X[(m-1) * n + i];
	F.Ode(T[m-1], x_m, f);

	// solve for x_m^0
	for(i = 0; i < n; i++)
	{	x_m[i] =  f[i];
		for(j = 0; j < m; j++)
			x_m[i] -= beta[j] * X[j * n + i];
		x_m[i] /= beta[m];
	}
	x_m0 = x_m;

	// evaluate partial w.r.t x of f(T[m], x_m^0)
	F.Ode_dep(T[m], x_m, f_x);

	// compute the matrix A = ( alpha[m] * I - f_x )
	for(i = 0; i < n; i++)
	{	for(j = 0; j < n; j++)
			A[i * n + j]  = - f_x[i * n + j];
		A[i * n + i] += alpha[m];
	}

	// LU factor (and overwrite) the matrix A
	int sign;
	CppAD::vector<size_t> ip(n) , jp(n);
	sign = LuFactor(ip, jp, A);
	CPPAD_ASSERT_KNOWN(
		sign != 0,
		"OdeGear: step size is to large"
	);

	// Iterations of Newton's method
	for(k = 0; k < 3; k++)
	{
		// only evaluate f( T[m] , x_m ) keep f_x during iteration
		F.Ode(T[m], x_m, f);

		// b = f + f_x x_m - alpha[0] x_0 - ... - alpha[m-1] x_{m-1}
		for(i = 0; i < n; i++)
		{	b[i]         = f[i];
			for(j = 0; j < n; j++)
				b[i]         -= f_x[i * n + j] * x_m[j];
			for(j = 0; j < m; j++)
				b[i] -= alpha[j] * X[ j * n + i ];
		}
		LuInvert(ip, jp, A, b);
		x_m = b;
	}

	// return estimate for x( t[k] ) and the estimated error bound
	for(i = 0; i < n; i++)
	{	X[m * n + i] = x_m[i];
		e[i]         = x_m[i] - x_m0[i];
		if( e[i] < zero )
			e[i] = - e[i];
	}
}