Пример #1
0
//--------------------------------------------------------------------------
void Omu_IntODE::solve(int kk, double tstart, double tend,
		       const Omu_VariableVec &x, const Omu_VariableVec &u,
		       Omu_Program *sys, Omu_DependentVec &Ft,
		       Omu_StateVec &xt)
{
  int i, j;

  _sys = sys;	// propagate to syseq()
  _xt_ptr = &xt;
  _Ft_ptr = &Ft;

  v_zero(_y);
    
  for (i = 0; i < _nd; i++) {
    _u[i] = xt[i];
  }
  for (i = 0; i < _n; i++) {
    _y[i] = xt[_nd + i];		// initial states
  }
  for (i = 0; i < _nu; i++) {
    _u[_nd + i] = u[i];
  }

  v_zero(_dxt);	// time derivatives passed to continuous

  if (_sa) {
    for (i = 0; i < _n; i++) {
      for (j = 0; j < _nx; j++) {
	_y[(1 + j) * _n + i] = xt.Sx[_nd + i][j];
      }
      for (j = 0; j < _nu; j++) {
	_y[(1 + _nx + j) * _n + i] = xt.Su[_nd + i][j];
      }
    }
    m_zero(_dxt.Sx);
    m_zero(_dxt.Su);
  }

  _kk = kk;	// propagate to syseq()

  ode_solve(tstart, _y, _u, tend);

  for (i = 0; i < _n; i++) {
    xt[_nd + i] = _y[i];
  }

  if (_sa) {
    for (i = 0; i < _n; i++) {
      for (j = 0; j < _nx; j++) {
	xt.Sx[_nd + i][j] = _y[(1 + j) * _n + i];
      }
      for (j = 0; j < _nu; j++) {
	xt.Su[_nd + i][j] = _y[(1 + _nx + j) * _n + i];
      }
    }
  }
}
Пример #2
0
/*******************************RK4***********************************
 *********************************************************************/
