static void test_mgcr(ITER *ip, int i, MAT *Q, MAT *R)
#endif
{
VEC vt, vt1;
static MAT *R1 = MNULL;
static VEC *r = VNULL, *r1 = VNULL;
VEC *rr;
int k, j;
Real sm;
/* check Q*Q^T = I */
vt.dim = vt.max_dim = ip->b->dim;
vt1.dim = vt1.max_dim = ip->b->dim;
Q = m_resize(Q, i + 1, ip->b->dim);
R1 = m_resize(R1, i + 1, i + 1);
r = v_resize(r, ip->b->dim);
r1 = v_resize(r1, ip->b->dim);
MEM_STAT_REG(R1, TYPE_MAT);
MEM_STAT_REG(r, TYPE_VEC);
MEM_STAT_REG(r1, TYPE_VEC);
m_zero(R1);
for (k = 1; k <= i; k++)
for (j = 1; j <= i; j++) {
vt.ve = Q->me[k];
vt1.ve = Q->me[j];
R1->me[k][j] = in_prod(&vt, &vt1);
}
for (j = 1; j <= i; j++)
R1->me[j][j] -= 1.0;
#ifndef MEX
if (m_norm_inf(R1) > MACHEPS * ip->b->dim)
printf(" ! (mgcr:) m_norm_inf(Q*Q^T) = %g\n", m_norm_inf(R1));
#endif
/* check (r_i,Ap_j) = 0 for j <= i */
ip->Ax(ip->A_par, ip->x, r);
v_sub(ip->b, r, r);
rr = r;
if (ip->Bx) {
ip->Bx(ip->B_par, r, r1);
rr = r1;
}
#ifndef MEX
printf(" ||r|| = %g\n", v_norm2(rr));
#endif
sm = 0.0;
for (j = 1; j <= i; j++) {
vt.ve = Q->me[j];
sm = max(sm, in_prod(&vt,rr));
}
#ifndef MEX
if (sm >= MACHEPS * ip->b->dim)
printf(" ! (mgcr:) max_j (r,Ap_j) = %g\n", sm);
#endif
}
/**********************Normalize vector*******************************
********************************************************************/
void normalize_vec(VEC *vec)
{
double norm;
norm = v_norm2(vec);
if(norm!=0)
{
sv_mlt((1/norm), vec, vec);
}
}
VEC *iter_mgcr(ITER *ip)
#endif
{
STATIC VEC *As=VNULL, *beta=VNULL, *alpha=VNULL, *z=VNULL;
STATIC MAT *N=MNULL, *H=MNULL;
VEC *rr, v, s; /* additional pointer and structures */
Real nres; /* norm of a residual */
Real dd; /* coefficient d_i */
int i,j;
int done; /* if TRUE then stop the iterative process */
int dim; /* dimension of the problem */
/* ip cannot be NULL */
if (ip == INULL) error(E_NULL,"mgcr");
/* Ax, b and stopping criterion must be given */
if (! ip->Ax || ! ip->b || ! ip->stop_crit)
error(E_NULL,"mgcr");
/* at least one direction vector must exist */
if ( ip->k <= 0) error(E_BOUNDS,"mgcr");
/* if the vector x is given then b and x must have the same dimension */
if ( ip->x && ip->x->dim != ip->b->dim)
error(E_SIZES,"mgcr");
if (ip->eps <= 0.0) ip->eps = MACHEPS;
dim = ip->b->dim;
As = v_resize(As,dim);
alpha = v_resize(alpha,ip->k);
beta = v_resize(beta,ip->k);
MEM_STAT_REG(As,TYPE_VEC);
MEM_STAT_REG(alpha,TYPE_VEC);
MEM_STAT_REG(beta,TYPE_VEC);
H = m_resize(H,ip->k,ip->k);
N = m_resize(N,ip->k,dim);
MEM_STAT_REG(H,TYPE_MAT);
MEM_STAT_REG(N,TYPE_MAT);
/* if a preconditioner is defined */
if (ip->Bx) {
z = v_resize(z,dim);
MEM_STAT_REG(z,TYPE_VEC);
}
/* if x is NULL then it is assumed that x has
entries with value zero */
if ( ! ip->x ) {
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
}
/* v and s are additional pointers to rows of N */
/* they must have the same dimension as rows of N */
v.dim = v.max_dim = s.dim = s.max_dim = dim;
done = FALSE;
for (ip->steps = 0; ip->steps < ip->limit; ) {
(*ip->Ax)(ip->A_par,ip->x,As); /* As = A*x */
v_sub(ip->b,As,As); /* As = b - A*x */
rr = As; /* rr is an additional pointer */
/* if a preconditioner is defined */
if (ip->Bx) {
(*ip->Bx)(ip->B_par,As,z); /* z = B*(b-A*x) */
rr = z;
}
/* norm of the residual */
nres = v_norm2(rr);
dd = nres; /* dd = ||r_i|| */
/* check if the norm of the residual is zero */
if (ip->steps == 0) {
/* information for a user */
if (ip->info) (*ip->info)(ip,nres,As,rr);
ip->init_res = fabs(nres);
}
if (nres == 0.0) {
/* iterative process is finished */
done = TRUE;
break;
}
/* save this residual in the first row of N */
v.ve = N->me[0];
v_copy(rr,&v);
for (i = 0; i < ip->k && ip->steps < ip->limit; i++) {
ip->steps++;
v.ve = N->me[i]; /* pointer to a row of N (=s_i) */
/* note that we must use here &v, not v */
(*ip->Ax)(ip->A_par,&v,As);
rr = As; /* As = A*s_i */
if (ip->Bx) {
(*ip->Bx)(ip->B_par,As,z); /* z = B*A*s_i */
rr = z;
}
if (i < ip->k - 1) {
s.ve = N->me[i+1]; /* pointer to a row of N (=s_{i+1}) */
v_copy(rr,&s); /* s_{i+1} = B*A*s_i */
for (j = 0; j <= i-1; j++) {
v.ve = N->me[j+1]; /* pointer to a row of N (=s_{j+1}) */
/* beta->ve[j] = in_prod(&v,rr); */ /* beta_{j,i} */
/* modified Gram-Schmidt algorithm */
beta->ve[j] = in_prod(&v,&s); /* beta_{j,i} */
/* s_{i+1} -= beta_{j,i}*s_{j+1} */
v_mltadd(&s,&v,- beta->ve[j],&s);
}
/* beta_{i,i} = ||s_{i+1}||_2 */
beta->ve[i] = nres = v_norm2(&s);
if ( nres <= MACHEPS*ip->init_res) {
/* s_{i+1} == 0 */
i--;
done = TRUE;
break;
}
sv_mlt(1.0/nres,&s,&s); /* normalize s_{i+1} */
v.ve = N->me[0];
alpha->ve[i] = in_prod(&v,&s); /* alpha_i = (s_0 , s_{i+1}) */
}
else {
for (j = 0; j <= i-1; j++) {
v.ve = N->me[j+1]; /* pointer to a row of N (=s_{j+1}) */
beta->ve[j] = in_prod(&v,rr); /* beta_{j,i} */
}
nres = in_prod(rr,rr); /* rr = B*A*s_{k-1} */
for (j = 0; j <= i-1; j++)
nres -= beta->ve[j]*beta->ve[j];
if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
/* s_k is zero */
i--;
done = TRUE;
break;
}
if (nres < 0.0) { /* do restart */
i--;
ip->steps--;
break;
}
beta->ve[i] = sqrt(nres); /* beta_{k-1,k-1} */
v.ve = N->me[0];
alpha->ve[i] = in_prod(&v,rr);
for (j = 0; j <= i-1; j++)
alpha->ve[i] -= beta->ve[j]*alpha->ve[j];
alpha->ve[i] /= beta->ve[i]; /* alpha_{k-1} */
}
set_col(H,i,beta);
/* other method of computing dd */
/* if (fabs((double)alpha->ve[i]) > dd) {
nres = - dd*dd + alpha->ve[i]*alpha->ve[i];
nres = sqrt((double) nres);
if (ip->info) (*ip->info)(ip,-nres,VNULL,VNULL);
break;
} */
/* to avoid overflow/underflow in computing dd */
/* dd *= cos(asin((double)(alpha->ve[i]/dd))); */
nres = alpha->ve[i]/dd;
if (fabs(nres-1.0) <= MACHEPS*ip->init_res)
dd = 0.0;
else {
nres = 1.0 - nres*nres;
if (nres < 0.0) {
nres = sqrt((double) -nres);
if (ip->info) (*ip->info)(ip,-dd*nres,VNULL,VNULL);
break;
}
dd *= sqrt((double) nres);
}
if (ip->info) (*ip->info)(ip,dd,VNULL,VNULL);
if ( ip->stop_crit(ip,dd,VNULL,VNULL) ) {
/* stopping criterion is satisfied */
done = TRUE;
break;
}
} /* end of for */
if (i >= ip->k) i = ip->k - 1;
/* use (i+1) by (i+1) submatrix of H */
H = m_resize(H,i+1,i+1);
alpha = v_resize(alpha,i+1);
Usolve(H,alpha,alpha,0.0); /* c_i is saved in alpha */
for (j = 0; j <= i; j++) {
v.ve = N->me[j];
v_mltadd(ip->x,&v,alpha->ve[j],ip->x);
}
if (done) break; /* stop the iterative process */
alpha = v_resize(alpha,ip->k);
H = m_resize(H,ip->k,ip->k);
} /* end of while */
#ifdef THREADSAFE
V_FREE(As);
V_FREE(beta);
V_FREE(alpha);
V_FREE(z);
M_FREE(N);
M_FREE(H);
#endif
return ip->x; /* return the solution */
}
VEC *iter_gmres(ITER *ip)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL, *rhs = VNULL;
STATIC VEC *givs=VNULL, *givc=VNULL, *z = VNULL;
STATIC MAT *Q = MNULL, *R = MNULL;
VEC *rr, v, v1; /* additional pointers (not real vectors) */
int i,j, done;
Real nres;
/* Real last_h; */
if (ip == INULL)
error(E_NULL,"iter_gmres");
if ( ! ip->Ax || ! ip->b )
error(E_NULL,"iter_gmres");
if ( ! ip->stop_crit )
error(E_NULL,"iter_gmres");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_gmres");
if (ip->x != VNULL && ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_gmres");
if (ip->eps <= 0.0) ip->eps = MACHEPS;
r = v_resize(r,ip->k+1);
u = v_resize(u,ip->b->dim);
rhs = v_resize(rhs,ip->k+1);
givs = v_resize(givs,ip->k); /* Givens rotations */
givc = v_resize(givc,ip->k);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(rhs,TYPE_VEC);
MEM_STAT_REG(givs,TYPE_VEC);
MEM_STAT_REG(givc,TYPE_VEC);
R = m_resize(R,ip->k+1,ip->k);
Q = m_resize(Q,ip->k,ip->b->dim);
MEM_STAT_REG(R,TYPE_MAT);
MEM_STAT_REG(Q,TYPE_MAT);
if (ip->x == VNULL) { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
}
v.dim = v.max_dim = ip->b->dim; /* v and v1 are pointers to rows */
v1.dim = v1.max_dim = ip->b->dim; /* of matrix Q */
if (ip->Bx != (Fun_Ax)NULL) { /* if precondition is defined */
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
}
done = FALSE;
for (ip->steps = 0; ip->steps < ip->limit; ) {
/* restart */
ip->Ax(ip->A_par,ip->x,u); /* u = A*x */
v_sub(ip->b,u,u); /* u = b - A*x */
rr = u; /* rr is a pointer only */
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z); /* tmp = B*(b-A*x) */
rr = z;
}
nres = v_norm2(rr);
if (ip->steps == 0) {
if (ip->info) ip->info(ip,nres,VNULL,VNULL);
ip->init_res = nres;
}
if ( nres == 0.0 ) {
done = TRUE;
break;
}
v.ve = Q->me[0];
sv_mlt(1.0/nres,rr,&v);
v_zero(r);
v_zero(rhs);
rhs->ve[0] = nres;
for ( i = 0; i < ip->k && ip->steps < ip->limit; i++ ) {
ip->steps++;
v.ve = Q->me[i];
(ip->Ax)(ip->A_par,&v,u);
rr = u;
if (ip->Bx) {
(ip->Bx)(ip->B_par,u,z);
rr = z;
}
if (i < ip->k - 1) {
v1.ve = Q->me[i+1];
v_copy(rr,&v1);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* r->ve[j] = in_prod(&v,rr); */
/* modified Gram-Schmidt algorithm */
r->ve[j] = in_prod(&v,&v1);
v_mltadd(&v1,&v,-r->ve[j],&v1);
}
r->ve[i+1] = nres = v_norm2(&v1);
if (nres <= MACHEPS*ip->init_res) {
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
set_col(R,i,r);
done = TRUE;
break;
}
sv_mlt(1.0/nres,&v1,&v1);
}
else { /* i == ip->k - 1 */
/* Q->me[ip->k] need not be computed */
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
r->ve[j] = in_prod(&v,rr);
}
nres = in_prod(rr,rr) - in_prod(r,r);
if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
set_col(R,i,r);
done = TRUE;
break;
}
if (nres < 0.0) { /* do restart */
i--;
ip->steps--;
break;
}
r->ve[i+1] = sqrt(nres);
}
/* QR update */
/* last_h = r->ve[i+1]; */ /* for test only */
for (j = 0; j < i; j++)
rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
givens(r->ve[i],r->ve[i+1],&givc->ve[i],&givs->ve[i]);
rot_vec(r,i,i+1,givc->ve[i],givs->ve[i],r);
rot_vec(rhs,i,i+1,givc->ve[i],givs->ve[i],rhs);
set_col(R,i,r);
nres = fabs((double) rhs->ve[i+1]);
if (ip->info) ip->info(ip,nres,VNULL,VNULL);
if ( ip->stop_crit(ip,nres,VNULL,VNULL) ) {
done = TRUE;
break;
}
}
/* use ixi submatrix of R */
if (i >= ip->k) i = ip->k - 1;
R = m_resize(R,i+1,i+1);
rhs = v_resize(rhs,i+1);
/* test only */
/* test_gmres(ip,i,Q,R,givc,givs,last_h); */
Usolve(R,rhs,rhs,0.0); /* solve a system: R*x = rhs */
/* new approximation */
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
v_mltadd(ip->x,&v,rhs->ve[j],ip->x);
}
if (done) break;
/* back to old dimensions */
rhs = v_resize(rhs,ip->k+1);
R = m_resize(R,ip->k+1,ip->k);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
V_FREE(rhs);
V_FREE(givs);
V_FREE(givc);
V_FREE(z);
M_FREE(Q);
M_FREE(R);
#endif
return ip->x;
}
MAT *iter_arnoldi(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL;
VEC v; /* auxiliary vector */
int i,j;
Real h_val, c;
if (ip == INULL)
error(E_NULL,"iter_arnoldi");
if ( ! ip->Ax || ! Q || ! ip->x )
error(E_NULL,"iter_arnoldi");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_arnoldi");
if ( Q->n != ip->x->dim || Q->m != ip->k )
error(E_SIZES,"iter_arnoldi");
m_zero(Q);
H = m_resize(H,ip->k,ip->k);
m_zero(H);
u = v_resize(u,ip->x->dim);
r = v_resize(r,ip->k);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
v.dim = v.max_dim = ip->x->dim;
c = v_norm2(ip->x);
if ( c <= 0.0)
return H;
else {
v.ve = Q->me[0];
sv_mlt(1.0/c,ip->x,&v);
}
v_zero(r);
for ( i = 0; i < ip->k; i++ )
{
v.ve = Q->me[i];
u = (ip->Ax)(ip->A_par,&v,u);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* modified Gram-Schmidt */
r->ve[j] = in_prod(&v,u);
v_mltadd(u,&v,-r->ve[j],u);
}
h_val = v_norm2(u);
/* if u == 0 then we have an exact subspace */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
set_col(H,i,r);
if ( i == ip->k-1 )
{
*h_rem = h_val;
continue;
}
/* H->me[i+1][i] = h_val; */
m_set_val(H,i+1,i,h_val);
v.ve = Q->me[i+1];
sv_mlt(1.0/h_val,u,&v);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
#endif
return H;
}
MAT *iter_arnoldi_iref(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
STATIC VEC *u=VNULL, *r=VNULL, *s=VNULL, *tmp=VNULL;
VEC v; /* auxiliary vector */
int i,j;
Real h_val, c;
if (ip == INULL)
error(E_NULL,"iter_arnoldi_iref");
if ( ! ip->Ax || ! Q || ! ip->x )
error(E_NULL,"iter_arnoldi_iref");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_arnoldi_iref");
if ( Q->n != ip->x->dim || Q->m != ip->k )
error(E_SIZES,"iter_arnoldi_iref");
m_zero(Q);
H = m_resize(H,ip->k,ip->k);
m_zero(H);
u = v_resize(u,ip->x->dim);
r = v_resize(r,ip->k);
s = v_resize(s,ip->k);
tmp = v_resize(tmp,ip->x->dim);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(s,TYPE_VEC);
MEM_STAT_REG(tmp,TYPE_VEC);
v.dim = v.max_dim = ip->x->dim;
c = v_norm2(ip->x);
if ( c <= 0.0)
return H;
else {
v.ve = Q->me[0];
sv_mlt(1.0/c,ip->x,&v);
}
v_zero(r);
v_zero(s);
for ( i = 0; i < ip->k; i++ )
{
v.ve = Q->me[i];
u = (ip->Ax)(ip->A_par,&v,u);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
/* modified Gram-Schmidt */
r->ve[j] = in_prod(&v,u);
v_mltadd(u,&v,-r->ve[j],u);
}
h_val = v_norm2(u);
/* if u == 0 then we have an exact subspace */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
/* iterative refinement -- ensures near orthogonality */
do {
v_zero(tmp);
for (j = 0; j <= i; j++) {
v.ve = Q->me[j];
s->ve[j] = in_prod(&v,u);
v_mltadd(tmp,&v,s->ve[j],tmp);
}
v_sub(u,tmp,u);
v_add(r,s,r);
} while ( v_norm2(s) > 0.1*(h_val = v_norm2(u)) );
/* now that u is nearly orthogonal to Q, update H */
set_col(H,i,r);
/* check once again if h_val is zero */
if ( h_val <= 0.0 )
{
*h_rem = h_val;
return H;
}
if ( i == ip->k-1 )
{
*h_rem = h_val;
continue;
}
/* H->me[i+1][i] = h_val; */
m_set_val(H,i+1,i,h_val);
v.ve = Q->me[i+1];
sv_mlt(1.0/h_val,u,&v);
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(r);
V_FREE(s);
V_FREE(tmp);
#endif
return H;
}
VEC *iter_lsqr(ITER *ip)
#endif
{
STATIC VEC *u = VNULL, *v = VNULL, *w = VNULL, *tmp = VNULL;
Real alpha, beta, phi, phi_bar;
Real rho, rho_bar, rho_max, theta, nres;
Real s, c; /* for Givens' rotations */
int m, n;
if ( ! ip || ! ip->b || !ip->Ax || !ip->ATx )
error(E_NULL,"iter_lsqr");
if ( ip->x == ip->b )
error(E_INSITU,"iter_lsqr");
if (!ip->stop_crit || !ip->x)
error(E_NULL,"iter_lsqr");
if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;
m = ip->b->dim;
n = ip->x->dim;
u = v_resize(u,(unsigned int)m);
v = v_resize(v,(unsigned int)n);
w = v_resize(w,(unsigned int)n);
tmp = v_resize(tmp,(unsigned int)n);
MEM_STAT_REG(u,TYPE_VEC);
MEM_STAT_REG(v,TYPE_VEC);
MEM_STAT_REG(w,TYPE_VEC);
MEM_STAT_REG(tmp,TYPE_VEC);
if (ip->x != VNULL) {
ip->Ax(ip->A_par,ip->x,u); /* u = A*x */
v_sub(ip->b,u,u); /* u = b-A*x */
}
else { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
v_copy(ip->b,u); /* u = b */
}
beta = v_norm2(u);
if ( beta == 0.0 ) return ip->x;
sv_mlt(1.0/beta,u,u);
(ip->ATx)(ip->AT_par,u,v);
alpha = v_norm2(v);
if ( alpha == 0.0 ) return ip->x;
sv_mlt(1.0/alpha,v,v);
v_copy(v,w);
phi_bar = beta;
rho_bar = alpha;
rho_max = 1.0;
for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) {
tmp = v_resize(tmp,m);
(ip->Ax)(ip->A_par,v,tmp);
v_mltadd(tmp,u,-alpha,u);
beta = v_norm2(u);
sv_mlt(1.0/beta,u,u);
tmp = v_resize(tmp,n);
(ip->ATx)(ip->AT_par,u,tmp);
v_mltadd(tmp,v,-beta,v);
alpha = v_norm2(v);
sv_mlt(1.0/alpha,v,v);
rho = sqrt(rho_bar*rho_bar+beta*beta);
if ( rho > rho_max )
rho_max = rho;
c = rho_bar/rho;
s = beta/rho;
theta = s*alpha;
rho_bar = -c*alpha;
phi = c*phi_bar;
phi_bar = s*phi_bar;
/* update ip->x & w */
if ( rho == 0.0 )
error(E_BREAKDOWN,"iter_lsqr");
v_mltadd(ip->x,w,phi/rho,ip->x);
v_mltadd(v,w,-theta/rho,w);
nres = fabs(phi_bar*alpha*c)*rho_max;
if (ip->info) ip->info(ip,nres,w,VNULL);
if (ip->steps == 0) ip->init_res = nres;
if ( ip->stop_crit(ip,nres,w,VNULL) ) break;
}
#ifdef THREADSAFE
V_FREE(u);
V_FREE(v);
V_FREE(w);
V_FREE(tmp);
#endif
return ip->x;
}
static int reml(VEC *Y, MAT *X, MAT **Vk, int n_k, int max_iter,
double fit_limit, VEC *teta) {
volatile int n_iter = 0;
int i;
volatile double rel_step = DBL_MAX;
VEC *rhs = VNULL;
VEC *dteta = VNULL;
MAT *Vw = MNULL, *Tr_m = MNULL, *VinvIminAw = MNULL;
Vw = m_resize(Vw, X->m, X->m);
VinvIminAw = m_resize(VinvIminAw, X->m, X->m);
rhs = v_resize(rhs, n_k);
Tr_m = m_resize(Tr_m, n_k, n_k);
dteta = v_resize(dteta, n_k);
while (n_iter < max_iter && rel_step > fit_limit) {
print_progress(n_iter, max_iter);
n_iter++;
dteta = v_copy(teta, dteta);
/* fill Vw, calc VinvIminAw, rhs; */
for (i = 0, m_zero(Vw); i < n_k; i++)
ms_mltadd(Vw, Vk[i], teta->ve[i], Vw); /* Vw = Sum_i teta[i]*V[i] */
VinvIminAw = calc_VinvIminAw(Vw, X, VinvIminAw, n_iter == 1);
calc_rhs_Tr_m(n_k, Vk, VinvIminAw, Y, rhs, Tr_m);
/* Tr_m * teta = Rhs; symmetric, solve for teta: */
LDLfactor(Tr_m);
LDLsolve(Tr_m, rhs, teta);
if (DEBUG_VGMFIT) {
printlog("teta_%d [", n_iter);
for (i = 0; i < teta->dim; i++)
printlog(" %g", teta->ve[i]);
printlog("] -(log.likelyhood): %g\n",
calc_ll(Vw, X, Y, n_k));
}
v_sub(teta, dteta, dteta); /* dteta = teta_prev - teta_curr */
if (v_norm2(teta) == 0.0)
rel_step = 0.0;
else
rel_step = v_norm2(dteta) / v_norm2(teta);
} /* while (n_iter < gl_iter && rel_step > fit_limit) */
print_progress(max_iter, max_iter);
if (n_iter == gl_iter)
pr_warning("No convergence after %d iterations", n_iter);
if (DEBUG_VGMFIT) { /* calculate and report covariance matrix */
/* first, update to current est */
for (i = 0, m_zero(Vw); i < n_k; i++)
ms_mltadd(Vw, Vk[i], teta->ve[i], Vw); /* Vw = Sum_i teta[i]*V[i] */
VinvIminAw = calc_VinvIminAw(Vw, X, VinvIminAw, 0);
calc_rhs_Tr_m(n_k, Vk, VinvIminAw, Y, rhs, Tr_m);
m_inverse(Tr_m, Tr_m);
sm_mlt(2.0, Tr_m, Tr_m); /* Var(YAY)=2tr(AVAV) */
printlog("Lower bound of parameter covariance matrix:\n");
m_logoutput(Tr_m);
printlog("# Negative log-likelyhood: %g\n", calc_ll(Vw, X, Y, n_k));
}
m_free(Vw);
m_free(VinvIminAw);
m_free(Tr_m);
v_free(rhs);
v_free(dteta);
return (n_iter < max_iter && rel_step < fit_limit); /* converged? */
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
void expandstate(MAT *q_x_err, VEC *x_full, MAT *q_x_expanded)
{
VEC *q_temp, *q_err_vec, *q_err_quat;
double temp, n_err_states, n_states;
int i, j;
n_states = 7+6;
n_err_states = 6+6;
q_temp = v_get(4);
v_zero(q_temp);
q_err_vec = v_get(3);
q_err_quat = v_get(4);
for(j=0; j<n_err_states; j++)
{
for(i=0; i<3; i++)
{
q_err_vec->ve[i] = q_x_err->me[i][j];
}
temp = v_norm2(q_err_vec);
if(temp<=1 ){
q_err_quat->ve[0] = q_err_vec->ve[0];
q_err_quat->ve[1] = q_err_vec->ve[1];
q_err_quat->ve[2] = q_err_vec->ve[2];
q_err_quat->ve[3] = sqrt(1-temp);
}
else//baad main daala hai
{
q_err_quat->ve[0] = (q_err_vec->ve[0])/(sqrt(1+temp));
q_err_quat->ve[1] = (q_err_vec->ve[1])/(sqrt(1+temp));
q_err_quat->ve[2] = (q_err_vec->ve[2])/(sqrt(1+temp));
q_err_quat->ve[3] = 1/sqrt(1+temp);
}
quat_mul(q_err_quat, x_full, q_temp);
for(i=0; i<4; i++)
{
q_x_expanded->me[i][j] = q_temp->ve[i];
}
for(i=4; i<7; i++)
{
q_x_expanded->me[i][j] = q_x_err->me[i-1][j] + x_full->ve[i];
}
for(i=7; i<10; i++)
{
q_x_expanded->me[i][j] = q_x_err->me[i-1][j] + x_full->ve[i];
}
for(i=10; i<13; i++)
{
q_x_expanded->me[i][j] = q_x_err->me[i-1][j] + x_full->ve[i];
}
}
//
v_free(q_temp);
v_free(q_err_vec);
v_free(q_err_quat);
}
void iter_lanczos(ITER *ip, VEC *a, VEC *b, Real *beta2, MAT *Q)
#endif
{
int j;
STATIC VEC *v = VNULL, *w = VNULL, *tmp = VNULL;
Real alpha, beta, c;
if ( ! ip )
error(E_NULL,"iter_lanczos");
if ( ! ip->Ax || ! ip->x || ! a || ! b )
error(E_NULL,"iter_lanczos");
if ( ip->k <= 0 )
error(E_BOUNDS,"iter_lanczos");
if ( Q && ( Q->n < ip->x->dim || Q->m < ip->k ) )
error(E_SIZES,"iter_lanczos");
a = v_resize(a,(unsigned int)ip->k);
b = v_resize(b,(unsigned int)(ip->k-1));
v = v_resize(v,ip->x->dim);
w = v_resize(w,ip->x->dim);
tmp = v_resize(tmp,ip->x->dim);
MEM_STAT_REG(v,TYPE_VEC);
MEM_STAT_REG(w,TYPE_VEC);
MEM_STAT_REG(tmp,TYPE_VEC);
beta = 1.0;
v_zero(a);
v_zero(b);
if (Q) m_zero(Q);
/* normalise x as w */
c = v_norm2(ip->x);
if (c <= MACHEPS) { /* ip->x == 0 */
*beta2 = 0.0;
return;
}
else
sv_mlt(1.0/c,ip->x,w);
(ip->Ax)(ip->A_par,w,v);
for ( j = 0; j < ip->k; j++ )
{
/* store w in Q if Q not NULL */
if ( Q ) set_row(Q,j,w);
alpha = in_prod(w,v);
a->ve[j] = alpha;
v_mltadd(v,w,-alpha,v);
beta = v_norm2(v);
if ( beta == 0.0 )
{
*beta2 = 0.0;
return;
}
if ( j < ip->k-1 )
b->ve[j] = beta;
v_copy(w,tmp);
sv_mlt(1/beta,v,w);
sv_mlt(-beta,tmp,v);
(ip->Ax)(ip->A_par,w,tmp);
v_add(v,tmp,v);
}
*beta2 = beta;
#ifdef THREADSAFE
V_FREE(v); V_FREE(w); V_FREE(tmp);
#endif
}
static int fit_GaussNewton(VARIOGRAM *vp, PERM *p, LM *lm, int iter, int *bounded) {
double s = 0.0, x, y, z;
int i, j, n_fit, model, fit_ranges = 0;
IVEC *fit = NULL;
VEC *start = NULL;
if (p->size == 0)
return 1;
fit = iv_resize(fit, 2 * vp->n_models);
/* index fit parameters: parameter fit->ive[j] corresponds to model i */
for (i = n_fit = 0; i < vp->n_models; i++) {
if (vp->part[i].fit_sill)
fit->ive[n_fit++] = i;
if (vp->part[i].fit_range) {
fit->ive[n_fit++] = i + vp->n_models; /* large -->> ranges */
fit_ranges = 1;
}
}
if (n_fit == 0) {
iv_free(fit);
return 0;
}
fit = iv_resize(fit, n_fit); /* shrink to fit */
lm->X = m_resize(lm->X, p->size, n_fit);
lm->y = v_resize(lm->y, p->size);
start = v_resize(start, n_fit);
for (i = 0; i < n_fit; i++) {
if (fit->ive[i] < vp->n_models) {
model = fit->ive[i];
start->ve[i] = vp->part[model].sill;
} else {
model = fit->ive[i] - vp->n_models;
start->ve[i] = vp->part[model].range[0];
}
}
for (i = 0; i < p->size; i++) {
x = vp->ev->direction.x * vp->ev->dist[p->pe[i]];
y = vp->ev->direction.y * vp->ev->dist[p->pe[i]];
z = vp->ev->direction.z * vp->ev->dist[p->pe[i]];
/* fill y with current residuals: */
if (is_variogram(vp))
s = get_semivariance(vp, x, y, z);
else
s = get_covariance(vp, x, y, z);
lm->y->ve[i] = vp->ev->gamma[p->pe[i]] - s;
/* fill X: */
for (j = 0; j < n_fit; j++) { /* cols */
if (fit->ive[j] < vp->n_models) {
model = fit->ive[j];
ME(lm->X, i, j) = (is_variogram(vp) ?
UnitSemivariance(vp->part[model],x,y,z) :
UnitCovariance(vp->part[model],x,y,z));
} else {
model = fit->ive[j] - vp->n_models;
ME(lm->X, i, j) = (is_variogram(vp) ?
da_Semivariance(vp->part[model],x,y,z) :
-da_Semivariance(vp->part[model],x,y,z));
}
}
}
if (iter == 0 && fill_weights(vp, p, lm)) {
iv_free(fit);
v_free(start);
return 1;
}
lm->has_intercept = 1; /* does not affect the fit */
lm = calc_lm(lm); /* solve WLS eqs. for beta */
if (DEBUG_FIT) {
Rprintf("beta: ");
v_logoutput(lm->beta);
}
if (lm->is_singular) {
iv_free(fit);
v_free(start);
return 1;
}
if (fit_ranges) {
s = v_norm2(lm->beta) / v_norm2(start);
if (s > 0.2) {
/* don't allow steps > 20% ---- */
sv_mlt(0.2 / s, lm->beta, lm->beta);
*bounded = 1;
} else
*bounded = 0; /* a `free', voluntary step */
} else /* we're basically doing linear regression here: */
*bounded = 0;
for (i = 0; i < n_fit; i++) {
if (fit->ive[i] < vp->n_models) {
model = fit->ive[i];
vp->part[model].sill = start->ve[i] + lm->beta->ve[i];
} else {
model = fit->ive[i] - vp->n_models;;
vp->part[model].range[0] = start->ve[i] + lm->beta->ve[i];
}
}
iv_free(fit);
v_free(start);
return 0;
}
double QRcondest(const MAT *QR)
#endif
{
STATIC VEC *y=VNULL;
Real norm1, norm2, sum, tmp1, tmp2;
int i, j, limit;
if ( QR == MNULL )
error(E_NULL,"QRcondest");
limit = min(QR->m,QR->n);
for ( i = 0; i < limit; i++ )
if ( QR->me[i][i] == 0.0 )
return HUGE_VAL;
y = v_resize(y,limit);
MEM_STAT_REG(y,TYPE_VEC);
/* use the trick for getting a unit vector y with ||R.y||_inf small
from the LU condition estimator */
for ( i = 0; i < limit; i++ )
{
sum = 0.0;
for ( j = 0; j < i; j++ )
sum -= QR->me[j][i]*y->ve[j];
sum -= (sum < 0.0) ? 1.0 : -1.0;
y->ve[i] = sum / QR->me[i][i];
}
UTmlt(QR,y,y);
/* now apply inverse power method to R^T.R */
for ( i = 0; i < 3; i++ )
{
tmp1 = v_norm2(y);
sv_mlt(1/tmp1,y,y);
UTsolve(QR,y,y,0.0);
tmp2 = v_norm2(y);
sv_mlt(1/v_norm2(y),y,y);
Usolve(QR,y,y,0.0);
}
/* now compute approximation for ||R^{-1}||_2 */
norm1 = sqrt(tmp1)*sqrt(tmp2);
/* now use complementary approach to compute approximation to ||R||_2 */
for ( i = limit-1; i >= 0; i-- )
{
sum = 0.0;
for ( j = i+1; j < limit; j++ )
sum += QR->me[i][j]*y->ve[j];
y->ve[i] = (sum >= 0.0) ? 1.0 : -1.0;
y->ve[i] = (QR->me[i][i] >= 0.0) ? y->ve[i] : - y->ve[i];
}
/* now apply power method to R^T.R */
for ( i = 0; i < 3; i++ )
{
tmp1 = v_norm2(y);
sv_mlt(1/tmp1,y,y);
Umlt(QR,y,y);
tmp2 = v_norm2(y);
sv_mlt(1/tmp2,y,y);
UTmlt(QR,y,y);
}
norm2 = sqrt(tmp1)*sqrt(tmp2);
/* printf("QRcondest: norm1 = %g, norm2 = %g\n",norm1,norm2); */
#ifdef THREADSAFE
V_FREE(y);
#endif
return norm1*norm2;
}
