/*! calculate the least-squares-least-norm solution for overdetermined or
underdetermined A*x=y using dgelss\n
*/
inline long dgematrix::dgelss(dcovector& B, dcovector& S, long& RANK,
const double RCOND =-1. )
{
#ifdef CPPL_VERBOSE
std::cerr << "# [MARK] dgematrix::dgelss(dcovector&, dcovector&, long&, const double)"
<< std::endl;
#endif//CPPL_VERBOSE
#ifdef CPPL_DEBUG
if(M!=B.L){
std::cerr << "[ERROR] dgematrix::dgelss"
<< "(dcovector&, dcovector&, long, double) " << std::endl
<< "These matrix and vector cannot be solved." << std::endl
<< "Your input was (" << M << "x" << N << ") and ("
<< B.L << ")." << std::endl;
exit(1);
}
#endif//CPPL_DEBUG
if(M<N){ //underdetermined
dcovector tmp(N);
for(long i=0; i<B.L; i++){ tmp(i)=B(i); }
B.clear();
swap(B,tmp);
}
S.resize(min(M,N));
long NRHS(1), LDA(M), LDB(B.L),
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"
<< "(dcovector&, docvector&, long, const double) "
<< "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 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;
}