void skew_mat(MAT *skew_mat, VEC *w)
{
    	
	if((skew_mat->m !=3) || (skew_mat->m !=4)){
		m_resize(skew_mat,4,4);
		m_zero(skew_mat);
	}
	if(skew_mat->m == 4){
        
        skew_mat->me[0][1] = w->ve[2]; 
        skew_mat->me[0][2] = (-1)*w->ve[1];
        skew_mat->me[0][3] = w->ve[0]; 
        skew_mat->me[1][0] = (-1)*w->ve[2];
		skew_mat->me[1][2] = w->ve[0];
        skew_mat->me[1][3] = w->ve[1]; 
        skew_mat->me[2][0] = w->ve[1]; 
		skew_mat->me[2][1] = (-1)*w->ve[0];
		skew_mat->me[2][3] = w->ve[2];         
        skew_mat->me[3][0] = (-1)*w->ve[0];        
		skew_mat->me[3][1] = (-1)*w->ve[1];
        skew_mat->me[3][2] = (-1)*w->ve[2];
    }
	else if(skew_mat->m == 3){// not needed
        
        skew_mat->me[0][1] = w->ve[2]; 
        skew_mat->me[0][2] = (-1)*w->ve[1];
        skew_mat->me[1][2] = w->ve[0]; 
        skew_mat->me[1][0] = (-1)*w->ve[2];
        skew_mat->me[2][0] = w->ve[1]; 
        skew_mat->me[2][1] = (-1)*w->ve[0];
    }
    
}
Пример #3
0
void booz_sensors_model_mag_init( double time ) {

  bsm.mag = v_get(AXIS_NB);
  bsm.mag->ve[AXIS_X] = 0.;
  bsm.mag->ve[AXIS_Y] = 0.;
  bsm.mag->ve[AXIS_Z] = 0.;
  //  bsm.mag_resolution = BSM_MAG_RESOLUTION;

  bsm.mag_imu_to_sensor = m_get(AXIS_NB, AXIS_NB);
  VEC* tmp_eulers = v_get(AXIS_NB);
  tmp_eulers->ve[EULER_PHI]   = BSM_MAG_IMU_TO_SENSOR_PHI;
  tmp_eulers->ve[EULER_THETA] = BSM_MAG_IMU_TO_SENSOR_THETA;
  tmp_eulers->ve[EULER_PSI]   = BSM_MAG_IMU_TO_SENSOR_PSI;
  dcm_of_eulers (tmp_eulers, bsm.mag_imu_to_sensor );

  bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB);
  m_zero(bsm.mag_sensitivity);
  bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = BSM_MAG_SENSITIVITY_XX;
  bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_MAG_SENSITIVITY_YY;
  bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_MAG_SENSITIVITY_ZZ;

  bsm.mag_neutral = v_get(AXIS_NB);
  bsm.mag_neutral->ve[AXIS_X] = BSM_MAG_NEUTRAL_X;
  bsm.mag_neutral->ve[AXIS_Y] = BSM_MAG_NEUTRAL_Y;
  bsm.mag_neutral->ve[AXIS_Z] = BSM_MAG_NEUTRAL_Z;

  bsm.mag_noise_std_dev = v_get(AXIS_NB);
  bsm.mag_noise_std_dev->ve[AXIS_X] = BSM_MAG_NOISE_STD_DEV_X;
  bsm.mag_noise_std_dev->ve[AXIS_Y] = BSM_MAG_NOISE_STD_DEV_Y;
  bsm.mag_noise_std_dev->ve[AXIS_Z] = BSM_MAG_NOISE_STD_DEV_Z;

  bsm.mag_next_update = time;
  bsm.mag_available = FALSE;

}
Пример #4
0
MAT *XVXt_mlt(MAT *X, MAT *V, MAT *out) {
/* for a symmetric matrix V, return X V X' */
	static MAT *VXt = MNULL;
	int i, j, k;

	if (X==(MAT *)NULL || V==(MAT *)NULL )
		error(E_NULL, "XtVX_mlt");
	if (X->n != V->m)
		error(E_SIZES, "XtVX_mlt");
	if (V->m != V->n)
		error(E_SQUARE, "XtVX_mlt");

	out = m_resize(out, X->m, X->m);
	VXt = m_resize(VXt, V->m, X->n);
	m_zero(out);

	VXt = mmtr_mlt(V, X, VXt);
	for (i = 0; i < X->m; i++) {
		for (j = i; j < X->m; j++)
			for (k = 0; k < X->n; k++)
				out->me[i][j] += X->me[i][k] * VXt->me[k][j];
		for (j = 0; j <= i; j++) /* symmetry */
			out->me[i][j] = out->me[j][i];
	}
	return out;
}
Пример #5
0
struct mbuf *
m_free(struct mbuf *m)
{
	struct mbuf *n;

	if (m == NULL)
		return (NULL);

	mtx_enter(&mbstatmtx);
	mbstat.m_mtypes[m->m_type]--;
	mtx_leave(&mbstatmtx);

	n = m->m_next;
	if (m->m_flags & M_ZEROIZE) {
		m_zero(m);
		/* propagate M_ZEROIZE to the next mbuf in the chain */
		if (n)
			n->m_flags |= M_ZEROIZE;
	}
	if (m->m_flags & M_PKTHDR)
		m_tag_delete_chain(m);
	if (m->m_flags & M_EXT)
		m_extfree(m);

	pool_put(&mbpool, m);

	return (n);
}
Пример #6
0
void booz_sensors_model_accel_init(double time) {

  bsm.accel = v_get(AXIS_NB);
  bsm.accel->ve[AXIS_X] = 0.;
  bsm.accel->ve[AXIS_Y] = 0.;
  bsm.accel->ve[AXIS_Z] = 0.;
  bsm.accel_resolution = BSM_ACCEL_RESOLUTION;

  bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
  m_zero(bsm.accel_sensitivity);
  bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX;
  bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY;
  bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ;

  bsm.accel_neutral = v_get(AXIS_NB);
  bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X;
  bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y;
  bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z;

  bsm.accel_noise_std_dev = v_get(AXIS_NB);
  bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X;
  bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y;
  bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z;

  bsm.accel_bias = v_get(AXIS_NB);
  bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X;
  bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y;
  bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z;

  bsm.accel_next_update = time;
  bsm.accel_available = FALSE;

}
Пример #7
0
MAT	*mtrm_mlt(const MAT *A, const MAT *B, MAT *OUT)
#endif
{
	int	i, k, limit;
	/* Real	*B_row, *OUT_row, multiplier; */

	if ( ! A || ! B )
		error(E_NULL,"mmtr_mlt");
	if ( A == OUT || B == OUT )
		error(E_INSITU,"mtrm_mlt");
	if ( A->m != B->m )
		error(E_SIZES,"mmtr_mlt");
	if ( ! OUT || OUT->m != A->n || OUT->n != B->n )
		OUT = m_resize(OUT,A->n,B->n);

	limit = B->n;
	m_zero(OUT);
	for ( k = 0; k < A->m; k++ )
		for ( i = 0; i < A->n; i++ )
		{
		    if ( A->me[k][i] != 0.0 )
			__mltadd__(OUT->me[i],B->me[k],A->me[k][i],(int)limit);
		    /**************************************************
		    multiplier = A->me[k][i];
		    OUT_row = OUT->me[i];
		    B_row   = B->me[k];
		    for ( j = 0; j < limit; j++ )
			*(OUT_row++) += multiplier*(*B_row++);
		    **************************************************/
		}

	return OUT;
}
Пример #8
0
MAT *band2mat(const BAND *bA, MAT *A)
#endif
{
   int i,j,l,n,n1;
   int lb, ub;
   Real **bmat;

   if ( !bA )
     error(E_NULL,"band2mat");
   if ( bA->mat == A )
     error(E_INSITU,"band2mat");

   ub = bA->ub;
   lb = bA->lb;
   n = bA->mat->n;
   n1 = n-1;
   bmat = bA->mat->me;

   A = m_resize(A,n,n);
   m_zero(A);

   for (j=0; j < n; j++)
     for (i=min(n1,j+lb),l=lb+j-i; i >= max(0,j-ub); i--,l++)
       A->me[i][j] = bmat[l][j];

   return A;
}
Пример #9
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

}
Пример #10
0
BAND	*bd_zero(BAND *A)
#endif
{
  if ( ! A )
    error(E_NULL,"bd_zero");

  m_zero(A->mat);
  return A;
}
Пример #11
0
/* mat_id -- set A to being closest to identity matrix as possible
	-- i.e. A[i][j] == 1 if i == j and 0 otherwise */
