Exemplo n.º 1
0
/* v_lincomb -- returns sum_i a[i].v[i], a[i] real, v[i] vectors */
VEC	*v_lincomb(int n,VEC *v[],Real a[],VEC *out)
/* number of a's and v's */
{
	int	i;

	if ( ! a || ! v )
		error(E_NULL,"v_lincomb");
	if ( n <= 0 )
		return VNULL;

	for ( i = 1; i < n; i++ )
		if ( out == v[i] )
		    error(E_INSITU,"v_lincomb");

	out = sv_mlt(a[0],v[0],out);
	for ( i = 1; i < n; i++ )
	{
		if ( ! v[i] )
			error(E_NULL,"v_lincomb");
		if ( v[i]->dim != out->dim )
			error(E_SIZES,"v_lincomb");
		out = v_mltadd(out,v[i],a[i],out);
	}

	return out;
}
Exemplo n.º 2
0
/* v_linlist -- linear combinations taken from a list of arguments;
   calling:
      v_linlist(out,v1,a1,v2,a2,...,vn,an,NULL);
   where vi are vectors (VEC *) and ai are numbers (double)
*/
VEC  *v_linlist(VEC *out,VEC *v1,double a1,...)
{
   va_list ap;
   VEC *par;
   double a_par;

   if ( ! v1 )
     return VNULL;
   
   va_start(ap, a1);
   out = sv_mlt(a1,v1,out);
   
   while (par = va_arg(ap,VEC *)) {   /* NULL ends the list*/
      a_par = va_arg(ap,double);
      if (a_par == 0.0) continue;
      if ( out == par )		
	error(E_INSITU,"v_linlist");
      if ( out->dim != par->dim )	
	error(E_SIZES,"v_linlist");

      if (a_par == 1.0)
	out = v_add(out,par,out);
      else if (a_par == -1.0)
	out = v_sub(out,par,out);
      else
	out = v_mltadd(out,par,a_par,out); 
   } 
   
   va_end(ap);
   return out;
}
Exemplo n.º 3
0
double rk4(double t, VEC *x, double h, VEC *Torq)
{
    VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL;
    VEC *temp=VNULL;
    double step_size=0.5*h;

    if ( x == VNULL )
        error(E_NULL,"rk4");

    v1   = v_get(x->dim);
    v2   = v_get(x->dim);
    v3   = v_get(x->dim);
    v4   = v_get(x->dim);
    temp = v_get(x->dim);
   
    Sy_m(x, Torq, v1); 
    v_mltadd(x,v1,step_size,temp);    
    Sy_m(temp, Torq, v2); 
    v_mltadd(x,v2,step_size,temp);   
    Sy_m(temp, Torq, v3);
    step_size = h;
    v_mltadd(x, v3, step_size, temp);        
    Sy_m(temp, Torq, v4);
    temp = v_copy(v1,temp);           
    v_mltadd(temp,v2,2.0,temp); 
    v_mltadd(temp,v3,2.0,temp); 
    v_add(temp,v4,temp);
    v_mltadd(x,temp,(h/6.0),x);   
     return t+h;   
     
    V_FREE(v1);
    V_FREE(v2);
    V_FREE(v3);
    V_FREE(v4);
    V_FREE(temp);
    
}
Exemplo n.º 4
0
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 */
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
VEC	*iter_cgs(ITER *ip, VEC *r0)
#endif
{
    STATIC VEC  *p = VNULL, *q = VNULL, *r = VNULL, *u = VNULL;
    STATIC VEC  *v = VNULL, *z = VNULL;
    VEC  *tmp;
    Real	alpha, beta, nres, rho, old_rho, sigma, inner;

    if (ip == INULL)
        error(E_NULL,"iter_cgs");
    if (!ip->Ax || !ip->b || !r0)
        error(E_NULL,"iter_cgs");
    if ( ip->x == ip->b )
        error(E_INSITU,"iter_cgs");
    if (!ip->stop_crit)
        error(E_NULL,"iter_cgs");
    if ( r0->dim != ip->b->dim )
        error(E_SIZES,"iter_cgs");

    if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;

    p = v_resize(p,ip->b->dim);
    q = v_resize(q,ip->b->dim);
    r = v_resize(r,ip->b->dim);
    u = v_resize(u,ip->b->dim);
    v = v_resize(v,ip->b->dim);

    MEM_STAT_REG(p,TYPE_VEC);
    MEM_STAT_REG(q,TYPE_VEC);
    MEM_STAT_REG(r,TYPE_VEC);
    MEM_STAT_REG(u,TYPE_VEC);
    MEM_STAT_REG(v,TYPE_VEC);

    if (ip->Bx) {
        z = v_resize(z,ip->b->dim);
        MEM_STAT_REG(z,TYPE_VEC);
    }

    if (ip->x != VNULL) {
        if (ip->x->dim != ip->b->dim)
            error(E_SIZES,"iter_cgs");
        ip->Ax(ip->A_par,ip->x,v);    		/* v = A*x */
        if (ip->Bx) {
            v_sub(ip->b,v,v);			/* v = b - A*x */
            (ip->Bx)(ip->B_par,v,r);		/* r = B*(b-A*x) */
        }
        else v_sub(ip->b,v,r);			/* r = b-A*x */
    }
    else {  /* ip->x == 0 */
        ip->x = v_get(ip->b->dim);		/* x == 0 */
        ip->shared_x = FALSE;
        if (ip->Bx) (ip->Bx)(ip->B_par,ip->b,r);    /* r = B*b */
        else v_copy(ip->b,r);                       /* r = b */
    }

    v_zero(p);
    v_zero(q);
    old_rho = 1.0;

    for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) {

        inner = in_prod(r,r);
        nres = sqrt(fabs(inner));
        if (ip->steps == 0) ip->init_res = nres;

        if (ip->info) ip->info(ip,nres,r,VNULL);
        if ( ip->stop_crit(ip,nres,r,VNULL) ) break;

        rho = in_prod(r0,r);
        if ( old_rho == 0.0 )
            error(E_BREAKDOWN,"iter_cgs");
        beta = rho/old_rho;
        v_mltadd(r,q,beta,u);
        v_mltadd(q,p,beta,v);
        v_mltadd(u,v,beta,p);

        (ip->Ax)(ip->A_par,p,q);
        if (ip->Bx) {
            (ip->Bx)(ip->B_par,q,z);
            tmp = z;
        }
        else tmp = q;

        sigma = in_prod(r0,tmp);
        if ( sigma == 0.0 )
            error(E_BREAKDOWN,"iter_cgs");
        alpha = rho/sigma;
        v_mltadd(u,tmp,-alpha,q);
        v_add(u,q,v);

        (ip->Ax)(ip->A_par,v,u);
        if (ip->Bx) {
            (ip->Bx)(ip->B_par,u,z);
            tmp = z;
        }
        else tmp = u;

        v_mltadd(r,tmp,-alpha,r);
        v_mltadd(ip->x,v,alpha,ip->x);

        old_rho = rho;
    }

