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