MAT	*m_ident(MAT *A)
{
	int	i, size;

	if ( A == MNULL )
		error(E_NULL,"m_ident");

	m_zero(A);
	size = min(A->m,A->n);
	for ( i = 0; i < size; i++ )
		A->me[i][i] = 1.0;

	return A;
}
Пример #12
0
void m_invert(matrix m,matrix &dest)
{
	int i,j;
	float d;

	d = m_det(m);
  if (d == 0.0)
  {
      m_zero(dest);
      return;
  }
  for (i = 0;i<=3;i++)
    for (j = 0;j<=3;j++)
      dest[i][j] = m_signedsubdet(m, i, j);
  m_trans(dest);
  m_mults(dest, 1/d, dest);

}
Пример #13
0
MAT	*m_mlt(const MAT *A, const MAT *B, MAT *OUT)
#endif
{
	unsigned int	i, /* j, */ k, m, n, p;
	Real	**A_v, **B_v /*, *B_row, *OUT_row, sum, tmp */;

	if ( A==(MAT *)NULL || B==(MAT *)NULL )
		error(E_NULL,"m_mlt");
	if ( A->n != B->m )
		error(E_SIZES,"m_mlt");
	if ( A == OUT || B == OUT )
		error(E_INSITU,"m_mlt");
	m = A->m;	n = A->n;	p = B->n;
	A_v = A->me;		B_v = B->me;

	if ( OUT==(MAT *)NULL || OUT->m != A->m || OUT->n != B->n )
		OUT = m_resize(OUT,A->m,B->n);

/****************************************************************
	for ( i=0; i<m; i++ )
		for  ( j=0; j<p; j++ )
		{
			sum = 0.0;
			for ( k=0; k<n; k++ )
				sum += A_v[i][k]*B_v[k][j];
			OUT->me[i][j] = sum;
		}
****************************************************************/
	m_zero(OUT);
	for ( i=0; i<m; i++ )
		for ( k=0; k<n; k++ )
		{
		    if ( A_v[i][k] != 0.0 )
		        __mltadd__(OUT->me[i],B_v[k],A_v[i][k],(int)p);
		    /**************************************************
		    B_row = B_v[k];	OUT_row = OUT->me[i];
		    for ( j=0; j<p; j++ )
			(*OUT_row++) += tmp*(*B_row++);
		    **************************************************/
		}

	return OUT;
}
Пример #14
0
MAT *XtdX_mlt(MAT *X, VEC *d, MAT *out) {
/* for a diagonal matrix in d, return X' d X */
	int i, j, k;

	if (X==(MAT *)NULL || d==(VEC *)NULL )
		error(E_NULL, "XtVX_mlt");
	if (X->m != d->dim)
		error(E_SIZES, "XtVX_mlt");

	out = m_resize(out, X->n, X->n);
	m_zero(out);

	for (i = 0; i < X->n; i++) {
		for (j = i; j < X->n; j++)
			for (k = 0; k < X->m; k++)
				out->me[i][j] += X->me[k][i] * X->me[k][j] * d->ve[k];
		for (j = 0; j <= i; j++) /* symmetry */
			out->me[i][j] = out->me[j][i];
	}
	return out;
}
Пример #15
0
//--------------------------------------------------------------------------
void Omu_Integrator::setup_struct(int k,
				  const Omu_VariableVec &x,
				  const Omu_VariableVec &u,
				  const Omu_DependentVec &Ft)
{
  // initialize Jacobians for high-level integrator interface

  if (k >= _K) {
    m_error(E_INTERN, "Omu_Integrator::setup_struct"
	    " that was called with wrong integrator setup");
  }

  int i;
  Omu_DepVec &Fc = _Fcs[k];

  init_dims(k, x, u, Ft);

  Fc.size(_n, _n, 0, _n, 0, _nx+_nu);
  m_move(Ft.Jx, _nd, _nd, _n, _n, Fc.Jx, 0, 0);
  m_move(Ft.Jdx, _nd, _nd, _n, _n, Fc.Jdx, 0, 0);
  m_zero(Fc.Jq); // zero Jq wrt. continuous states as Jx gets chained with Sx
  m_move(Ft.Jx, _nd, 0, _n, _nd, Fc.Jq, 0, 0);
  m_move(Ft.Ju, _nd, 0, _n, _nu, Fc.Jq, 0, _nx);

  Fc.c_setup = true;
  for (i = 0; i < _n; i++) {
    int wrt = 0;
    if (Ft.is_linear_element(_nd + i, Omu_Dependent::WRT_x))
      wrt |= Omu_Dependent::WRT_x;
    if (Ft.is_linear_element(_nd + i, Omu_Dependent::WRT_dx))
      wrt |= Omu_Dependent::WRT_dx;
    if ((_nd == 0 || Ft.is_linear_element(_nd + i, Omu_Dependent::WRT_x)) &&
	Ft.is_linear_element(_nd + i, Omu_Dependent::WRT_u))
      wrt |= Omu_Dependent::WRT_q;
    Fc.set_linear_element(i, wrt);
  }
  Fc.analyze_struct();
  Fc.c_setup = false;
}
Пример #16
0
// alternative implementation calling high-level _sys->continuous
//--------------------------------------------------------------------------
void Omu_IntODE::syseq_forward(double t, const VECP y, const VECP u,
			       VECP f)
{
#ifdef OMU_WITH_ADOLC
  int i, j;
  Omu_DependentVec &Ft = *_Ft_ptr;

  //
  // form a vector of independent variables
  //

  for (i = 0; i < _nd; i++) {
    _x[i] = u[i];
  }
  for (i = 0; i < _n; i++) {
    _x[_nd + i] = y[i];
    _x[_nd + _n + i] = 0.0;	// yprime[i]
  }
  for (i = 0; i < _nu; i++) {
    _x[_nd + 2 * _n + i] = u[_nd + i];
  }
      
  //
  // evaluate residual
  //

  //  adoublev ax(_nd + _n);
  static adoublev ax; ax.alloc(_nd + _n);
  //  adoublev adx(_nd + _n);
  static adoublev adx; adx.alloc(_nd + _n);
  //  adoublev au(_nu);
  static adoublev au; au.alloc(_nu);
  //  adoublev aF(_nd + _n);
  static adoublev aF; aF.alloc(_nd + _n);

  for (i = 0; i < _nd; i++)
    adx[i] = 0.0;
  for (i = _nd; i < _nxt; i++)
    aF[i] = 0.0;

  if (_sa)
    trace_on(3);	// tape 3
  ax <<= _x->ve;
  for (i = 0; i < _n; i++)
    adx[_nd + i] <<= _x->ve[_nd + _n + i];
  au <<= _x->ve + _nd + 2 * _n;

  _sys->continuous(_kk, t, ax, au, adx, aF);

  for (i = _nd; i < _nxt; i++) {
    aF[i] >>= f[i - _nd];
    f[i - _nd] /= -Ft.Jdx[i][i];
  }
      
  if (_sa) {
    trace_off();

    int nindep = _nd + 2 * _n + _nu;
    int npar = _nx + _nu;

    m_zero(_X2);
    for (i = 0; i < _nd; i++) {
      _X2[i][i] = 1.0;
    }
    for (i = 0; i < _n; i++) {
      for (j = 0; j < npar; j++) {
	_X2[_nd + i][j] = y[(1 + j) * _n + i];
	_X2[_nd + _n + i][j] = 0.0; // yprime[(1 + j) * _n + i];
      }
    }
    for (i = 0; i < _nu; i++) {
      _X2[_nd + 2 * _n + i][_nd + _n + i] = 1.0;
    }
      
    forward(3, _n, nindep, npar, _x->ve, _X2->me, f->ve, _Y2->me);

    for (i = _nd; i < _nxt; i++) {
      f[i - _nd] /= -Ft.Jdx[i][i];
      for (j = 0; j < npar; j++) {
	f[(1 + j) * _n + i - _nd] = _Y2[i - _nd][j] / -Ft.Jdx[i][i];
      }
    }
  }

  _res_evals++;
  if (_sa)
    _sen_evals++;
#else
  m_error(E_NULL, "Omu_IntODE::syseq_forward: was compiled without ADOL-C");
#endif
}
Пример #17
0
/*  Routine to take the matrix given and calculate the log-
 * determinant, by calling LU decomposition routine and
 * then multiplying down diagonals. Returns the
 * log-determinant calculated.*/
