예제 #1
0
/*! return its inverse matrix */
inline _zgematrix i(const zgbmatrix& mat)
{
#ifdef  CPPL_VERBOSE
  std::cerr << "# [MARK] i(const zgbmatrix&)"
            << std::endl;
#endif//CPPL_VERBOSE
  
#ifdef  CPPL_DEBUG
  if(mat.M!=mat.N){
    std::cerr << "[ERROR] i(zgbmatrix&) " << std::endl
              << "This matrix is not square and has no inverse matrix."
              << std::endl
              << "Your input was (" << mat.M << "x" << mat.N << ")."
              << std::endl;
    exit(1);
  }
#endif//CPPL_DEBUG
  
  zgbmatrix mat_cp(mat);
  zgematrix mat_inv(mat.M,mat.N);
  mat_inv.identity();
  mat_cp.zgbsv(mat_inv);
  
  return _(mat_inv);
}
예제 #2
0
파일: pos.c 프로젝트: Aerobota/PenguPilot
static void kalman_correct(kalman_t *kf, float pos, float speed)
{
   /* update H matrix: */
   kf->H.me[1][1] = 1.0f * (kf->use_speed && speed != 0.0f);
   kf->z.ve[0] = pos;
   kf->z.ve[1] = speed;

   /* K = P * HT * inv(H * P * HT + R) */
   mat_mul(&kf->T0, &kf->H, &kf->P);   // T0 = H * P
   mmtr_mul(&kf->T1, &kf->T0, &kf->H); // T1 = T0 * HT
   mat_add(&kf->T0, &kf->T1, &kf->R);  // T0 = T1 + R
   mat_inv(&kf->T1, &kf->T0);          // T1 = inv(T0)
   mmtr_mul(&kf->T0, &kf->P, &kf->H);  // T0 = P * HT
   mat_mul(&kf->K, &kf->T0, &kf->T1);  // K = T0 * T1

   /* x = x + K * (z - H * x) */
   mat_vec_mul(&kf->t0, &kf->H, &kf->x);  // t0 = H * x
   vec_sub(&kf->t1, &kf->z, &kf->t0);     // t1 = z - t0
   mat_vec_mul(&kf->t0, &kf->K, &kf->t1); // t0 = K * t1
   vec_add(&kf->x, &kf->x, &kf->t0);      // x = x + t0

   /* P = (I - K * H) * P */
   mat_mul(&kf->T0, &kf->K, &kf->H);  // T0 = K * H
   mat_sub(&kf->T1, &kf->I, &kf->T0); // T1 = I - T0
   mat_mul(&kf->T0, &kf->T1, &kf->P); // T0 = T1 * P
   mat_copy(&kf->P, &kf->T0);         // P = T0
}
예제 #3
0
/*! return its inverse matrix */
inline _zgematrix i(const zhematrix& mat)
{VERBOSE_REPORT;
  zhematrix mat_cp(mat);
  zgematrix mat_inv(mat.n,mat.n);
  mat_inv.identity();
  mat_cp.zhesv(mat_inv);
  
  return _(mat_inv);
}
예제 #4
0
파일: cluster.c 프로젝트: rforge/muste
static int init_tilat()
        {
        int jj;
        int i,j,gr;
     // double det;

/*      sur_print("\nInitializing work matrices...");  */
        hav=muste_fopen(tempfile,"r+b");
        for (i=0; i<ng; ++i)
            {
            N1[i]=0.0;
            for (j=0; j<m; ++j) S[i+ng*j]=0.0;
            }

        for (jj=0L; jj<n; ++jj)
            {
            hav_read1(jj,&gr); --gr;
            hav_read3(jj,xx);
            ++N1[gr];
            for (i=0; i<m; ++i) S[gr+ng*i]+=xx[i];
            }

        for (i=0; i<ng; ++i)
            {
            if (N1[i]==0.0)
                {
                if (maxiter>1) { muste_fclose(hav); return(-1); }
                sprintf(sbuf,"\nNo cases in initial setting for group %d",i+1);
                sur_print(sbuf); WAIT; return(-1);
                }
            }

        mat_mmt(H1,S,ng,m);
        for (i=0; i<ng; ++i)
            for (j=0; j<ng; ++j)
                {
                if (i==j) H1[i+ng*j]=N1[i]-H1[i+ng*j];
                else      H1[i+ng*j]=-H1[i+ng*j];
                }
        mat_inv(H2,H1,ng,&f2);
        for (i=0; i<ng; ++i) f2/=N1[i];


/* for (i=0; i<ng2*ng2; ++i) Q[i]=0.0;     tarpeeton */


        for (i=0; i<ng; ++i)
            for (j=0; j<ng; ++j)
                Q[i+ng2*j]=H2[i+ng*j];

/*  sprintf(sbuf,"\nlambda=%g",f2); sur_print(sbuf); */
/* Rprintf("\nH2:"); matprint(H2,ng,ng);   */

        muste_fclose(hav);
        return(1);
        }
