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