double * determinant(void){
  int a,max,c;
  extern int branches;
  double *det;
  extern int mode;
  extern int nodecount;
  extern double **expect;
  extern double **rootedexpect;
  extern int individual;
  extern int interesting_branches[];
  extern int is_kappa;
  double **matrix;
  MAT * matrix2;

  is_kappa=0;
  if(ISMODE(HKY) && NOTMODE(NOKAPPA))
    is_kappa=1;
  matrix=expect;
  max=branches;
  if(ISMODE(ROOTED)){ /*  If want rooted tree then create new*/
    planttree(expect,rootedexpect);   /* matrix*/
    matrix=rootedexpect;
    max=nodecount+2;
    if(ISMODE(NODEASROOT))
      max=nodecount+1;
  }

  if(ISMODE(MATRICES)){  /* If want intermediate matrices dumped*/
    dump(matrix,max+is_kappa,"Full matrix");
  }

  if(ISMODE(INDIVIDUAL)){ /*  We want information about some, but
                           * not all of the elements*/
    if(NOTMODE(DETINDIV)){
      det=calloc(individual+is_kappa,sizeof(double));
      for(a=0;a<individual;a++)
        det[a]=matrix[interesting_branches[a]][interesting_branches[a]];
      if(is_kappa==1)
	det[individual]=matrix[max][max];
      is_kappa=0;
      return det;
    }

    /*  Case - we want the determinate of the sub-matrix formed 
     * by several parameters*/
    /*  Get memory for new matrix*/
    matrix2 = m_get(individual+is_kappa,individual+is_kappa);
    if(NULL==matrix2){
	    nomemory();
    }
    m_zero(matrix2);


    /*  Creates the sub-matrix from the original expected information
     * matrix*/
    for(a=0;a<individual;a++)
      for(c=0;c<individual;c++)
	matrix2->me[a][c]=matrix[interesting_branches[a]][interesting_branches[c]];
    if(is_kappa==1){
      matrix2->me[individual][individual]=matrix[max][max];
    }
    
    max=individual;
    if(ISMODE(MATRICES))
      dump(matrix2->me,max,"Sub-matrix to be calculated");
  } else {
      matrix2 = m_get(max,max);
      if(NULL==matrix2){
          nomemory();
      }
      m_zero(matrix2);
      for ( a=0 ; a<max ; a++){
          for ( c=0 ; c<max ; c++){
              matrix2->me[a][c] = matrix[a][c];
          }
      }
  }
 
  /*  Perform LU decomposition on whichever matrix we've been handed*/
  det=calloc(1+is_kappa,sizeof(double));
  matrix2=CHfactor(matrix2);

  /*  The determinant of the matrix is the product of
   * the diagonal elements of the decomposed form*/
  for(a=0;a<max;a++){
    det[0] += 2.0 * log(matrix2->me[a][a]);
  }
  if(is_kappa==1){
    det[1] = 2.0 * log(matrix2->me[max][max]);
  }

  M_FREE(matrix2);

  return det;
}
Пример #18
0
long lsfn(
    double *xd, double *yd, double *sy,       /* data */
    long nd,                                  /* number of data points */
    long nf,                                  /* y = a_0 + a_1*x ... a_nf*x^nf */
    double *coef,                             /* place to put co-efficients */
    double *s_coef,                           /* and their sigmas */
    double *chi,                              /* place to put reduced chi-squared */
    double *diff                              /* place to put difference table    */
    )
{
    long i, j, nt, unweighted;
    double xp, *x_i, x0;
    static MATRIX *X, *Y, *Yp, *C, *C_1, *Xt, *A, *Ca,
                  *XtC, *XtCX, *T, *Tt, *TC;

    nt = nf + 1;
    if (nd<nt) {
        printf("error: insufficient data for requested order of fit\n");
        printf("(%ld data points, %ld terms in fit)\n", nd, nt);
        exit(1);
        }

    unweighted = 1;
    if (sy)
      for (i=1; i<nd; i++)
        if (sy[i]!=sy[0]) {
          unweighted = 0;
          break;
        }

    /* allocate matrices */
    m_alloc(&X, nd, nt);
    m_alloc(&Y, nd, 1);
    m_alloc(&Yp, nd, 1);
    m_alloc(&Xt, nt, nd);
    if (!unweighted) {
        m_alloc(&C, nd, nd);
        m_alloc(&C_1, nd, nd);
        m_zero(C);
        m_zero(C_1);
        }
    m_alloc(&A, nt, 1);
    m_alloc(&Ca, nt, nt);
    m_alloc(&XtC, nt, nd);
    m_alloc(&XtCX, nt, nt);
    m_alloc(&T, nt, nd);
    m_alloc(&Tt, nd, nt);
    m_alloc(&TC, nt, nd);

    /* Compute X, Y, C, C_1.  X[i][j] = (xd[i])^j. Y[i][0] = yd[i].
     * C   = delta(i,j)*sy[i]^2  (covariance matrix of yd)
     * C_1 = INV(C)
     */
    for (i=0; i<nd; i++) {
        x_i = X->a[i];
        x0  = xd[i];
        xp  = 1.0;
        Y->a[i][0] = yd[i];
        if (!unweighted) {
            C->a[i][i] = sqr(sy[i]);
            C_1->a[i][i] = 1/C->a[i][i];
            }
        for (j=0; j<nt; j++) {
            x_i[j] = xp;
            xp *= x0;
            }
        }

    /* Compute A, the matrix of coefficients.
     * Weighted least-squares solution is A = INV(Xt.INV(C).X).Xt.INV(C).y 
     * Unweighted solution is A = INV(Xt.X).Xt.y 
     */
    if (unweighted) {
        /* eliminating 2 matrix operations makes this much faster than a weighted fit
         * if there are many data points.
         */
        if (!m_trans(Xt, X))
            return(p_merror("transposing X"));
        if (!m_mult(XtCX, Xt, X)) 
            return(p_merror("multiplying Xt.X"));
        if (!m_invert(XtCX, XtCX)) 
            return(p_merror("inverting XtCX"));
        if (!m_mult(T, XtCX, Xt)) 
            return(p_merror("multiplying XtX.Xt"));
        if (!m_mult(A, T, Y))
            return(p_merror("multiplying T.Y"));

        /* Compute covariance matrix of A, Ca = (T.Tt)*C[0][0] */
        if (!m_trans(Tt, T))
            return(p_merror("computing transpose of T"));
        if (!m_mult(Ca, T, Tt))
            return(p_merror("multiplying T.Tt"));
        if (!m_scmul(Ca, Ca, sy?sqr(sy[0]):1))
            return(p_merror("multiplying T.Tt by scalar"));
        }
    else {
        if (!m_trans(Xt, X))
            return(p_merror("transposing X"));
        if (!m_mult(XtC, Xt, C_1)) 
            return(p_merror("multiplying Xt.C_1"));
        if (!m_mult(XtCX, XtC, X)) 
            return(p_merror("multiplying XtC.X"));
        if (!m_invert(XtCX, XtCX)) 
            return(p_merror("inverting XtCX"));
        if (!m_mult(T, XtCX, XtC)) 
            return(p_merror("multiplying XtCX.XtC"));
        if (!m_mult(A, T, Y))
            return(p_merror("multiplying T.Y"));

        /* Compute covariance matrix of A, Ca = T.C.Tt */
        if (!m_mult(TC, T, C))
            return(p_merror("multiplying T.C"));
        if (!m_trans(Tt, T))
            return(p_merror("computing transpose of T"));
        if (!m_mult(Ca, TC, Tt))
            return(p_merror("multiplying TC.Tt"));
        }

    for (i=0; i<nt; i++) {
        coef[i]   = A->a[i][0];
        if (s_coef)
          s_coef[i] = sqrt(Ca->a[i][i]);
        }

    /* Compute Yp = X.A, use to compute chi-squared */
    if (chi) {
      if (!m_mult(Yp, X, A))
        return(p_merror("multiplying X.A"));
      *chi = 0;
      for (i=0; i<nd; i++) {
        xp = (Yp->a[i][0] - yd[i]);
        if (diff!=NULL)
          diff[i] = xp;
        xp /= sy?sy[i]:1;
        *chi += xp*xp;
      }
      if (nd!=nt)
        *chi /= (nd-nt);
    }
    
    m_free(&X);
    m_free(&Y);
    m_free(&Yp);
    m_free(&Xt);
    if (!unweighted) {
        m_free(&C);
        m_free(&C_1);
        }
    m_free(&A);
    m_free(&Ca);
    m_free(&XtC);
    m_free(&XtCX);
    m_free(&T);
    m_free(&Tt);
    m_free(&TC);
    return(1);
    }