예제 #5
0
double Gaussian_3d::pdf_3d(Sample_3d x){

    /* determinante mat covarianza */
    double det = det_3d(cov);

    //std::cout<<"Determinante: "<<det<<std::endl;

    /* vettore X - Mu */
    double diff[3];
    diff[0] = x[0] - mean[0];
    diff[1] = x[1] - mean[1];
    diff[2] = x[2] - mean[2];

    /* calcolo matrice inversa delle covarianze */
    boost::numeric::ublas::matrix<double> mat_inv(3,3);
    inv_3d(cov, mat_inv);

    //std::cout<<"inversa delle covarianze: "<<mat_inv<<std::endl;

    /* calcolo esponente della funzione gaussiana: (X - Mu) * Cov^(-1) * (X - Mu)' */
    double mahalanobis_dis =
        (diff[0] * mat_inv(0,0) + diff[1] * mat_inv(1,0) + diff[2] * mat_inv(2,0)) * diff[0] +
		(diff[0] * mat_inv(0,1) + diff[1] * mat_inv(1,1) + diff[2] * mat_inv(2,1)) * diff[1] +
		(diff[0] * mat_inv(0,2) + diff[1] * mat_inv(1,2) + diff[2] * mat_inv(2,2)) * diff[2];

    //std::cout<<"esponente della gaussiana: "<<mahalanobis_dis<<std::endl;

    /* calcolo coefficiente gaussiana */
    double pre = pow((2 * M_PI), 1.5) * pow(det, 0.5);
    /* calcolo esponenziale */
    double exp = pow(M_E, -0.5 * mahalanobis_dis);

    //std::cout<<"esponenziale gaussiana: "<<exp<<std::endl;
    //std::cout<<"coefficiente gaussiana: "<<pre<<std::endl;

    return pre/exp;
}
예제 #6
0
/*! return its inverse matrix */
inline _zgematrix i(const _zhematrix& mat)
{
#ifdef  CPPL_VERBOSE
  std::cerr << "# [MARK] i(const _zhematrix&)"
            << std::endl;
#endif//CPPL_VERBOSE
  
  zhematrix mat_cp;
  mat_cp.shallow_copy(mat);
  
  zgematrix mat_inv(mat.N,mat.N);
  mat_inv.identity();
  
  mat_cp.zhesv(mat_inv);
  
  return _(mat_inv);
}
예제 #7
0
/*! return its inverse matrix */
inline _zgematrix i(const _zgematrix& mat)
{VERBOSE_REPORT;
#ifdef  CPPL_DEBUG
  if(mat.m!=mat.n){
    ERROR_REPORT;
    std::cerr << "This matrix is not square and has no inverse matrix." << std::endl
              << "Your input was (" << mat.m << "x" << mat.n << ")." << std::endl;
    exit(1);
  }
#endif//CPPL_DEBUG
  zgematrix mat_cp(mat);
  zgematrix mat_inv(mat_cp.m,mat_cp.n);
  mat_inv.identity();
  mat_cp.zgesv(mat_inv);
  
  return _(mat_inv);
}
예제 #8
0
파일: NaMatrix.cpp 프로젝트: evlad/nnacs
//---------------------------------------------------------------------------
// Compute inverse matrix
NaMatrix&   NaMatrix::inv (NaMatrix& mInv) const
{
#ifdef __matrix_h
    if(dim_rows() != dim_cols()){
        throw(na_size_mismatch);
    }

    MATRIX  mat = mat_creat(dim_rows(), dim_cols(), UNDEFINED);

    unsigned    i, j, n = dim_rows();
    for(i = 0; i < n; ++i){
        for(j = 0; j < n; ++j){
            mat[i][j] = get(i, j);
        }
    }

    MATRIX  minv = mat_inv(mat);
    if(NULL == minv){
        throw(na_bad_value);
    }

    mat_free(mat);
    mInv.new_dim(n, n);
    for(i = 0; i < n; ++i){
        for(j = 0; j < n; ++j){
            mInv.fetch(i, j) = minv[i][j];
        }
    }

    mat_free(minv);
#if 0
    trans(mInv);

    NaReal  vDet = det();

    if(0 == vDet)
        throw(na_bad_value);
    else
        mInv.multiply(1/vDet);
#endif

    return mInv;
#else
    throw(na_not_implemented);
#endif /* __matrix_h */
}
예제 #9
0
파일: t2test.c 프로젝트: rforge/muste
static double T2_BF()
        {
        int i,j;
        double t2,det;

        if (orig_samples)
            {
            laske_summat(ss[0],ss2[0]);
            for (i=0; i<m; ++i)
                {
                ss[1][i]=s[i]-ss[0][i];
                for (j=0; j<=i; ++j) ss2[1][i+m*j]=s2[i+m*j]-ss2[0][i+m*j];
                }

            for (i=0; i<m; ++i)
                for (j=0; j<=i; ++j)
                    {
                    ss_apu[i+m*j]=
                     (ss2[0][i+m*j]-ss[0][i]*ss[0][j]/n[1])/(n[1]*(n[1]-1))
                    +(ss2[1][i+m*j]-ss[1][i]*ss[1][j]/n[2])/(n[2]*(n[2]-1));
                    ss_apu[j+m*i]=ss_apu[i+m*j];
                    }

            mat_inv(ss_inv,ss_apu,m,&det);
// mprint(ss_inv,m,m);
            }
        else
            laske_summat(ss[0],NULL);

        for (i=0; i<m; ++i)
            {
            ss[0][i]/=n[1];
            ss[1][i]/=n[2];
            }
        for (i=0; i<m; ++i) ss_apu[i]=ss[0][i]-ss[1][i];

        t2=0.0;
        for (i=0; i<m; ++i)
            {
            t2+=ss_apu[i]*ss_apu[i]*ss_inv[i*(m+1)];
            for (j=0; j<i; ++j)
                t2+=2*ss_apu[i]*ss_apu[j]*ss_inv[i+m*j];
            }
        return (t2);
        }   /* T2_BF */
