/*! 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; }