#ifdef THREADSAFE
    V_FREE(p);
    V_FREE(q);
    V_FREE(r);
    V_FREE(u);
    V_FREE(v);
    V_FREE(z);
#endif

    return ip->x;
}
Exemplo n.º 7
0
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;
}
Exemplo n.º 8
0
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;
}
Exemplo n.º 9
0
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;
}
Exemplo n.º 10
0
VEC  *iter_cgne(ITER *ip)
#endif
{
    STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL;
    Real	alpha, beta, inner, old_inner, nres;
    VEC *rr1;   /* pointer only */

    if (ip == INULL)
        error(E_NULL,"iter_cgne");
    if (!ip->Ax || ! ip->ATx || !ip->b)
        error(E_NULL,"iter_cgne");
    if ( ip->x == ip->b )
        error(E_INSITU,"iter_cgne");
    if (!ip->stop_crit)
        error(E_NULL,"iter_cgne");

    if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;

    r = v_resize(r,ip->b->dim);
    p = v_resize(p,ip->b->dim);
    q = v_resize(q,ip->b->dim);

    MEM_STAT_REG(r,TYPE_VEC);
    MEM_STAT_REG(p,TYPE_VEC);
    MEM_STAT_REG(q,TYPE_VEC);

    z = v_resize(z,ip->b->dim);
    MEM_STAT_REG(z,TYPE_VEC);

    if (ip->x) {
        if (ip->x->dim != ip->b->dim)
            error(E_SIZES,"iter_cgne");
        ip->Ax(ip->A_par,ip->x,p);    		/* p = A*x */
        v_sub(ip->b,p,z);		 		/* z = b - A*x */
    }
    else {  /* ip->x == 0 */
        ip->x = v_get(ip->b->dim);
        ip->shared_x = FALSE;
        v_copy(ip->b,z);
    }
    rr1 = z;
    if (ip->Bx) {
        (ip->Bx)(ip->B_par,rr1,p);
        rr1 = p;
    }
    (ip->ATx)(ip->AT_par,rr1,r);		/* r = A^T*B*(b-A*x)  */


    old_inner = 0.0;
    for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ )
    {
        rr1 = r;
        if ( ip->Bx ) {
            (ip->Bx)(ip->B_par,r,z);		/* rr = B*r */
            rr1 = z;
        }

        inner = in_prod(r,rr1);
        nres = sqrt(fabs(inner));
        if (ip->info) ip->info(ip,nres,r,rr1);
        if (ip->steps == 0) ip->init_res = nres;
        if ( ip->stop_crit(ip,nres,r,rr1) ) break;

        if ( ip->steps )	/* if ( ip->steps > 0 ) ... */
        {
            beta = inner/old_inner;
            p = v_mltadd(rr1,p,beta,p);
        }
        else		/* if ( ip->steps == 0 ) ... */
        {
            beta = 0.0;
            p = v_copy(rr1,p);
            old_inner = 0.0;
        }
        (ip->Ax)(ip->A_par,p,q);     /* q = A*p */
        if (ip->Bx) {
            (ip->Bx)(ip->B_par,q,z);
            (ip->ATx)(ip->AT_par,z,q);
            rr1 = q;			/* q = A^T*B*A*p */
        }
        else {
            (ip->ATx)(ip->AT_par,q,z);	/* z = A^T*A*p */
            rr1 = z;
        }

        alpha = inner/in_prod(rr1,p);
        v_mltadd(ip->x,p,alpha,ip->x);
        v_mltadd(r,rr1,-alpha,r);
        old_inner = inner;
    }

