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;
}