예제 #10
0
/*----------------------------------------------------------------------------*/
static MATRIX buildRMatrix(MATRIX UB, MATRIX planeNormal,
				     tasQEPosition qe, int *errorCode){
  MATRIX U1V, U2V, TV, TVINV, M;
  
  
  *errorCode = 1;
  U1V = tasReflectionToQC(qe,UB);
  if(U1V == NULL){
    *errorCode = UBNOMEMORY;
    return NULL;
  }
  normalizeVector(U1V);

  U2V = vectorCrossProduct(planeNormal,U1V);
  if(U2V == NULL){
    killVector(U1V);
    *errorCode = UBNOMEMORY;
    return NULL;
  }
  if(vectorLength(U2V) < .0001){
    *errorCode = BADUBORQ;
    killVector(U1V);
    killVector(U2V);
    return NULL;
  }

  TV = buildTVMatrix(U1V,U2V);
  if(TV == NULL){
    killVector(U1V);
    killVector(U2V);
    *errorCode = UBNOMEMORY;
    return NULL;
  }

  TVINV = mat_inv(TV);
  if(TVINV == NULL){
    *errorCode = BADUBORQ;
  }

  killVector(U1V);
  killVector(U2V);
  mat_free(TV);
  return TVINV;
}
예제 #11
0
/*------------------------------------------------------------------------*/
int calcTasQH(MATRIX UB, tasAngles angles, ptasQEPosition qe){
  MATRIX UBINV = NULL, QV = NULL, Q = NULL;
  double q;
  tasReflection r;
  int i;

  UBINV = mat_inv(UB);
  r.angles = angles;
  r.qe = *qe;
  QV = calcTasUVectorFromAngles(r);
  if(UBINV == NULL || QV == NULL){
    return UBNOMEMORY;
  }
  /*
    normalize the QV vector to be the length of the Q vector
    Thereby take into account the physicists magic fudge
    2PI factor
  */
  q = sqrt(qe->ki*qe->ki + qe->kf*qe->kf - 
	   2.*qe->ki*qe->kf*Cosd(angles.sample_two_theta));
  qe->qm = q;
  q /= 2. * PI;

  for(i = 0; i < 3; i++){
    QV[i][0] *= q;
  }

  Q = mat_mul(UBINV,QV);
  if(Q == NULL){
    mat_free(UBINV);
    killVector(QV);
    return UBNOMEMORY;
  }
  qe->qh = Q[0][0];
  qe->qk = Q[1][0];
  qe->ql = Q[2][0];

  killVector(QV);
  killVector(Q);
  mat_free(UBINV);

  return 1;
}
예제 #12
0
//--------------------------------------------------------------------------
// void 
// DiracOpClover::CloverMatChkb(ChkbType chkb, int inv) const
//--------------------------------------------------------------------------
// Purpose: 
//   Calculates the clover matrix for all sites of the specified checkerboard
//   and stores them in the memory pointed by lat.Aux0Ptr() for
//   chkb CHKB_EVEN and by lat.Aux1Ptr() for chkb CHKB_ODD.
// Arguments:
//   site[0..3]          coordinate[x,y,z,t]
// Convension:
//   Links and fermionic fields are in Wilson order, that is
//   checkerboarded U[t][y][z][x][x,y,z,t].
//--------------------------------------------------------------------------
void 
DiracOpClover::CloverMatChkb(ChkbType chkb, int inv) const
{
  char *fname = "CloverMatChkb()";
  VRB.Func(cname,fname);

  // get the pointer to the clover matrices.
  //------------------------------------------------------------------------
  IFloat *cl_mat_p = (IFloat *)(chkb == CHKB_EVEN ? 
			      lat.Aux0Ptr() : lat.Aux1Ptr());
  
  // loop in the order consistent with the Wilson checkerboarded storage
  // Note: the sites[0,1,2,3] is in order of [x,y,z,t] 
  //------------------------------------------------------------------------
  {
    IFloat *Ap = cl_mat_p;  
    const int parity = (chkb == CHKB_ODD ? 1 : 0);
    const int *nsites = clover_lib_arg->nsites;    
    int site[4];                                   
    for (site[3] = 0; site[3] < nsites[3]; ++(site[3])) {
      for (site[2] = 0; site[2] < nsites[2]; ++(site[2])) {
	for (site[1] = 0; site[1] < nsites[1]; ++(site[1])) {
	  site[0] = (site[3] + site[2] + site[1] + parity)%2;	
	  for (; site[0] < nsites[0]; site[0] += 2) {
	    SiteCloverMat(site, Ap);	  
	    Ap += CLOVER_MAT_SIZE;     
	  }
	}
      }
    }
  }


  // invert the clover matrix
  //------------------------------------------------------------------------
  if (inv) {
    IFloat *Ap = cl_mat_p; 
    for (int i = 0; i < GJP.VolNodeSites(); ++i) {
      mat_inv(Ap, Ap, 6, MAT_INV_ALG_LDL_CMPR, 0);    
      Ap += HALF_CLOVER_MAT_SIZE;         
    } 
  }
}
예제 #13
0
파일: md.c 프로젝트: SahanGH/psi4public
static void remove_system_drift(struct md *md)
{
	vec_t cp = get_system_com(md);
	vec_t cv = get_system_com_velocity(md);
	vec_t am = get_system_angular_momentum(md);

	mat_t inertia = get_system_inertia_tensor(md);
	mat_t inertia_inv = mat_zero;

	if (inertia.xx < EPSILON ||
	    inertia.yy < EPSILON ||
	    inertia.zz < EPSILON) {
		inertia_inv.xx = inertia.xx < EPSILON ? 0.0 : 1.0 / inertia.xx;
		inertia_inv.yy = inertia.yy < EPSILON ? 0.0 : 1.0 / inertia.yy;
		inertia_inv.zz = inertia.zz < EPSILON ? 0.0 : 1.0 / inertia.zz;
	}
	else {
		inertia_inv = mat_inv(&inertia);
	}

	vec_t av = mat_vec(&inertia_inv, &am);

	for (size_t i = 0; i < md->n_bodies; i++) {
		struct body *body = md->bodies + i;
		vec_t pos = wrap(md, &body->pos);

		vec_t cross = {
			av.y * (pos.z - cp.z) - av.z * (pos.y - cp.y),
			av.z * (pos.x - cp.x) - av.x * (pos.z - cp.z),
			av.x * (pos.y - cp.y) - av.y * (pos.x - cp.x)
		};

		body->vel.x -= cv.x + cross.x;
		body->vel.y -= cv.y + cross.y;
		body->vel.z -= cv.z + cross.z;
	}

	vec_t cv2 = get_system_com_velocity(md);
	vec_t am2 = get_system_angular_momentum(md);

	assert(vec_len(&cv2) < EPSILON && vec_len(&am2) < EPSILON);
}
예제 #14
0
/*! return its inverse matrix */
inline _dsymatrix i(const _dsymatrix& mat)
{VERBOSE_REPORT;
  dsymatrix mat_cp(mat);
  dsymatrix mat_inv(mat_cp.n);
  mat_inv.identity();
  
  char UPLO('l');
  long NRHS(mat.n), LDA(mat.n), *IPIV(new long[mat.n]), LDB(mat.n), LWORK(-1), INFO(1);
  double *WORK( new double[1] );
  dsysv_(UPLO, mat_cp.n, NRHS, mat_cp.array, LDA, IPIV, mat_inv.array, LDB, WORK, LWORK, INFO);
  LWORK = long(WORK[0]);
  delete [] WORK;  WORK = new double[LWORK];
  dsysv_(UPLO, mat_cp.n, NRHS, mat_cp.array, LDA, IPIV, mat_inv.array, LDB, WORK, LWORK, INFO);
  delete [] WORK; delete [] IPIV;
  if(INFO!=0){
    WARNING_REPORT;
    std::cerr << "Serious trouble happend. INFO = " << INFO << "." << std::endl;
  }
  
  return _(mat_inv);
}
예제 #15
0
파일: t2test.c 프로젝트: rforge/muste
static double T2()
        {
        int i,j;
        double t2,det;

        if (orig_samples)
            {
            laske_summat(ss[0],ss2[0]);

            for (i=0; i<m; ++i)
                {
                ss[1][i]=s[i]-ss[0][i];
                for (j=0; j<=i; ++j) ss2[1][i+m*j]=s2[i+m*j]-ss2[0][i+m*j];
                }
            }

        else
            laske_summat(ss[0],NULL);

        for (i=0; i<m; ++i)
            {
            ss[0][i]/=n[1];
            ss[1][i]/=n[2];
            }
// Rprintf("ka:\n");
// mprint(ss[0],1,m);
// mprint(ss[1],1,m);

        if (orig_samples)
          {
          for (i=0; i<m; ++i)
              for (j=0; j<=i; ++j)
                  {
                  ss2[0][i+m*j]+=-n[1]*ss[0][i]*ss[0][j]
                                 +ss2[1][i+m*j]-n[2]*ss[1][i]*ss[1][j];
                  ss2[0][j+m*i]=ss2[0][i+m*j];
                  }

          for (i=0; i<m; ++i)
              for (j=0; j<=i; ++j)
                  {
                  ss2[0][i+m*j]/=n[0]-2;
                  ss2[0][j+m*i]=ss2[0][i+m*j];
                  }
// mprint(ss2[0],m,m);
          mat_inv(ss2[1],ss2[0],m,&det);
// mat_inv() ->  mat_gj() joka varaa 3 vektoritilaa, mutta vapauttaa ne



// mprint(ss2[1],m,m);
          }

        for (i=0; i<m; ++i) ss[0][i]-=ss[1][i];

        t2=0.0;
        for (i=0; i<m; ++i)
            {
            t2+=ss[0][i]*ss[0][i]*ss2[1][i*(m+1)];
            for (j=0; j<i; ++j)
                t2+=2*ss[0][i]*ss[0][j]*ss2[1][i+m*j];
            }
        t2*=(double)n[1]*(double)n[2]/(double)n[0];
        return (t2);
        }