#ifdef THREADSAFE
    V_FREE(r);
    V_FREE(p);
    V_FREE(q);
    V_FREE(z);
#endif

    return ip->x;
}
Exemplo n.º 11
0
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);
     
}
Exemplo n.º 12
0
VEC  *iter_cg1(ITER *ip)
#endif
{
   STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL;
   Real	alpha;
   double inner,nres;
   VEC *rr;   /* rr == r or rr == z */
   
   if (ip == INULL)
     error(E_NULL,"iter_cg");
   if (!ip->Ax || !ip->b)
     error(E_NULL,"iter_cg");
   if ( ip->x == ip->b )
     error(E_INSITU,"iter_cg");
   if (!ip->stop_crit)
     error(E_NULL,"iter_cg");
   
   if ( ip->eps <= 0.0 )
     ip->eps = MACHEPS;
   
   r = v_resize(r,ip->b->dim);
   p = v_resize(p,ip->b->dim);
   q = v_resize(q,ip->b->dim);
   
   MEM_STAT_REG(r,TYPE_VEC);
   MEM_STAT_REG(p,TYPE_VEC);
   MEM_STAT_REG(q,TYPE_VEC);
   
   if (ip->Bx != (Fun_Ax)NULL) {
      z = v_resize(z,ip->b->dim);
      MEM_STAT_REG(z,TYPE_VEC);
      rr = z;
   }
   else rr = r;
   
   if (ip->x != VNULL) {
      if (ip->x->dim != ip->b->dim)
	error(E_SIZES,"iter_cg");
      ip->Ax(ip->A_par,ip->x,p);    		/* p = A*x */
      v_sub(ip->b,p,r);		 		/* r = b - A*x */
   }
   else {  /* ip->x == 0 */
      ip->x = v_get(ip->b->dim);
      ip->shared_x = FALSE;
      v_copy(ip->b,r);
   }
   
   if (ip->Bx) (ip->Bx)(ip->B_par,r,p);
   else v_copy(r,p);
   
   inner = in_prod(p,r);
   nres = sqrt(fabs(inner));
   if (ip->info) ip->info(ip,nres,r,p);
   if ( nres == 0.0) return ip->x;
   
   for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ )
   {
      ip->Ax(ip->A_par,p,q);
      inner = in_prod(q,p);
      if (sqrt(fabs(inner)) <= MACHEPS*ip->init_res)
	error(E_BREAKDOWN,"iter_cg1");

      alpha = in_prod(p,r)/inner;
      v_mltadd(ip->x,p,alpha,ip->x);
      v_mltadd(r,q,-alpha,r);
      
      rr = r;
      if (ip->Bx) {
	 ip->Bx(ip->B_par,r,z);
	 rr = z;
      }
      
      nres = in_prod(r,rr);
      if (nres < 0.0) {
	 warning(WARN_RES_LESS_0,"iter_cg");
	 break;
      }
      nres = sqrt(fabs(nres));
      if (ip->info) ip->info(ip,nres,r,z);
      if (ip->steps == 0) ip->init_res = nres;
      if ( ip->stop_crit(ip,nres,r,z) ) break;
      
      alpha = -in_prod(rr,q)/inner;
      v_mltadd(rr,p,alpha,p);
      
   }