Пример #19
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
}
Пример #20
0
void Sy_m( VEC *x, VEC *Torq_ext, VEC *out)
{
    static MAT *iI = {MNULL};
	MAT *sk_om = {MNULL};
    VEC *q, *w, *q_dot, *w_dot, *v_res1, *v_res2; 
    int i;

    if ( x == VNULL || out == VNULL ){
        error(E_NULL,"f");
	}
    if ( x->dim != 13 || out->dim != 13){
        error(E_SIZES,"f");
	}

 
    q = v_get(4);
    for (i=0; i<4; i++)
    {
        q->ve[i] = x->ve[i];
    }
    
    w = v_get(3);
    for (i=4; i<7; i++)
    {
        w->ve[i-4] = x->ve[i];
    }
               
    v_res1 = v_get(3);
    v_zero(v_res1);
    
    v_res2 = v_get(3);
    v_zero(v_res2);
    
    q_dot = v_get(4);
    v_zero(q_dot);
    
    w_dot = v_get(3);
    v_zero(w_dot);
    
    if(iI == MNULL){
        iI = m_get(3,3);
        m_zero(iI);
    }
    sk_om = m_get(4,4);
    skew_mat(sk_om, w);
           
    mv_mlt(sk_om, q, q_dot);
    sv_mlt(0.5, q_dot, q);
    
        
    for (i=0; i<4; i++)
    {
        out->ve[i] = q->ve[i];
    }
       
    /*****wd********/
    m_resize(sk_om, 3, 3);
    mv_mlt(inertia, w, v_res1);
    mv_mlt(sk_om, v_res1, v_res2);
    v_sub(Torq_ext, v_res2, v_res1);
    m_inverse(inertia,iI);
    mv_mlt(iI, v_res1, w_dot);    
        
    for (i=4; i<7; i++)
    {
        out->ve[i] = w_dot->ve[i-4];
    }
   
    
    for (i=7; i<13; i++)
    {
        out->ve[i] = 0;
    }
    
    
    M_FREE(sk_om);
    //M_FREE(iI);
    V_FREE(q);
    V_FREE(w);
    V_FREE(q_dot);
    V_FREE(w_dot);
    V_FREE(v_res1);
    V_FREE(v_res2);
    
      
}
Пример #21
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);
     
}
Пример #22
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? */
}
Пример #23
0
/*
 * n_vars is the number of variables to be considered,
 * d is the data array of variables d[0],...,d[n_vars-1],
 * pred determines which estimate is required: BLUE, BLUP, or BLP
 */
