/*! compute the singular value decomposition (SVD)\n
The arguments are dcocector S, dgematrix U and VT.
All of them need not to be initialized.
S, U and VT are overwitten and become singular values,
left singular vectors,
and right singular vectors respectively.
This matrix also overwritten.
*/
inline long dgematrix::dgesvd(dcovector& S, dgematrix& U, dgematrix& VT)
{
#ifdef CPPL_VERBOSE
std::cerr << "# [MARK] dgematrix::dgesvd(dcovector&, dgematrix&, dgematrix&)"
<< std::endl;
#endif//CPPL_VERBOSE
char JOBU('A'), JOBVT('A');
long LDA(M), LDU(M), LDVT(N),
LWORK(max(3*min(M,N)+max(M,N),5*min(M,N))), INFO(1);
double *WORK(new double[LWORK]);
S.resize(min(M,N)); U.resize(LDU,M); VT.resize(LDVT,N);
dgesvd_(JOBU, JOBVT, M, N, Array, LDA, S.Array, U.Array,
LDU, VT.Array, LDVT, WORK, LWORK, INFO);
delete [] WORK;
if(INFO!=0){
std::cerr << "[WARNING] dgematrix::dgesvd"
<< "(dceovector&, dgematrix&, dcovector&) "
<< "Serious trouble happend. INFO = " << INFO << "."
<< std::endl;
}
return INFO;
}
int Unscented_Kalman_Filter::U_Mean(const vector<dcovector> &sP,
dcovector & mean)
{
int N=sP[0].l;
int i;
mean.resize(N);
mean=sP[0]*w_0;
N=sP.size();
for (i=1;i<N;i++)
mean +=w * (sP[i]);
}
int multivariate_normal_draw(dcovector & X, gsl_rng * r, const dsymatrix & Q){
X.resize(Q.n);
dgematrix L;
int i;
for (i=0;i<X.l;i++)
X(i)=gsl_ran_gaussian(r,1.);
if( cholesky(Q,L)){
X.zero();
return 1;
}
X=L*X;
return 0;
}
/*! calculate the least-squares-least-norm solution for overdetermined or
underdetermined A*x=y using dgelss\n
*/
inline long dgematrix::dgelss(dgematrix& B, dcovector& S, long& RANK,
const double RCOND =-1. )
{
#ifdef CPPL_VERBOSE
std::cerr << "# [MARK] dgematrix::dgelss(dgematrix&, dcovector&, long& const double)"
<< std::endl;
#endif//CPPL_VERBOSE
#ifdef CPPL_DEBUG
if(M!=B.M){
std::cerr << "[ERROR] dgematrix::dgelss"
<< "(dgematrix&, dcovector&, long&, const double) " << std::endl
<< "These matrix and vector cannot be solved." << std::endl
<< "Your input was (" << M << "x" << N << ") and ("
<< B.M << "x" << B.N << ")." << std::endl;
exit(1);
}
#endif//CPPL_DEBUG
if(M<N){ //underdetermined
dgematrix tmp(N,B.N);
for(long i=0; i<B.M; i++){
for(long j=0; j<B.N; j++){
tmp(i,j)=B(i,j);
}
}
B.clear();
swap(B,tmp);
}
S.resize(min(M,N));
long NRHS(B.N), LDA(M), LDB(B.M),
LWORK(3*min(M,N)+max(max(2*min(M,N),max(M,N)), NRHS)), INFO(1);
double *WORK(new double[LWORK]);
dgelss_(M, N, NRHS, Array, LDA, B.Array, LDB, S.Array,
RCOND, RANK, WORK, LWORK, INFO);
delete [] WORK;
if(INFO!=0){
std::cerr << "[WARNING] dgematrix::dgelss"
<< "(dgematrix&, docvector&, long, const double) "
<< "Serious trouble happend. INFO = " << INFO << "."
<< std::endl;
}
return INFO;
}
int multivariate_normal_draw(dcovector & X, gsl_rng * r, const dgematrix & Q){
X.resize(Q.n);
dgematrix L;
int i;
int j;
int N=Q.m;
dsymatrix S(N);
for (i=0;i<X.l;i++)
X(i)=gsl_ran_gaussian(r,1.);
for (j=0;j<N;j++)
for(i=j;i<N;i++)
S(i,j)=Q(i,j);
if( cholesky(S,L)){
X.zero();
return 1;
}
X=L*X;
return 0;
}