#ifdef	THREADSAFE
   V_FREE(r);   V_FREE(p);   V_FREE(q);   V_FREE(z);
#endif

   return ip->x;
}
Exemplo n.º 13
0
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
}
Exemplo n.º 14
0
VEC  *iter_cg(ITER *ip)
#endif
{
   STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL;
   Real	alpha, beta, inner, old_inner, nres;
   VEC *rr;   /* rr == r or rr == z */
   
   if (ip == INULL)
     error(E_NULL,"iter_cg");
   if (!ip->Ax || !ip->b)
     error(E_NULL,"iter_cg");
   if ( ip->x == ip->b )
     error(E_INSITU,"iter_cg");
   if (!ip->stop_crit)
     error(E_NULL,"iter_cg");
   
   if ( ip->eps <= 0.0 )
     ip->eps = MACHEPS;
   
   r = v_resize(r,ip->b->dim);
   p = v_resize(p,ip->b->dim);
   q = v_resize(q,ip->b->dim);
   
   MEM_STAT_REG(r,TYPE_VEC);
   MEM_STAT_REG(p,TYPE_VEC);
   MEM_STAT_REG(q,TYPE_VEC);
   
   if (ip->Bx != (Fun_Ax)NULL) {
      z = v_resize(z,ip->b->dim);
      MEM_STAT_REG(z,TYPE_VEC);
      rr = z;
   }
   else rr = r;
   
   if (ip->x != VNULL) {
      if (ip->x->dim != ip->b->dim)
	error(E_SIZES,"iter_cg");
      ip->Ax(ip->A_par,ip->x,p);    		/* p = A*x */
      v_sub(ip->b,p,r);		 		/* r = b - A*x */
   }
   else {  /* ip->x == 0 */
      ip->x = v_get(ip->b->dim);
      ip->shared_x = FALSE;
      v_copy(ip->b,r);
   }
   
   old_inner = 0.0;
   for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ )
   {
      if ( ip->Bx )
	(ip->Bx)(ip->B_par,r,rr);		/* rr = B*r */
      
      inner = in_prod(rr,r);
      nres = sqrt(fabs(inner));
      if (ip->info) ip->info(ip,nres,r,rr);
      if (ip->steps == 0) ip->init_res = nres;
      if ( ip->stop_crit(ip,nres,r,rr) ) break;
      
      if ( ip->steps )	/* if ( ip->steps > 0 ) ... */
      {
	 beta = inner/old_inner;
	 p = v_mltadd(rr,p,beta,p);
      }
      else		/* if ( ip->steps == 0 ) ... */
      {
	 beta = 0.0;
	 p = v_copy(rr,p);
	 old_inner = 0.0;
      }
      (ip->Ax)(ip->A_par,p,q);     /* q = A*p */
      alpha = in_prod(p,q);
      if (sqrt(fabs(alpha)) <= MACHEPS*ip->init_res) 
	error(E_BREAKDOWN,"iter_cg");
      alpha = inner/alpha;
      v_mltadd(ip->x,p,alpha,ip->x);
      v_mltadd(r,q,-alpha,r);
      old_inner = inner;
   }

#ifdef	THREADSAFE
   V_FREE(r);   V_FREE(p);   V_FREE(q);   V_FREE(z);
#endif

   return ip->x;
}