size_t cholesky_decompose(const MATRIX& A, TRIA& L)
{
using namespace ublas;
assert( A.size1() == A.size2() );
assert( A.size1() == L.size1() );
assert( A.size2() == L.size2() );
const size_t n = A.size1();
for (size_t k=0 ; k < n; k++) {
double qL_kk = A(k,k) - inner_prod( project( row(L, k), range(0, k) ),
project( row(L, k), range(0, k) ) );
if (qL_kk <= 0) {
return 1 + k;
} else {
double L_kk = sqrt( qL_kk );
L(k,k) = L_kk;
matrix_column<TRIA> cLk(L, k);
project( cLk, range(k+1, n) )
= ( project( column(A, k), range(k+1, n) )
- prod( project(L, range(k+1, n), range(0, k)),
project(row(L, k), range(0, k) ) ) ) / L_kk;
}
}
return 0;
}
void cholesky_invert (MATRIX &M)
{
using namespace ublas;
typedef typename MATRIX::size_type size_type;
typedef typename MATRIX::value_type value_type;
size_type size = M.size1();
// determine the inverse of the lower traingular matrix
for (size_type i = 0; i < size; ++ i) {
M(i,i) = 1 / M(i,i);
for (size_type j = i+1; j < size; ++ j) {
value_type elem(0);
for (size_type k = i; k < j; ++ k) {
elem -= M(j,k)*M(k,i);
}
M(j,i) = elem / M(j,j);
}
}
// multiply the upper and lower inverses together
M = prod(trans(triangular_adaptor<MATRIX,lower>(M)), triangular_adaptor<MATRIX,lower>(M));
}
size_t cholesky_decompose(MATRIX& A)
{
using namespace ublas;
const MATRIX& A_c(A);
const size_t n = A.size1();
for (size_t k=0 ; k < n; k++) {
double qL_kk = A_c(k,k) - inner_prod( project( row(A_c, k), range(0, k) ),
project( row(A_c, k), range(0, k) ) );
if (qL_kk <= 0) {
return 1 + k;
} else {
double L_kk = sqrt( qL_kk );
matrix_column<MATRIX> cLk(A, k);
project( cLk, range(k+1, n) )
= ( project( column(A_c, k), range(k+1, n) )
- prod( project(A_c, range(k+1, n), range(0, k)),
project(row(A_c, k), range(0, k) ) ) ) / L_kk;
A(k,k) = L_kk;
}
}
return 0;
}
size_t incomplete_cholesky_decompose(MATRIX& A)
{
using namespace ublas;
typedef typename MATRIX::value_type T;
// read access to a const matrix is faster
const MATRIX& A_c(A);
const size_t n = A.size1();
for (size_t k=0 ; k < n; k++) {
double qL_kk = A_c(k,k) - inner_prod( project( row( A_c, k ), range(0, k) ),
project( row( A_c, k ), range(0, k) ) );
if (qL_kk <= 0) {
return 1 + k;
} else {
double L_kk = sqrt( qL_kk );
// aktualisieren
for (size_t i = k+1; i < A.size1(); ++i) {
T* Aik = A.find_element(i, k);
if (Aik != 0) {
*Aik = ( *Aik - inner_prod( project( row( A_c, k ), range(0, k) ),
project( row( A_c, i ), range(0, k) ) ) ) / L_kk;
}
}
A(k,k) = L_kk;
}
}
return 0;
}
template<typename MATRIX> MATRIX expm_pad(const MATRIX &H, typename type_traits<typename MATRIX::value_type>::real_type t = 1.0, const int p = 6){
typedef typename MATRIX::value_type value_type;
typedef typename MATRIX::size_type size_type;
typedef typename type_traits<value_type>::real_type real_value_type;
assert(H.size1() == H.size2());
assert(p >= 1);
const size_type n = H.size1();
const identity_matrix<value_type> I(n);
matrix<value_type> U(n,n),H2(n,n),P(n,n),Q(n,n);
real_value_type norm = 0.0;
// Calcuate Pade coefficients
vector<real_value_type> c(p+1);
c(0)=1;
for(size_type i = 0; i < (size_type) p; ++i)
c(i+1) = c(i) * ((p - i)/((i + 1.0) * (2.0 * p - i)));
// Calcuate the infinty norm of H, which is defined as the largest row sum of a matrix
for(size_type i=0; i<n; ++i) {
real_value_type temp = 0.0;
for(size_type j = 0; j < n; j++)
temp += std::abs(H(i, j));
norm = t * std::max<real_value_type>(norm, temp);
}
// If norm = 0, and all H elements are not NaN or infinity but zero,
// then U should be identity.
if (norm == 0.0) {
bool all_H_are_zero = true;
for(size_type i = 0; i < n; i++)
for(size_type j = 0; j < n; j++)
if( H(i,j) != value_type(0.0) )
all_H_are_zero = false;
if( all_H_are_zero == true ) return I;
// Some error happens, H has elements which are NaN or infinity.
std::cerr<<"Null input error in the template expm_pad.\n";
// std::cout << "Null INPUT : " << H <<"\n";
exit(0);
}
// Scaling, seek s such that || H*2^(-s) || < 1/2, and set scale = 2^(-s)
int s = 0;
real_value_type scale = 1.0;
if(norm > 0.5) {
s = std::max<int>(0, static_cast<int>((log(norm) / log(2.0) + 2.0)));
scale /= real_value_type(std::pow(2.0, s));
U.assign((scale * t) * H); // Here U is used as temp value due to that H is const
}
else
U.assign(H);
// Horner evaluation of the irreducible fraction, see the following ref above.
// Initialise P (numerator) and Q (denominator)
H2.assign( prod(U, U) );
Q.assign( c(p)*I );
P.assign( c(p-1)*I );
size_type odd = 1;
for( size_type k = p - 1; k > 0; --k) {
( odd == 1 ) ?
( Q = ( prod(Q, H2) + c(k-1) * I ) ) :
( P = ( prod(P, H2) + c(k-1) * I ) ) ;
odd = 1 - odd;
}
( odd == 1 ) ? ( Q = prod(Q, U) ) : ( P = prod(P, U) );
Q -= P;
// In origine expokit package, they use lapack ZGESV to obtain inverse matrix,
// and in that ZGESV routine, it uses LU decomposition for obtaing inverse matrix.
// Since in ublas, there is no matrix inversion template, I simply use the build-in
// LU decompostion package in ublas, and back substitute by myself.
// Implement Matrix Inversion
permutation_matrix<size_type> pm(n);
int res = lu_factorize(Q, pm);
if( res != 0) {
std::cerr << "Matrix inversion error in the template expm_pad.\n";
exit(0);
}
// H2 is not needed anymore, so it is temporary used as identity matrix for substituting.
H2.assign(I);
lu_substitute(Q, pm, H2);
(odd == 1) ?
( U.assign( -(I + real_value_type(2.0) * prod(H2, P))) ):
( U.assign( I + real_value_type(2.0) * prod(H2, P) ) );
// Squaring
for(size_type i = 0; i < (size_type) s; ++i)
U = (prod(U,U));
return U;
}
