Ejemplo n.º 1
0
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

}
Ejemplo n.º 2
0
/**********************Normalize vector*******************************
 ********************************************************************/
void normalize_vec(VEC *vec)
{
    double norm;
    norm = v_norm2(vec);
    if(norm!=0)
    {
        sv_mlt((1/norm), vec, vec);
    }
    
}
Ejemplo n.º 3
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 */
}
Ejemplo n.º 4
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;
}
Ejemplo n.º 5
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;
}
Ejemplo n.º 6
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;
}
Ejemplo n.º 7
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;
}
Ejemplo n.º 8
0
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? */
}
Ejemplo n.º 9
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);
     
}
Ejemplo n.º 10
0
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);
}
Ejemplo n.º 11
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
}
Ejemplo n.º 12
0
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;
}
Ejemplo n.º 13
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;
}