예제 #16
0
int
main(void)
{
    int i, result;
    flint_rand_t state;

    printf("inv... ");
    fflush(stdout);

    _randinit(state);

    /* Check aliasing */

    /* Managed element type (mpq_t) */
    for (i = 0; i < 50; i++)
    {
        long n;
        ctx_t ctx;
        mat_t A, B, C;
        long ansB, ansC;

        n = n_randint(state, 20) + 1;

        ctx_init_mpq(ctx);
        mat_init(A, n, n, ctx);
        mat_init(B, n, n, ctx);
        mat_init(C, n, n, ctx);

        mat_randtest(A, state, ctx);

        mat_set(B, A, ctx);
        ansC = mat_inv(C, B, ctx);
        ansB = mat_inv(B, B, ctx);

        result = ((ansB == ansC) && (!ansB || mat_equal(B, C, ctx)));
        if (!result)
        {
            printf("FAIL:\n\n");
            printf("A: \n"), mat_print(A, ctx), printf("\n");
            printf("B: \n"), mat_print(B, ctx), printf("\n");
            printf("C: \n"), mat_print(C, ctx), printf("\n");
            printf("ansB = %ld\n", ansB);
            printf("ansC = %ld\n", ansC);
        }

        mat_clear(A, ctx);
        mat_clear(B, ctx);
        mat_clear(C, ctx);
        ctx_clear(ctx);
    }

    /* Check A * A^{-1} == A^{-1} * A == Id */

    /* Managed element type (mpq_t) */
    for (i = 0; i < 50; i++)
    {
        long n;
        ctx_t ctx;
        mat_t A, B, C, D, I;
        long ansB;

        n = n_randint(state, 20) + 1;

        ctx_init_mpq(ctx);
        mat_init(A, n, n, ctx);
        mat_init(B, n, n, ctx);
        mat_init(C, n, n, ctx);
        mat_init(D, n, n, ctx);
        mat_init(I, n, n, ctx);

        mat_randtest(A, state, ctx);

        ansB = mat_inv(B, A, ctx);
        if (!ansB)
        {
            mat_mul(C, A, B, ctx);
            mat_mul(D, B, A, ctx);
        }

        result = (ansB || (mat_equal(C, D, ctx) && mat_is_one(C, ctx)));
        if (!result)
        {
            printf("FAIL:\n\n");
            printf("A: \n"), mat_print(A, ctx), printf("\n");
            printf("B: \n"), mat_print(B, ctx), printf("\n");
            printf("C: \n"), mat_print(C, ctx), printf("\n");
            printf("D: \n"), mat_print(D, ctx), printf("\n");
            printf("ansB = %ld\n", ansB);
        }

        mat_clear(A, ctx);
        mat_clear(B, ctx);
        mat_clear(C, ctx);
        mat_clear(D, ctx);
        ctx_clear(ctx);
    }


    _randclear(state);
    _fmpz_cleanup();
    printf("PASS\n");
    return EXIT_SUCCESS;
}
예제 #17
0
// Main get_nav filter function
void get_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
	double tnow, imu_dt;
	double dq[4], quat_new[4];

	// compute time-elapsed 'dt'
	// This compute the navigation state at the DAQ's Time Stamp
	tnow = sensorData_ptr->imuData_ptr->time;
	imu_dt = tnow - tprev;
	tprev = tnow;		
	
	// ==================  Time Update  ===================
	// Temporary storage in Matrix form
	quat[0] = navData_ptr->quat[0];
	quat[1] = navData_ptr->quat[1];
	quat[2] = navData_ptr->quat[2];
	quat[3] = navData_ptr->quat[3];
	
	a_temp31[0][0] = navData_ptr->vn; a_temp31[1][0] = navData_ptr->ve;
	a_temp31[2][0] = navData_ptr->vd;
	
	b_temp31[0][0] = navData_ptr->lat; b_temp31[1][0] = navData_ptr->lon;
	b_temp31[2][0] = navData_ptr->alt;
	
	// AHRS Transformations
	C_N2B = quat2dcm(quat, C_N2B);
	C_B2N = mat_tran(C_N2B, C_B2N);
	
	// Attitude Update
	// ... Calculate Navigation Rate
	nr = navrate(a_temp31,b_temp31,nr);
	
	dq[0] = 1;
	dq[1] = 0.5*om_ib[0][0]*imu_dt;
	dq[2] = 0.5*om_ib[1][0]*imu_dt;
	dq[3] = 0.5*om_ib[2][0]*imu_dt;
	
	qmult(quat,dq,quat_new);
	
	quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	
    if(quat[0] < 0) {
        // Avoid quaternion flips sign
        quat[0] = -quat[0];
        quat[1] = -quat[1];
        quat[2] = -quat[2];
        quat[3] = -quat[3];
    }
    
	navData_ptr->quat[0] = quat[0];
	navData_ptr->quat[1] = quat[1];
	navData_ptr->quat[2] = quat[2];
	navData_ptr->quat[3] = quat[3];
	
	quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
	
	// Velocity Update
	dx = mat_mul(C_B2N,f_b,dx);
	dx = mat_add(dx,grav,dx);
	navData_ptr->vn += imu_dt*dx[0][0];
	navData_ptr->ve += imu_dt*dx[1][0];
	navData_ptr->vd += imu_dt*dx[2][0];
	
	// Position Update
	dx = llarate(a_temp31,b_temp31,dx);
	navData_ptr->lat += imu_dt*dx[0][0];
	navData_ptr->lon += imu_dt*dx[1][0];
	navData_ptr->alt += imu_dt*dx[2][0];
	
	// JACOBIAN
	F = mat_fill(F, ZERO_MATRIX);
	// ... pos2gs
	F[0][3] = 1.0; 	F[1][4] = 1.0; 	F[2][5] = 1.0;
	// ... gs2pos
	F[5][2] = -2*g/EARTH_RADIUS;
	
	// ... gs2att
	temp33 = sk(f_b,temp33);
	atemp33 = mat_mul(C_B2N,temp33,atemp33);
	
	F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2];
	F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2];
	F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2];
	
	// ... gs2acc
	F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2];
	F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2];
	F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2];
	
	// ... att2att
	temp33 = sk(om_ib,temp33);
	F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2];
	F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2];
	F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2];
	
	// ... att2gyr
	F[6][12] = -0.5;
	F[7][13] = -0.5;
	F[8][14] = -0.5;
	
	// ... Accel Markov Bias
	F[9][9] = -1.0/TAU_A; 	F[10][10] = -1.0/TAU_A;	F[11][11] = -1.0/TAU_A;
	F[12][12] = -1.0/TAU_G; F[13][13] = -1.0/TAU_G;	F[14][14] = -1.0/TAU_G;
	
	//fprintf(stderr,"Jacobian Created\n");
	
	// State Transition Matrix: PHI = I15 + F*dt;
	temp1515 = mat_scalMul(F,imu_dt,temp1515);
	PHI = mat_add(I15,temp1515,PHI);
	
	// Process Noise
	G = mat_fill(G, ZERO_MATRIX);
	G[3][0] = -C_B2N[0][0];	G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2];
	G[4][0] = -C_B2N[1][0];	G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2];
	G[5][0] = -C_B2N[2][0];	G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2];
	
	G[6][3] = -0.5;
	G[7][4] = -0.5;
	G[8][5] = -0.5;
	
	G[9][6] = 1.0; 			G[10][7] = 1.0; 		G[11][8] = 1.0;
	G[12][9] = 1.0; 		G[13][10] = 1.0; 		G[14][11] = 1.0;
	//fprintf(stderr,"Process Noise Matrix G is created\n");
	// Discrete Process Noise
	temp1512 = mat_mul(G,Rw,temp1512);
	temp1515 = mat_transmul(temp1512,G,temp1515);	// Qw = G*Rw*G'
	Qw = mat_scalMul(temp1515,imu_dt,Qw);			// Qw = dt*G*Rw*G'
	Q = mat_mul(PHI,Qw,Q);						// Q = (I+F*dt)*Qw
	
	temp1515 = mat_tran(Q,temp1515);
	Q = mat_add(Q,temp1515,Q);
	Q = mat_scalMul(Q,0.5,Q);				// Q = 0.5*(Q+Q')
	//fprintf(stderr,"Discrete Process Noise is created\n");
	
	// Covariance Time Update
	temp1515 = mat_mul(PHI,P,temp1515);
	P = mat_transmul(temp1515,PHI,P); 		// P = PHI*P*PHI'
	P = mat_add(P,Q,P);						// P = PHI*P*PHI' + Q
	temp1515 = mat_tran(P, temp1515);
	P = mat_add(P,temp1515,P);
	P = mat_scalMul(P,0.5,P);				// P = 0.5*(P+P')
	//fprintf(stderr,"Covariance Updated through Time Update\n");
	
	navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
	navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
	navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
	navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
	navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
	
	navData_ptr->err_type = TU_only;
	//fprintf(stderr,"Time Update Done\n");
	// ==================  DONE TU  ===================
	
	if(sensorData_ptr->gpsData_ptr->newData){
	
		// ==================  GPS Update  ===================
		sensorData_ptr->gpsData_ptr->newData = 0; // Reset the flag
		
		// Position, converted to NED
		a_temp31[0][0] = navData_ptr->lat;
		a_temp31[1][0] = navData_ptr->lon; a_temp31[2][0] = navData_ptr->alt;
		pos_ins_ecef = lla2ecef(a_temp31,pos_ins_ecef);
		
		a_temp31[2][0] = 0.0;
		//pos_ref = lla2ecef(a_temp31,pos_ref);
		pos_ref = mat_copy(a_temp31,pos_ref);
		pos_ins_ned = ecef2ned(pos_ins_ecef,pos_ins_ned,pos_ref);
		
		pos_gps[0][0] = sensorData_ptr->gpsData_ptr->lat*D2R;
		pos_gps[1][0] = sensorData_ptr->gpsData_ptr->lon*D2R;
		pos_gps[2][0] = sensorData_ptr->gpsData_ptr->alt;
		
		pos_gps_ecef = lla2ecef(pos_gps,pos_gps_ecef);
		
		pos_gps_ned = ecef2ned(pos_gps_ecef,pos_gps_ned,pos_ref);
		
		// Create Measurement: y
		y[0][0] = pos_gps_ned[0][0] - pos_ins_ned[0][0];
		y[1][0] = pos_gps_ned[1][0] - pos_ins_ned[1][0];
		y[2][0] = pos_gps_ned[2][0] - pos_ins_ned[2][0];
		
		y[3][0] = sensorData_ptr->gpsData_ptr->vn - navData_ptr->vn;
		y[4][0] = sensorData_ptr->gpsData_ptr->ve - navData_ptr->ve;
		y[5][0] = sensorData_ptr->gpsData_ptr->vd - navData_ptr->vd;
		
		//fprintf(stderr,"Measurement Matrix, y, created\n");
		
		// Kalman Gain
		temp615 = mat_mul(H,P,temp615);
		temp66 = mat_transmul(temp615,H,temp66);
		atemp66 = mat_add(temp66,R,atemp66);
		temp66 = mat_inv(atemp66,temp66); // temp66 = inv(H*P*H'+R)
		//fprintf(stderr,"inv(H*P*H'+R) Computed\n");
		
		temp156 = mat_transmul(P,H,temp156); // P*H'
		//fprintf(stderr,"P*H' Computed\n");
		K = mat_mul(temp156,temp66,K);	   // K = P*H'*inv(H*P*H'+R)
		//fprintf(stderr,"Kalman Gain Computed\n");
		
		// Covariance Update
		temp1515 = mat_mul(K,H,temp1515);
		ImKH = mat_sub(I15,temp1515,ImKH);	// ImKH = I - K*H
		
		temp615 = mat_transmul(R,K,temp615);
		KRKt = mat_mul(K,temp615,KRKt);		// KRKt = K*R*K'
		
		temp1515 = mat_transmul(P,ImKH,temp1515);
		P = mat_mul(ImKH,temp1515,P);		// ImKH*P*ImKH'
		temp1515 = mat_add(P,KRKt,temp1515);
		P = mat_copy(temp1515,P);			// P = ImKH*P*ImKH' + KRKt
		//fprintf(stderr,"Covariance Updated through GPS Update\n");
		
		navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
		navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
		navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
		navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
		navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
		
		// State Update
		x = mat_mul(K,y,x);
		denom = (1.0 - (ECC2 * sin(navData_ptr->lat) * sin(navData_ptr->lat)));
		denom = sqrt(denom*denom);

		Re = EARTH_RADIUS / sqrt(denom);
		Rn = EARTH_RADIUS*(1-ECC2) / denom*sqrt(denom);
		navData_ptr->alt = navData_ptr->alt - x[2][0];
		navData_ptr->lat = navData_ptr->lat + x[0][0]/(Re + navData_ptr->alt);
		navData_ptr->lon = navData_ptr->lon + x[1][0]/(Rn + navData_ptr->alt)/cos(navData_ptr->lat);
		
		navData_ptr->vn = navData_ptr->vn + x[3][0];
		navData_ptr->ve = navData_ptr->ve + x[4][0];
		navData_ptr->vd = navData_ptr->vd + x[5][0];
		
		quat[0] = navData_ptr->quat[0];
		quat[1] = navData_ptr->quat[1];
		quat[2] = navData_ptr->quat[2];
		quat[3] = navData_ptr->quat[3];
		
		// Attitude correction
		dq[0] = 1.0;
		dq[1] = x[6][0];
		dq[2] = x[7][0];
		dq[3] = x[8][0];
		
		qmult(quat,dq,quat_new);
		
		quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		
		navData_ptr->quat[0] = quat[0];
		navData_ptr->quat[1] = quat[1];
		navData_ptr->quat[2] = quat[2];
		navData_ptr->quat[3] = quat[3];
		
		quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
		
		navData_ptr->ab[0] = navData_ptr->ab[0] + x[9][0];
		navData_ptr->ab[1] = navData_ptr->ab[1] + x[10][0];
		navData_ptr->ab[2] = navData_ptr->ab[2] + x[11][0];
		
		navData_ptr->gb[0] = navData_ptr->gb[0] + x[12][0];
		navData_ptr->gb[1] = navData_ptr->gb[1] + x[13][0];
		navData_ptr->gb[2] = navData_ptr->gb[2] + x[14][0];
		
		navData_ptr->err_type = gps_aided;
		//fprintf(stderr,"Measurement Update Done\n");
	}
	
	// Remove current estimated biases from rate gyro and accels
	sensorData_ptr->imuData_ptr->p -= navData_ptr->gb[0];
	sensorData_ptr->imuData_ptr->q -= navData_ptr->gb[1];
	sensorData_ptr->imuData_ptr->r -= navData_ptr->gb[2];
	sensorData_ptr->imuData_ptr->ax -= navData_ptr->ab[0];
	sensorData_ptr->imuData_ptr->ay -= navData_ptr->ab[1];
	sensorData_ptr->imuData_ptr->az -= navData_ptr->ab[2];

	// Get the new Specific forces and Rotation Rate,
	// use in the next time update
	f_b[0][0] = sensorData_ptr->imuData_ptr->ax;
	f_b[1][0] = sensorData_ptr->imuData_ptr->ay;
	f_b[2][0] = sensorData_ptr->imuData_ptr->az;

	om_ib[0][0] = sensorData_ptr->imuData_ptr->p;
	om_ib[1][0] = sensorData_ptr->imuData_ptr->q;
	om_ib[2][0] = sensorData_ptr->imuData_ptr->r;


}