/*! 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;
}
/*! solve overdetermined or underdetermined A*X=Y using dgels
with the sum of residual squares output\n
The residual is set as the columnwise sum of residual squares
for overdetermined problems
while it is always zero for underdetermined problems.
*/
inline long dgematrix::dgels(dgematrix& mat, drovector& residual)
{
#ifdef CPPL_VERBOSE
std::cerr << "# [MARK] dgematrix::dgels(dgematrix&, drovector&)"
<< std::endl;
#endif//CPPL_VERBOSE
#ifdef CPPL_DEBUG
if(M!=mat.M){
std::cerr << "[ERROR] dgematrix::dgels(dgematrix&, drovector&) "
<< std::endl
<< "These two matrices cannot be solved." << std::endl
<< "Your input was (" << M << "x" << N << ") and ("
<< mat.M << "x" << mat.N << ")." << std::endl;
exit(1);
}
#endif//CPPL_DEBUG
residual.resize(mat.N); residual.zero();
if(M<N){ //underdetermined
dgematrix tmp(N,mat.N);
for(long i=0; i<mat.M; i++){ for(long j=0; j<mat.N; j++){
tmp(i,j) =mat(i,j);
}}
mat.clear();
swap(mat,tmp);
}
char TRANS('N');
long NRHS(mat.N), LDA(M), LDB(mat.M),
LWORK(min(M,N)+max(min(M,N),NRHS)), INFO(1);
double *WORK(new double[LWORK]);
dgels_(TRANS, M, N, NRHS, Array, LDA, mat.Array, LDB, WORK, LWORK, INFO);
delete [] WORK;
if(M>N){ //overdetermined
for(long i=0; i<residual.L; i++){ for(long j=0; j<M-N; j++){
residual(i) += std::pow(mat(N+j,i), 2.0);
}}
dgematrix tmp(N,mat.N);
for(long i=0; i<tmp.M; i++){ for(long j=0; j<tmp.N; j++){
tmp(i,j) =mat(i,j);
}}
mat.clear();
swap(mat,tmp);
}
if(INFO!=0){
std::cerr << "[WARNING] dgematrix::dgels(dgematrix&, drovector&) "
<< "Serious trouble happend. INFO = " << INFO << "."
<< std::endl;
}
return INFO;
}
int Unscented_Kalman_Filter::U_Cov(const vector<dcovector> &sP1,
const dcovector &m1,
const vector<dcovector> &sP2,
const dcovector &m2,
dgematrix & cov)
{
int N=sP1[0].l;
int M=sP2[0].l;
int i;
cov.resize(N,M);
cov.zero();
N=sP1.size();
drovector Xt = CPPL::t(sP2[0] - m2);
dcovector X = (sP1[0] - m1);
cov = X* Xt;
cov*=w_0c;
for(i=1;i<N;i++)
cov += w * ((sP1[i] - m1) * CPPL::t(sP2[i] - m2));
}
int cholesky(const dsymatrix & A, dgematrix &L){
const int N=A.n;
L.resize(N,N);
L.zero();
int i,j,k;
double sum,x;
x=(A(0,0));
if(x<=0.)
{
return 1;
}
L(0,0)=sqrt(x);
for (i=1;i<N;i++)
L(i,0)=A(0,i)/L(0,0);
i=1;
for (i=1;i<N;i++){
sum=0.;
for (k=0;k<i;k++)
sum+=L(i,k)*L(i,k);
x=(A(i,i)-sum);
if(x<=0.)
{
return 1;
}
L(i,i)=sqrt(x);
for (j=i+1;j<N;j++){
sum=0.;
for (k=0;k<i;k++)
sum+=L(i,k)*L(j,k);
L(j,i)=(A(i,j) - sum)/L(i,i);
}
}
return 0;
}
int eigen_decomposition(const dgematrix &A, dgematrix & P, dgematrix &D)
{
int N= A.n;
vector<double> wr, wi;
vector<dcovector > vr, vi;
dgematrix _A(A);
int i,j;
if(A.n != A.m)
{
cerr<<"BFilt :: Matrix must be square"<<endl;
return 1;
}
D.resize(N,N);
P.resize(N,N);
if(_A.dgeev(wr,wi,vr,vi))
return 1;
P.zero();
for(j=0; j<P.m; j++)
{
for(i=0; i<P.n; i++)
{
P(i,j) = vr[j](i);
}
}
D.zero();
for(i=0;i<N;i++)
{
if(wi[i] != 0)
{
cerr<<"BFilt :: Matrix must be real"<<endl;
return 1;
}
D(i,i) = wr[i];
}
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;
}