void gls(DATA **d /* pointer to DATA array */,
         int n_vars, /* length of DATA array (to consider) */
         enum GLS_WHAT pred, /* what type of prediction is requested */
         DPOINT *where, /* prediction location */
         double *est /* output: array that holds the predicted values and variances */)
{
    GLM *glm = NULL; /* to be copied to/from d */
    static MAT *X0 = MNULL, *C0 = MNULL, *MSPE = MNULL, *CinvC0 = MNULL,
                *Tmp1 = MNULL, *Tmp2 = MNULL, *Tmp3 = MNULL, *R = MNULL;
    static VEC *blup = VNULL, *tmpa = VNULL, *tmpb = VNULL;
    PERM *piv = PNULL;
    volatile unsigned int i, rows_C;
    unsigned int j, k, l = 0, row, col, start_i, start_j, start_X, global,
                       one_nbh_empty;
    VARIOGRAM *v = NULL;
    static enum GLS_WHAT last_pred = GLS_INIT; /* the initial value */
    double c_value, *X_ori;
    int info;

    if (d == NULL) { /* clean up */
        if (X0 != MNULL) M_FREE(X0);
        if (C0 != MNULL) M_FREE(C0);
        if (MSPE != MNULL) M_FREE(MSPE);
        if (CinvC0 != MNULL) M_FREE(CinvC0);
        if (Tmp1 != MNULL) M_FREE(Tmp1);
        if (Tmp2 != MNULL) M_FREE(Tmp2);
        if (Tmp3 != MNULL) M_FREE(Tmp3);
        if (R != MNULL) M_FREE(R);
        if (blup != VNULL) V_FREE(blup);
        if (tmpa != VNULL) V_FREE(tmpa);
        if (tmpb != VNULL) V_FREE(tmpb);
        last_pred = GLS_INIT;
        return;
    }

    if (DEBUG_COV) {
        printlog("we're at %s X: %g Y: %g Z: %g\n",
                 IS_BLOCK(where) ? "block" : "point",
                 where->x, where->y, where->z);
    }

    if (pred != UPDATE) /* it right away: */
        last_pred = pred;

    assert(last_pred != GLS_INIT);

    if (d[0]->glm == NULL) { /* allocate and initialize: */
        glm = new_glm();
        d[0]->glm = (void *) glm;
    } else
        glm = (GLM *) d[0]->glm;

    glm->mu0 = v_resize(glm->mu0, n_vars);
    MSPE = m_resize(MSPE, n_vars, n_vars);
    if (pred == GLS_BLP || UPDATE_BLP) {
        X_ori = where->X;
        for (i = 0; i < n_vars; i++) { /* mu(0) */
            glm->mu0->ve[i] = calc_mu(d[i], where);
            blup = v_copy(glm->mu0, v_resize(blup, glm->mu0->dim));
            where->X += d[i]->n_X; /* shift to next x0 entry */
        }
        where->X = X_ori; /* ... and set back */
        for (i = 0; i < n_vars; i++) { /* Cij(0,0): */
            for (j = 0; j <= i; j++) {
                v = get_vgm(LTI(d[i]->id,d[j]->id));
                ME(MSPE, i, j) = ME(MSPE, j, i) = COVARIANCE0(v, where, where, d[j]->pp_norm2);
            }
        }
        fill_est(NULL, blup, MSPE, n_vars, est); /* in case of empty neighbourhood */
    }
    /* xxx */
    /*
    logprint_variogram(v, 1);
    */

    /*
     * selection dependent problem dimensions:
     */
    for (i = rows_C = 0, one_nbh_empty = 0; i < n_vars; i++) {
        rows_C += d[i]->n_sel;
        if (d[i]->n_sel == 0)
            one_nbh_empty = 1;
    }

    if (rows_C == 0 /* all selection lists empty */
            || one_nbh_empty == 1) { /* one selection list empty */
        if (pred == GLS_BLP || UPDATE_BLP)
            debug_result(blup, MSPE, pred);
        return;
    }

    for (i = 0, global = 1; i < n_vars && global; i++)
        global = (d[i]->sel == d[i]->list
                  && d[i]->n_list == d[i]->n_original
                  && d[i]->n_list == d[i]->n_sel);

    /*
     * global things: enter whenever (a) first time, (b) local selections or
     * (c) the size of the problem grew since the last call (e.g. simulation)
     */
    if (glm->C == NULL || !global || rows_C > glm->C->m) {
        /*
         * fill y:
         */
        glm->y = get_y(d, glm->y, n_vars);

        if (pred != UPDATE) {
            glm->C = m_resize(glm->C, rows_C, rows_C);
            if (gl_choleski == 0) /* use LDL' decomposition, allocate piv: */
                piv = px_resize(piv, rows_C);
            m_zero(glm->C);
            glm->X = get_X(d, glm->X, n_vars);
            M_DEBUG(glm->X, "X");
            glm->CinvX = m_resize(glm->CinvX, rows_C, glm->X->n);
            glm->XCinvX = m_resize(glm->XCinvX, glm->X->n, glm->X->n);
            glm->beta = v_resize(glm->beta, glm->X->n);
            for (i = start_X = start_i = 0; i < n_vars; i++) { /* row var */
                /* fill C, mu: */
                for (j = start_j = 0; j <= i; j++) { /* col var */
                    v = get_vgm(LTI(d[i]->id,d[j]->id));
                    for (k = 0; k < d[i]->n_sel; k++) { /* rows */
                        row = start_i + k;
                        for (l = 0, col = start_j; col <= row && l < d[j]->n_sel; l++, col++) {
                            if (pred == GLS_BLUP)
                                c_value = GCV(v, d[i]->sel[k], d[j]->sel[l]);
                            else
                                c_value = COVARIANCE(v, d[i]->sel[k], d[j]->sel[l]);
                            /* on the diagonal, if necessary, add measurement error variance */
                            if (d[i]->colnvariance && i == j && k == l)
                                c_value += d[i]->sel[k]->variance;
                            ME(glm->C, col, row) = c_value; /* fill upper */
                            if (col != row)
                                ME(glm->C, row, col) = c_value; /* fill all */
                        } /* for l */
                    } /* for k */
                    start_j += d[j]->n_sel;
                } /* for j */
                start_i += d[i]->n_sel;
                if (d[i]->n_sel > 0)
                    start_X += d[i]->n_X - d[i]->n_merge;
            } /* for i */

            /*
            if (d[0]->colnvmu)
            	glm->C = convert_vmuC(glm->C, d[0]);
            */
            if (d[0]->variance_fn) {
                glm->mu = get_mu(glm->mu, glm->y, d, n_vars);
                convert_C(glm->C, glm->mu, d[0]->variance_fn);
            }

            if (DEBUG_COV && pred == GLS_BLUP)
                printlog("[using generalized covariances: max_val - semivariance()]");
            M_DEBUG(glm->C, "Covariances (x_i, x_j) matrix C (upper triangle)");
            /*
             * factorize C:
             */
            CHfactor(glm->C, piv, &info);
            if (info != 0) { /* singular: */
                pr_warning("Covariance matrix singular at location [%g,%g,%g]: skipping...",
                           where->x, where->y, where->z);
                m_free(glm->C);
                glm->C = MNULL; /* assure re-entrance if global */
                P_FREE(piv);
                return;
            }
            if (piv == NULL)
                M_DEBUG(glm->C, "glm->C, Choleski decomposed:")
                else
                    M_DEBUG(glm->C, "glm->C, LDL' decomposed:")
                } /* if (pred != UPDATE) */
Пример #24
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;
}
Пример #25
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;
}
Пример #26
0
/*
 * n_vars is the number of variables to be considered,
 * d is the data array of variables d[0],...,d[n_vars-1],
 * pred determines which estimate is required: BLUE, BLUP, or BLP
 */
void gls(DATA **d /* pointer to DATA array */,
		int n_vars, /* length of DATA array (to consider) */
		enum GLS_WHAT pred, /* what type of prediction is requested */
		DPOINT *where, /* prediction location */
		double *est /* output: array that holds the predicted values and variances */)
{
	GLM *glm = NULL; /* to be copied to/from d */
	static MAT *X0 = MNULL, *C0 = MNULL, *MSPE = MNULL, *CinvC0 = MNULL,
		*Tmp1 = MNULL, *Tmp2 = MNULL, *Tmp3, *R = MNULL;
	static VEC *blup = VNULL, *tmpa = VNULL, *tmpb = VNULL;
	volatile unsigned int i, rows_C;
	unsigned int j, k, l = 0, row, col, start_i, start_j, start_X, global;
	VARIOGRAM *v = NULL;
	static enum GLS_WHAT last_pred = GLS_INIT; /* the initial value */
	double c_value, *X_ori;

	if (d == NULL) { /* clean up */
		if (X0 != MNULL) M_FREE(X0); 
		if (C0 != MNULL) M_FREE(C0);
		if (MSPE != MNULL) M_FREE(MSPE);
		if (CinvC0 != MNULL) M_FREE(CinvC0);
		if (Tmp1 != MNULL) M_FREE(Tmp1);
		if (Tmp2 != MNULL) M_FREE(Tmp2);
		if (Tmp3 != MNULL) M_FREE(Tmp3);
		if (R != MNULL) M_FREE(R);
		if (blup != VNULL) V_FREE(blup);
		if (tmpa != VNULL) V_FREE(tmpa);
		if (tmpb != VNULL) V_FREE(tmpb);
		last_pred = GLS_INIT;
		return;
	}
#ifndef HAVE_SPARSE
	if (gl_sparse) {
		pr_warning("sparse matrices not supported: compile with --with-sparse");
		gl_sparse = 0;
	}
#endif

	if (DEBUG_COV) {
		printlog("we're at %s X: %g Y: %g Z: %g\n",
			IS_BLOCK(where) ? "block" : "point",
			where->x, where->y, where->z);
	}

	if (pred != UPDATE) /* it right away: */
		last_pred = pred;

	assert(last_pred != GLS_INIT);

	if (d[0]->glm == NULL) { /* allocate and initialize: */
		glm = new_glm();
		d[0]->glm = (void *) glm;
	} else
		glm = (GLM *) d[0]->glm;

	glm->mu0 = v_resize(glm->mu0, n_vars);
	MSPE = m_resize(MSPE, n_vars, n_vars);
	if (pred == GLS_BLP || UPDATE_BLP) {
		X_ori = where->X;
		for (i = 0; i < n_vars; i++) { /* mu(0) */
			glm->mu0->ve[i] = calc_mu(d[i], where);
			blup = v_copy(glm->mu0, v_resize(blup, glm->mu0->dim));
			where->X += d[i]->n_X; /* shift to next x0 entry */
		}
		where->X = X_ori; /* ... and set back */
		for (i = 0; i < n_vars; i++) { /* Cij(0,0): */
			for (j = 0; j <= i; j++) {
				v = get_vgm(LTI(d[i]->id,d[j]->id));
				MSPE->me[i][j] = MSPE->me[j][i] = COVARIANCE0(v, where, where, d[j]->pp_norm2);
			}
		}
		fill_est(NULL, blup, MSPE, n_vars, est); /* in case of empty neighbourhood */
	}
	/* xxx */
	/*
	logprint_variogram(v, 1);
	*/

/* 
 * selection dependent problem dimensions: 
 */
	for (i = rows_C = 0; i < n_vars; i++)
		rows_C += d[i]->n_sel;

	if (rows_C == 0) { /* empty selection list(s) */
		if (pred == GLS_BLP || UPDATE_BLP)
			debug_result(blup, MSPE, pred);
		return;
	}

	for (i = 0, global = 1; i < n_vars && global; i++)
		global = (d[i]->sel == d[i]->list && d[i]->n_list == d[i]->n_original);

/*
 * global things: enter whenever (a) first time, (b) local selections or
 * (c) the size of the problem grew since the last call (e.g. simulation)
 */
	if ((glm->C == NULL && glm->spC == NULL) || !global || rows_C > glm->C->m) {
/* 
 * fill y: 
 */
		glm->y = get_y(d, glm->y, n_vars);

		if (pred != UPDATE) {
			if (! gl_sparse) {
				glm->C = m_resize(glm->C, rows_C, rows_C);
				m_zero(glm->C);
			} 
#ifdef HAVE_SPARSE
			else {
				if (glm->C == NULL) {
					glm->spC = sp_get(rows_C, rows_C, gl_sparse);
					/* d->spLLT = spLLT = sp_get(rows_C, rows_C, gl_sparse); */
				} else {
					glm->spC = sp_resize(glm->spC, rows_C, rows_C);
					/* d->spLLT = spLLT = sp_resize(spLLT, rows_C, rows_C); */
				}
				sp_zero(glm->spC);
			} 
#endif
			glm->X = get_X(d, glm->X, n_vars);
			M_DEBUG(glm->X, "X");
			glm->CinvX = m_resize(glm->CinvX, rows_C, glm->X->n);
			glm->XCinvX = m_resize(glm->XCinvX, glm->X->n, glm->X->n);
			glm->beta = v_resize(glm->beta, glm->X->n);
			for (i = start_X = start_i = 0; i < n_vars; i++) { /* row var */
				/* fill C, mu: */
				for (j = start_j = 0; j <= i; j++) { /* col var */
					v = get_vgm(LTI(d[i]->id,d[j]->id));
					for (k = 0; k < d[i]->n_sel; k++) { /* rows */
						row = start_i + k;
						for (l = 0, col = start_j; col <= row && l < d[j]->n_sel; l++, col++) {
							if (pred == GLS_BLUP)
								c_value = GCV(v, d[i]->sel[k], d[j]->sel[l]);
							else
								c_value = COVARIANCE(v, d[i]->sel[k], d[j]->sel[l]);
							/* on the diagonal, if necessary, add measurement error variance */
							if (d[i]->colnvariance && i == j && k == l)
								c_value += d[i]->sel[k]->variance;
							if (! gl_sparse)
								glm->C->me[row][col] = c_value;
#ifdef HAVE_SPARSE
							else {
								if (c_value != 0.0)
									sp_set_val(glm->spC, row, col, c_value);
							} 
#endif
						} /* for l */
					} /* for k */
					start_j += d[j]->n_sel;
				} /* for j */
				start_i += d[i]->n_sel;
				if (d[i]->n_sel > 0)
					start_X += d[i]->n_X - d[i]->n_merge;
			} /* for i */

			/*
			if (d[0]->colnvmu)
				glm->C = convert_vmuC(glm->C, d[0]);
			*/
			if (d[0]->variance_fn) {
				glm->mu = get_mu(glm->mu, glm->y, d, n_vars);
				convert_C(glm->C, glm->mu, d[0]->variance_fn);
			}

			if (DEBUG_COV && pred == GLS_BLUP)
				printlog("[using generalized covariances: max_val - semivariance()]");
			if (! gl_sparse) {
				M_DEBUG(glm->C, "Covariances (x_i, x_j) matrix C (lower triangle only)");
			}
#ifdef HAVE_SPARSE
			else {
				SM_DEBUG(glm->spC, "Covariances (x_i, x_j) sparse matrix C (lower triangle only)")
			}
#endif
/* check for singular C: */
			if (! gl_sparse && gl_cn_max > 0.0) {
				for (i = 0; i < rows_C; i++) /* row */ 
					for (j = i+1; j < rows_C; j++) /* col > row */
						glm->C->me[i][j] = glm->C->me[j][i]; /* fill symmetric */
				if (is_singular(glm->C, gl_cn_max)) {
					pr_warning("Covariance matrix (nearly) singular at location [%g,%g,%g]: skipping...",
						where->x, where->y, where->z);
					m_free(glm->C); glm->C = MNULL; /* assure re-entrance if global */
					return;
				}
			}
/* 
 * factorize C: 
 */
			if (! gl_sparse)
				LDLfactor(glm->C);
#ifdef HAVE_SPARSE
			else {
				sp_compact(glm->spC, 0.0);
				spCHfactor(glm->spC);
			}
#endif
		} /* if (pred != UPDATE) */
		if (pred != GLS_BLP && !UPDATE_BLP) { /* C-1 X and X'C-1 X, beta */
/* 
 * calculate CinvX: 
 */
    		tmpa = v_resize(tmpa, rows_C);
    		for (i = 0; i < glm->X->n; i++) {
				tmpa = get_col(glm->X, i, tmpa);
				if (! gl_sparse)
					tmpb = LDLsolve(glm->C, tmpa, tmpb);
#ifdef HAVE_SPARSE
				else
					tmpb = spCHsolve(glm->spC, tmpa, tmpb);
#endif
				set_col(glm->CinvX, i, tmpb);
			}
/* 
 * calculate X'C-1 X: 
 */
			glm->XCinvX = mtrm_mlt(glm->X, glm->CinvX, glm->XCinvX); /* X'C-1 X */
			M_DEBUG(glm->XCinvX, "X'C-1 X");
			if (gl_cn_max > 0.0 && is_singular(glm->XCinvX, gl_cn_max)) {
				pr_warning("X'C-1 X matrix (nearly) singular at location [%g,%g,%g]: skipping...",
					where->x, where->y, where->z);
				m_free(glm->C); glm->C = MNULL; /* assure re-entrance if global */
				return;
			}
			m_inverse(glm->XCinvX, glm->XCinvX);
/* 
 * calculate beta: 
 */
			tmpa = vm_mlt(glm->CinvX, glm->y, tmpa); /* X'C-1 y */
			glm->beta = vm_mlt(glm->XCinvX, tmpa, glm->beta); /* (X'C-1 X)-1 X'C-1 y */
			V_DEBUG(glm->beta, "beta");
			M_DEBUG(glm->XCinvX, "Cov(beta), (X'C-1 X)-1");
			M_DEBUG(R = get_corr_mat(glm->XCinvX, R), "Corr(beta)");
		} /* if pred != GLS_BLP */
	} /* if redo the heavy part */