MATSTACK mat_eig_sym(MATRIX symmat, MATSTACK result) { int m, n; MATRIX im, tmp_result0 = NULL, tmp_result1 = NULL; INT_VECTOR indcs = NULL; MATSTACK tmp = NULL; m = MatCol(symmat); n = MatRow(symmat); if(m!=n) mat_error(MAT_SIZEMISMATCH); if(result==NULL) { if ((result = matstack_creat(2)) == NULL) return matstack_error(MATSTACK_MALLOC); result[0] = NULL; result[1] = NULL; } im = mat_creat(m, 1, UNDEFINED); tmp_result0 = mat_creat(m, 1, UNDEFINED); tmp_result1 = mat_copy(symmat, tmp_result1); mat_tred2(tmp_result1, tmp_result0, im); mat_tqli(tmp_result0, im, tmp_result1); tmp = mat_qsort(tmp_result0, ROWS, tmp); result[0] = mat_copy(tmp[0], result[0]); indcs = mat_2int_vec(tmp[1]); result[1] = mat_get_sub_matrix_from_cols(tmp_result1, indcs, result[1]); int_vec_free(indcs); mat_free(im); mat_free(tmp_result0); mat_free(tmp_result1); return result; }
// 掛け算 int mat_mul(matrix *mat1, matrix mat2, matrix mat3){ int i, j, k; matrix mat2_w, mat3_w; if(check_mul(*mat1, mat2, mat3) != 0){ printf("%s\n", "エラーが発生しました。"); return -1; } mat_alloc(&mat2_w, mat2.row, mat2.col); mat_alloc(&mat3_w, mat3.row, mat3.col); mat_copy(&mat2_w, mat2); mat_copy(&mat3_w, mat3); for(i=0; i < mat1->row; i++){ for(j=0; j < mat1->col; j++){ mat1->element[i][j] = 0; for(k=0; k<mat2_w.col; k++){ mat1->element[i][j] += mat2_w.element[i][k] * mat3_w.element[k][j]; } } } mat_free(&mat2_w); mat_free(&mat3_w); return 0; }
/* *----------------------------------------------------------------------------- * funct: mat_lsolve * desct: solve linear equations * given: a = square matrix A * b = column matrix B * retrn: column matrix X (of AX = B) *----------------------------------------------------------------------------- */ MATRIX mat_lsolve(MATRIX a,MATRIX b,MATRIX X) { MATRIX A, B, P; int n; n = MatCol(a); if ( ( A = mat_creat(n, n, UNDEFINED) ) == NULL ) return (NULL); if ( ( B = mat_creat(n, 1, UNDEFINED) ) == NULL ) return (NULL); mat_copy(a,A); mat_copy(b,B); if ( ( P = mat_creat(n, 1, UNDEFINED) ) == NULL ) return (NULL); // if dimensions of C is wrong if ( MatRow(X) != n || MatCol(X) != 1 ) { printf("mat_lsolve error: incompatible output matrix size\n"); _exit(-1); // if dimensions of C is correct } else { mat_lu( A, P ); mat_backsubs1( A, B, X, P, 0 ); } mat_free(A); mat_free(B); mat_free(P); return(X); }
MATRIX mat_concat(MATRIX A, MATRIX B, int dim) { int i, j, m, n, o, p; MATRIX result; if(A==NULL) { return mat_copy(B, NULL); } else { m = MatCol(A); n = MatRow(A); } if(B==NULL) { return mat_copy(A, NULL); } else { o = MatCol(B); p = MatRow(B); } if((dim==ROWS)&&((m==o) ||!((m==0)&&(o==0)))) { if((result = mat_creat(n+p, m, UNDEFINED))==NULL) return NULL; #pragma omp parallel for private(j) for(i=0; i<n; ++i) { for(j=0; j<m; ++j) { result[i][j] = A[i][j]; } } #pragma omp parallel for private(j) for(i=0; i<p; ++i) { for(j=0; j<m; ++j) { result[i+n][j] = B[i][j]; } } return result; } if((dim==COLS)&&((n==p) ||!((n==0)&&(p==0)))) { if((result = mat_creat(n, m+o, UNDEFINED))==NULL) return NULL; #pragma omp parallel for private(j) for(i=0; i<n; ++i) { for(j=0; j<m; ++j) result[i][j] = A[i][j]; for(j=0; j<o; ++j) result[i][j+m] = B[i][j]; } return result; } return mat_error(MAT_SIZEMISMATCH); }
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 }
static void _load_bones (struct ov_skeleton *s, unsigned char *data, struct _iqm_header *h) { struct _iqm_joint *j = malloc(h->num_joints * sizeof(struct _iqm_joint)); memcpy(j, data + h->ofs_joints, h->num_joints * sizeof(struct _iqm_joint)); s->num_bones = h->num_joints; s->bones = malloc(h->num_joints * sizeof(struct iqm_bone)); int i; for (i=0; i<h->num_joints; i++) { float q[16]; char *name = (char *)(data + h->ofs_text + j[i].name); s->bones[i].parent = j[i].parent; s->bones[i].name = malloc(strlen(name)*sizeof(char)+1); strcpy(s->bones[i].name, name); read_pose(&s->bones[i].bind_pose, &j[i]); mat_from_pose(q, s->bones[i].bind_pose.translate, s->bones[i].bind_pose.rotate, s->bones[i].bind_pose.scale); if (s->bones[i].parent >= 0) { struct ov_bone *parent = &s->bones[s->bones[i].parent]; mat_mul44(s->bones[i].bind_matrix, parent->bind_matrix, q); } else { mat_copy(s->bones[i].bind_matrix, q); } mat_invert(s->bones[i].inv_bind_matrix, s->bones[i].bind_matrix); } free (j); }
int32 CGeometricObject::m_Clone(CGeometricObject *source) { if (!source) return(0); strcpy(p_FatherName, source->p_FatherName); p_Father=source->p_Father; p_HasChildrens=source->p_HasChildrens; p_BaseMaterial=source->p_BaseMaterial; for (int32 i=0; i<MAX_LODS; i++) p_Lods[i]=source->p_Lods[i]; p_NumLods=source->p_NumLods; p_VertexBuffer=source->p_VertexBuffer; p_StaticVertex=source->p_StaticVertex; p_PosTrack=source->p_PosTrack; p_RotTrack=source->p_RotTrack; p_ScaleTrack=source->p_ScaleTrack; vect_copy(&source->p_Pivot, &p_Pivot); vect_copy(&source->p_CurrentPosition, &p_CurrentPosition); quat_copy(&source->p_CurrentRotationQuaternion, &p_CurrentRotationQuaternion); mat_copy(&source->p_CurrentRotationMatrix, &p_CurrentRotationMatrix); vect_copy(&source->p_CurrentScale, &p_CurrentScale); vect_copy(&source->p_TotalScale, &p_TotalScale); mat_copy(&source->p_WorldMatrix, &p_WorldMatrix); if (source->p_SPHEREBoundVolume) { p_SPHEREBoundVolume=new CBoundVolume; source->p_SPHEREBoundVolume->m_Copy(CLONE, p_SPHEREBoundVolume); } if (source->p_AABBBoundVolume) { p_AABBBoundVolume=new CBoundVolume; source->p_AABBBoundVolume->m_Copy(CLONE, p_AABBBoundVolume); } if (source->p_OBBBoundVolume) { p_OBBBoundVolume=new CBoundVolume; source->p_OBBBoundVolume->m_Copy(CLONE, p_OBBBoundVolume); } return(1); }
void blocked_plaq(int Nsmear, int block) { register int i; register site *s; int j, stride = 1; double plaq = 0.0, plaqSq = 0.0, re = 0.0, reSq = 0.0, im = 0.0, imSq = 0.0; double sum = 0.0, norm = 1.0 / volume, tr; complex det = cmplx(0.0, 0.0), tc; matrix tmat, tmat2, tmat3; // Set number of links to stride, 2^block for (j = 0; j < block; j++) stride *= 2; // Compute the strided plaquette, exploiting a symmetry under TUP<-->XUP // Copy links to tempmat and tempmat2 to be shifted FORALLSITES(i, s) { mat_copy(&(s->link[TUP]), &(tempmat[i])); mat_copy(&(s->link[XUP]), &(tempmat2[i])); }
static void take_inverse( m_elem **in, m_elem **out, int n ) { #ifdef PRINT_DEBUG printf( "ekf: calculating inverse\n" ); #endif /* Nothing fancy for now, just a Gauss-Jordan technique, with good pivoting (thanks to NR). */ gaussj( in, n, out, 0 ); /* out is SCRATCH */ mat_copy( in, out, n, n ); }
void extended_kalman_init( m_elem **GQGt, m_elem **R, m_elem **P, m_elem *x, int num_state, int num_measurement ) { #ifdef PRINT_DEBUG printf( "ekf: Initializing filter\n" ); #endif alloc_globals( num_state, num_measurement ); sys_transfer = matrix( 1, num_state, 1, num_state ); mea_transfer = matrix( 1, num_measurement, 1, num_state ); /* Init the global variables using the arguments. */ vec_copy( x, state_post, state_size ); vec_copy( x, state_pre, state_size ); mat_copy( P, cov_post, state_size, state_size ); mat_copy( P, cov_pre, state_size, state_size ); sys_noise_cov = GQGt; mea_noise_cov = R; }
/* *----------------------------------------------------------------------------- * funct: mat_inv * desct: find inverse of a matrix * given: a = square matrix a * retrn: square matrix Inverse(A) * NULL = fails, singular matrix, or malloc() fails * 1 = success *----------------------------------------------------------------------------- */ MATRIX mat_inv(MATRIX a, MATRIX C) { MATRIX A, B, P; int i, n; n = MatCol(a); if ( ( A = mat_creat( n, n, UNDEFINED ) ) == NULL ) return (NULL); mat_copy(a,A); if ( ( B = mat_creat( n, 1, UNDEFINED ) ) == NULL ) return (NULL); if ( ( P = mat_creat( n, 1, UNDEFINED ) ) == NULL ) return (NULL); // if dimensions of C is wrong if ( MatRow(a) != MatRow(C) || MatCol(a) != MatCol(C) ) { printf("mat_inv error: incompatible output matrix size\n"); _exit(-1); // if dimensions of C is correct } else { /* * - LU-decomposition - * also check for singular matrix */ if (mat_lu(A, P) == -1) { mat_free(A); mat_free(B); mat_free(C); mat_free(P); printf("mat_inv error: failed to invert\n"); return (NULL); } for (i=0; i<n; i++) { mat_fill(B, ZERO_MATRIX); B[i][0] = 1.0; mat_backsubs1( A, B, C, P, i ); } } mat_free(A); mat_free(B); mat_free(P); if (C==NULL) { printf("mat_inv error: failed to invert\n"); return(NULL); } else { return (C); } }
/* *----------------------------------------------------------------------------- * funct: mat_det * desct: find determinant * given: A = matrix * retrn: the determinant of A * comen: *----------------------------------------------------------------------------- */ double mat_det( MATRIX a ) { MATRIX A, P; int j; int i, n; double result; n = MatRow(a); A = mat_creat(MatRow(a), MatCol(a), UNDEFINED); A = mat_copy(a, A); P = mat_creat(n, 1, UNDEFINED); /* * take a LUP-decomposition */ i = mat_lu(A, P); switch (i) { /* * case for singular matrix */ case -1: result = 0.0; break; /* * normal case: |A| = |L||U||P| * |L| = 1, * |U| = multiplication of the diagonal * |P| = +-1 */ default: result = 1.0; for (j=0; j<MatRow(A); j++) { result *= A[(int)P[j][0]][j]; } result *= signa[i%2]; break; } mat_free(A); mat_free(P); return (result); }
void AD_WindModifier::build_objectmatrix (float4 framepos) { AD_Vect3D postmp, stmp; AD_Quaternion objrot; AD_Matrix posttrans, scaling, maux; accum_scale.x=accum_scale.y=accum_scale.z=1.0f; mat_identity(¤tmatrix_rot); // estrazione dei dati col keyframer: niente di piu' facile col c++ !!! if (rotationtrack.numkey>0) { rotationtrack.get_data(framepos, &objrot); quat_rotquat_to_matrix(&objrot, ¤tmatrix_rot); } mat_copy(¤tmatrix_rot, ¤tmatrix); if (scaletrack.numkey>0) { scaletrack.get_data(framepos, &stmp); mat_setmatrix_of_scaling(&scaling, stmp.x, stmp.y, stmp.z); accum_scale.x*=stmp.x; accum_scale.y*=stmp.y; accum_scale.z*=stmp.z; } else mat_identity(&scaling); if (positiontrack.numkey>0) positiontrack.get_data(framepos, ¤tpos); mat_setmatrix_of_pretraslation(&posttrans, ¤tpos); mat_mul(&scaling, ¤tmatrix_rot, &maux); mat_mul(&posttrans, &maux, ¤tmatrix); if (father!=(AD_Object3D *)NULL) { mat_mulaffine(&father->currentmatrix_rot, ¤tmatrix_rot, ¤tmatrix_rot); mat_mul(&father->currentmatrix, ¤tmatrix, ¤tmatrix); mat_mulvect(&father->currentmatrix, ¤tpos, &postmp); vect_copy(&postmp, ¤tpos); accum_scale.x*=father->accum_scale.x; accum_scale.y*=father->accum_scale.y; accum_scale.z*=father->accum_scale.z; } mat_transpose(¤tmatrix_rot, &inverse_rotmatrix); mat_get_row(¤tmatrix, 1, &forza); vect_auto_normalize(&forza); vect_scale(&forza, strenght*0.00016f*forceScaleFactor, &forza); }
void AD_PatchObject::build_objectmatrix (float4 framepos) // costruisce la matrice di trasformazione, che servira' poi per trasformare // i vertici dell'oggetto; { AD_Vect3D postmp, stmp; AD_Quaternion objrot; AD_Matrix posttrans, scaling, maux; accum_scale.x=accum_scale.y=accum_scale.z=1.0f; mat_identity(¤tmatrix_rot); // estrazione dei dati col keyframer: niente di piu' facile col c++ !!! if (rotationtrack.numkey>0) { rotationtrack.get_data(framepos, &objrot); quat_rotquat_to_matrix(&objrot, ¤tmatrix_rot); } mat_copy(¤tmatrix_rot, ¤tmatrix); if (scaletrack.numkey>0) { scaletrack.get_data(framepos, &stmp); mat_setmatrix_of_scaling(&scaling, stmp.x, stmp.y, stmp.z); accum_scale.x=accum_scale.x*stmp.x; accum_scale.y=accum_scale.x*stmp.y; accum_scale.z=accum_scale.x*stmp.z; } else mat_identity(&scaling); if (positiontrack.numkey>0) positiontrack.get_data(framepos, ¤tpos); mat_setmatrix_of_pretraslation(&posttrans, ¤tpos); mat_mul(&scaling, ¤tmatrix_rot, &maux); mat_mul(&posttrans, &maux, ¤tmatrix); if (father!=(AD_Object3D *)NULL) { mat_mulaffine(&father->currentmatrix_rot, ¤tmatrix_rot, ¤tmatrix_rot); mat_mul(&father->currentmatrix, ¤tmatrix, ¤tmatrix); mat_mulvect(&father->currentmatrix, ¤tpos, &postmp); vect_copy(&postmp, ¤tpos); accum_scale.x*=father->accum_scale.x; accum_scale.y*=father->accum_scale.y; accum_scale.z*=father->accum_scale.z; } mat_transpose(¤tmatrix_rot, &inverse_rotmatrix); }
void block_N_fatten(int N) { register int i, dir; register site* s; int j; block_and_fatten(); if (N > 1) { // Save unsmeared links in gauge_field_save for (dir = 0; dir < 4; dir++) { FORALLSITES(i, s) mat_copy(gauge_field_thin[dir] + i, gauge_field_save[dir] + i); } // block_and_fatten starts by copying s->link into gauge_field_thin for (j = 1; j < N; j++) block_and_fatten(); // Reset gauge_field_thin to hold unsmeared links for (dir = 0; dir < 4; dir++) { FORALLSITES(i, s) mat_copy(gauge_field_save[dir] + i, gauge_field_thin[dir] + i); } } }
void EcefToEnu(MATRIX outputVector, MATRIX inputVector, MATRIX position) { int i; double lat, lon; MATRIX C, ned, ref_position; MATRIX position_copy, delta_pos; C = mat_creat(3,3,ZERO_MATRIX); ref_position = mat_creat(3,1,ZERO_MATRIX); delta_pos = mat_creat(3,1,ZERO_MATRIX); position_copy = mat_creat(MatRow(position),MatCol(position),ZERO_MATRIX); mat_copy(position, position_copy); lat = position[0][0]; lon = position[1][0]; LatLonAltToEcef(ref_position,position_copy); mat_sub(inputVector,ref_position,delta_pos); C[0][0] = -sin(lon); C[0][1] = cos(lon); C[0][2] = 0; C[1][0] = -sin(lat)*cos(lon); C[1][1] = -sin(lat)*sin(lon); C[1][2] = cos(lat); C[2][0] = cos(lat)*cos(lon); C[2][1] = cos(lat)*sin(lon); C[2][2] = sin(lat); ned = mat_creat(MatRow(C),MatCol(delta_pos),ZERO_MATRIX); mat_mul(C,delta_pos,ned); for (i = 0; i < 3; i++) { outputVector[i][0] = ned[i][0]; } mat_free(ned); mat_free(C); mat_free(delta_pos); mat_free(ref_position); mat_free(position_copy); }
void CGeometricObject::m_BuildWorldMatrix(void) { AD_Vect3D postmp; AD_Matrix posttrans, scaling, maux; AD_Matrix iRot, iScale, iTrans; // matrice di rotazione e sua inversa quat_rotquat_to_matrix(&p_CurrentRotationQuaternion, &p_CurrentRotationMatrix); mat_copy(&p_CurrentRotationMatrix, &p_WorldMatrix); mat_transpose(&p_CurrentRotationMatrix, &iRot); // matrice di scaling e sua inversa mat_setmatrix_of_scaling(&scaling, p_CurrentScale.x, p_CurrentScale.y, p_CurrentScale.z); mat_setmatrix_of_scaling(&iScale, 1.0f/p_CurrentScale.x, 1.0f/p_CurrentScale.y, 1.0f/p_CurrentScale.z); // matrice di traslazione e sua inversa mat_setmatrix_of_pretraslation(&posttrans, &p_CurrentPosition); vect_neg(&p_CurrentPosition, &postmp); mat_setmatrix_of_pretraslation(&iTrans, &postmp); // prima pivot, poi scaling, poi rotazione, e infine traslazione //vect_neg(&p_Pivot, &postmp); //mat_setmatrix_of_pretraslation(&pivot, &postmp); //mat_mul(&scaling, &pivot, &maux); //mat_mul(&p_CurrentRotationMatrix, &maux, &maux); mat_mul(&p_CurrentRotationMatrix, &scaling, &maux); mat_mul(&posttrans, &maux, &p_WorldMatrix); // per la inversa: prima traslazione, rotazione, scaling e pivot //mat_setmatrix_of_pretraslation(&iPivot, &p_Pivot); mat_mul(&iRot, &iTrans, &maux); mat_mul(&iScale, &maux, &p_InverseWorldMatrix); //mat_mul(&iScale, &maux, &maux); //mat_mul(&iPivot, &maux, &p_InverseWorldMatrix); if (p_Father) { mat_mulaffine(&p_Father->p_CurrentRotationMatrix, &p_CurrentRotationMatrix, &p_CurrentRotationMatrix); mat_mul(&p_Father->p_WorldMatrix, &p_WorldMatrix, &p_WorldMatrix); mat_mul(&p_InverseWorldMatrix, &p_Father->p_InverseWorldMatrix, &p_InverseWorldMatrix); p_TotalScale.x*=p_Father->p_TotalScale.x; p_TotalScale.y*=p_Father->p_TotalScale.y; p_TotalScale.z*=p_Father->p_TotalScale.z; } }
void hvy_pot_polar() { register int i; register site *s; int j, t_dist, x_dist, y_dist, z_dist, y_start, z_start; double wloop; matrix tmat, tmat2, *mat; msg_tag *mtag = NULL; node0_printf("hvy_pot_polar: MAX_T = %d, MAX_X = %d\n", MAX_T, MAX_X); FORALLSITES(i, s) { // Polar projection of gauge-fixed links // To be multiplied together after projecting // !!! Overwrites links polar(&(s->link[TUP]), &tmat, &tmat2); mat_copy(&tmat, &(s->link[TUP])); }
// ----------------------------------------------------------------- // Calculate U = exp(A).U // Goes to eighth order in the exponential: // exp(A) * U = ( 1 + A + A^2/2 + A^3/3 ...) * U // = U + A*(U + (A/2)*(U + (A/3)*( ... ))) void exp_mult(int dir, double eps, anti_hermitmat *A) { register int i; register site *s; matrix *link, temp1, temp2, htemp; register Real t2, t3, t4, t5, t6, t7, t8; // Take divisions out of site loop (can't be done by compiler) t2 = eps / 2.0; t3 = eps / 3.0; t4 = eps / 4.0; t5 = eps / 5.0; t6 = eps / 6.0; t7 = eps / 7.0; t8 = eps / 8.0; FORALLSITES(i, s) { uncompress_anti_hermitian(&(A[i]), &htemp); link = &(s->link[dir]); mult_nn(&htemp, link, &temp1); scalar_mult_add_matrix(link, &temp1, t8, &temp2); mult_nn(&htemp, &temp2, &temp1); scalar_mult_add_matrix(link, &temp1, t7, &temp2); mult_nn(&htemp, &temp2, &temp1); scalar_mult_add_matrix(link, &temp1, t6, &temp2); mult_nn(&htemp, &temp2, &temp1); scalar_mult_add_matrix(link, &temp1, t5, &temp2); mult_nn(&htemp, &temp2, &temp1); scalar_mult_add_matrix(link, &temp1, t4, &temp2); mult_nn(&htemp, &temp2, &temp1); scalar_mult_add_matrix(link, &temp1, t3, &temp2); mult_nn(&htemp, &temp2, &temp1); scalar_mult_add_matrix(link, &temp1, t2, &temp2); mult_nn(&htemp, &temp2, &temp1); scalar_mult_add_matrix(link, &temp1, eps, &temp2); mat_copy(&temp2, link); // This step updates the link U[dir] }
void kalman_init( m_elem **GQGt, m_elem **Phi, m_elem **H, m_elem **R, m_elem **P, m_elem *x, int num_state, int num_measurement ) { alloc_globals( num_state, num_measurement ); /* Init the global variables using the arguments. */ vec_copy( x, state_post, state_size ); mat_copy( P, cov_post, state_size, state_size ); sys_noise_cov = GQGt; mea_noise_cov = R; sys_transfer = Phi; mea_transfer = H; /***************** Gain Loop *****************/ estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre ); update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain ); }
// 転置行列を返す int mat_trans(matrix *mat1, matrix mat2){ int i, j; matrix mat2_w; if(check_trans(*mat1, mat2) != 0){ printf("%s\n", "エラーが発生しました。"); return -1; } mat_alloc(&mat2_w, mat2.row, mat2.col); mat_copy(&mat2_w, mat2); for(i=0; i < mat1->row; i++){ for(j=0; j < mat1->col; j++){ mat1->element[i][j] = mat2_w.element[j][i]; } } mat_free(&mat2_w); return 0; }
//逆行列(はき出し法)a_oriの逆行列 struct matrix *mat_inv(struct matrix *a_ori){ int i,j,k,m,n; double d,dd; struct matrix *a; struct matrix *a_inv; if(a_ori==NULL){ printf("error at mat_inv:input is NULL\n");return NULL;} if(a_ori->row!=a_ori->col){printf("error at mat_inv:a_ori\n");return NULL;} a_inv=mat_alloc(a_ori->row,a_ori->col); a=mat_copy(a_ori); for(i=0;i<a->row;i++){ for(j=0;j<a->row;j++){ if(i==j)*(a_inv->element+j+i*a->col)=1; else *(a_inv->element+j+i*a->col)=0; } } for(i=0;i<a->row;i++){ d=*(a->element+i+i*a->col); for(j=0;j<a->row;j++){ *(a->element+j+i*a->col)=(1/d)* *(a->element+j+i*a->col); *(a_inv->element+j+i*a->col)=(1/d)* *(a_inv->element+j+i*a->col); } for(k=0;k<a->row;k++){ if(i!=k){ dd=*(a->element+i+k*a->col); for(n=0;n<a->row;n++){ *(a->element+n+k*a->col)= *(a->element+n+k*a->col)- dd * *(a->element+n+i*a->col); *(a_inv->element+n+k*a->col)= *(a_inv->element+n+k*a->col)-dd * *(a_inv->element+n+i*a->col); } } } } return(a_inv); }
void cvx_clustering ( double ** dist_mat, int fw_max_iter, int max_iter, int D, int N, double* lambda, double ** W) { // parameters double alpha = 0.1; double rho = 1; // iterative optimization double error = INF; double ** wone = mat_init (N, N); double ** wtwo = mat_init (N, N); double ** yone = mat_init (N, N); double ** ytwo = mat_init (N, N); double ** z = mat_init (N, N); double ** diffzero = mat_init (N, N); double ** diffone = mat_init (N, N); double ** difftwo = mat_init (N, N); mat_zeros (yone, N, N); mat_zeros (ytwo, N, N); mat_zeros (z, N, N); mat_zeros (diffzero, N, N); mat_zeros (diffone, N, N); mat_zeros (difftwo, N, N); int iter = 0; // Ian: usually we count up (instead of count down) while ( iter < max_iter ) { // stopping criteria #ifdef SPARSE_CLUSTERING_DUMP cout << "it is place 0 iteration #" << iter << ", going to get into frank_wolfe" << endl; #endif // mat_set (wone, z, N, N); // mat_set (wtwo, z, N, N); // mat_zeros (wone, N, N); // mat_zeros (wtwo, N, N); // STEP ONE: resolve w_1 and w_2 frank_wolf (dist_mat, yone, z, wone, rho, N, fw_max_iter); // for w_1 #ifdef SPARSE_CLUSTERING_DUMP cout << "norm2(w_1) = " << mat_norm2 (wone, N, N) << endl; #endif blockwise_closed_form (ytwo, z, wtwo, rho, lambda, N); // for w_2 #ifdef SPARSE_CLUSTERING_DUMP cout << "norm2(w_2) = " << mat_norm2 (wtwo, N, N) << endl; #endif // STEP TWO: update z by averaging w_1 and w_2 mat_add (wone, wtwo, z, N, N); mat_dot (0.5, z, z, N, N); #ifdef SPARSE_CLUSTERING_DUMP cout << "norm2(z) = " << mat_norm2 (z, N, N) << endl; #endif // STEP THREE: update the y_1 and y_2 by w_1, w_2 and z mat_sub (wone, z, diffone, N, N); double trace_wone_minus_z = mat_norm2 (diffone, N, N); mat_dot (alpha, diffone, diffone, N, N); mat_add (yone, diffone, yone, N, N); mat_sub (wtwo, z, difftwo, N, N); //double trace_wtwo_minus_z = mat_norm2 (difftwo, N, N); mat_dot (alpha, difftwo, difftwo, N, N); mat_add (ytwo, difftwo, ytwo, N, N); // STEP FOUR: trace the objective function if (iter % 100 == 0) { error = overall_objective (dist_mat, lambda, N, z); // get current number of employed centroid #ifdef NCENTROID_DUMP int nCentroids = get_nCentroids (z, N, N); #endif cout << "[Overall] iter = " << iter << ", Overall Error: " << error #ifdef NCENTROID_DUMP << ", nCentroids: " << nCentroids #endif << endl; } iter ++; } // STEP FIVE: memory recollection mat_free (wone, N, N); mat_free (wtwo, N, N); mat_free (yone, N, N); mat_free (ytwo, N, N); mat_free (diffone, N, N); mat_free (difftwo, N, N); // STEP SIX: put converged solution to destination W mat_copy (z, W, N, N); }
int main() { int i, j; int w, h; int levels, ct_levels, wt_levels,level_init; double rate,rate_init; ivec dfb_levels; mat source, dest; contourlet_t *contourlet; mat wavelet; int length; unsigned char *buffer; //³õʼ»¯²ÎÊý int argc=6; rate_init=2; level_init=5; #define LEVELS 5 #define IMPULSE 100. source = mat_pgm_read("1.pgm"); h = mat_height(source); w = mat_width(source); dest = mat_new(w, h); rate = rate_init * w * h; levels = level_init; ct_levels = argc - 4; /* contourlet levels */ wt_levels = levels - ct_levels; /* wavelet levels */ dfb_levels = ivec_new(ct_levels); for(i = 0; i < ct_levels; i++) dfb_levels[i] = 4+i; buffer = bvec_new_zeros(BUFFER_SIZE); contourlet = contourlet_new(ct_levels, dfb_levels); contourlet->wt_levels = wt_levels; contourlet_transform(contourlet, source); wavelet = it_dwt2D(contourlet->low, it_wavelet_lifting_97, wt_levels); contourlet->dwt = it_wavelet2D_split(wavelet, wt_levels); /* normalize the subbands */ for(i = 0; i < ct_levels; i++) for(j = 0; j < (1 << dfb_levels[i]); j++) mat_mul_by(contourlet->high[i][j], norm_high[1+i][dfb_levels[i]][j]); mat_mul_by(contourlet->low, norm_low[ct_levels]); /* make flat images */ mat_pgm_write("dwt.pgm", wavelet); for(i = 0; i < ct_levels; i++) { char filename[256]; mat dfb_rec = mat_new((h >> i) + 1, (w >> i) + 1); if(dfb_levels[i]) dfb_flatten(contourlet->high[i], dfb_rec, dfb_levels[i]); else mat_set_submatrix(dfb_rec, contourlet->high[i][0], 0, 0); mat_incr(dfb_rec, 128); sprintf(filename, "dfb%d.pgm", i); mat_pgm_write(filename, dfb_rec); mat_decr(dfb_rec, 128); mat_delete(dfb_rec); } /* EZBC encoding */ length = ezbc_encode(contourlet, buffer, BUFFER_SIZE, rate); /* EZBC decoding */ ezbc_decode(contourlet, buffer, BUFFER_SIZE, rate); mat_pgm_write("rec_low.pgm", contourlet->dwt[0]); /* make flat images */ for(i = 0; i < ct_levels; i++) { char filename[256]; mat dfb_rec = mat_new((h >> i) + 1, (w >> i) + 1); if(dfb_levels[i]) dfb_flatten(contourlet->high[i], dfb_rec, dfb_levels[i]); else mat_set_submatrix(dfb_rec, contourlet->high[i][0], 0, 0); mat_incr(dfb_rec, 128); sprintf(filename, "rec_dfb%d.pgm", i); mat_pgm_write(filename, dfb_rec); mat_decr(dfb_rec, 128); mat_delete(dfb_rec); } /* normalize the subbands */ for(i = 0; i < ct_levels; i++) for(j = 0; j < (1 << dfb_levels[i]); j++) mat_div_by(contourlet->high[i][j], norm_high[1+i][dfb_levels[i]][j]); mat_div_by(contourlet->low, norm_low[ct_levels]); // mat_pgm_write("rec_low.pgm", contourlet->dwt[0]); /* TODO: fix this in libit */ if(wt_levels) wavelet = it_wavelet2D_merge(contourlet->dwt, wt_levels); else mat_copy(wavelet, contourlet->dwt[0]); mat_pgm_write("rec_dwt.pgm", wavelet); contourlet->low = it_idwt2D(wavelet, it_wavelet_lifting_97, wt_levels); contourlet_itransform(contourlet, dest); contourlet_delete(contourlet); mat_pgm_write("rec.pgm", dest); printf("rate = %f PSNR = %f\n", length * 8. / (w*h), 10*log10(255*255/mat_distance_mse(source, dest, 0))); ivec_delete(dfb_levels); mat_delete(dest); mat_delete(source); bvec_delete(buffer); return(0); }
void random_gauge_trans(Twist_Fermion *TF) { int a, b, i, j, x = 1, t = 1, s = node_index(x, t); complex tc; matrix Gmat, tmat, etamat, psimat[NUMLINK], chimat; if (this_node != 0) { printf("random_gauge_trans: only implemented in serial so far\n"); fflush(stdout); terminate(1); } if (nx < 4 || nt < 4) { printf("random_gauge_trans: doesn't deal with boundaries, "); printf("needs to be run on larger volume\n"); fflush(stdout); terminate(1); } // Set up random gaussian matrix, then unitarize it clear_mat(&tmat); for (j = 0; j < DIMF; j++) { #ifdef SITERAND tc.real = gaussian_rand_no(&(lattice[0].site_prn)); tc.imag = gaussian_rand_no(&(lattice[0].site_prn)); #else tc.real = gaussian_rand_no(&(lattice[0].node_prn)); tc.imag = gaussian_rand_no(&(lattice[0].node_prn)); #endif c_scalar_mult_sum_mat(&(Lambda[j]), &tc, &tmat); } polar(&tmat, &Gmat); // Confirm unitarity or check invariance when Gmat = I // mult_na(&Gmat, &Gmat, &tmat); // dumpmat(&tmat); // mat_copy(&tmat, &Gmat); // Left side of local eta clear_mat(&etamat); // Construct G eta = sum_j eta^j G Lambda^j for (j = 0; j < DIMF; j++) { mult_nn(&Gmat, &(Lambda[j]), &tmat); tc = TF[s].Fsite.c[j]; c_scalar_mult_sum_mat(&tmat, &tc, &etamat); } // Project out eta^j = -Tr[Lambda^j G eta] for (j = 0; j < DIMF; j++) { mult_nn(&(Lambda[j]), &etamat, &tmat); tc = trace(&tmat); CNEGATE(tc, TF[s].Fsite.c[j]); } // Right side of local eta clear_mat(&etamat); // Construct eta Gdag = sum_j eta^j Lambda^j Gdag for (j = 0; j < DIMF; j++) { mult_na(&(Lambda[j]), &Gmat, &tmat); tc = TF[s].Fsite.c[j]; c_scalar_mult_sum_mat(&tmat, &tc, &etamat); } // Project out eta^j = -Tr[eta Gdag Lambda^j] for (j = 0; j < DIMF; j++) { mult_nn(&etamat, &(Lambda[j]), &tmat); tc = trace(&tmat); CNEGATE(tc, TF[s].Fsite.c[j]); } // Left side of local links and psis; right side of local chis FORALLDIR(a) { mult_nn(&Gmat, &(lattice[s].link[a]), &tmat); mat_copy(&tmat, &(lattice[s].link[a])); clear_mat(&(psimat[a])); for (j = 0; j < DIMF; j++) { mult_nn(&Gmat, &(Lambda[j]), &tmat); tc = TF[s].Flink[a].c[j]; c_scalar_mult_sum_mat(&tmat, &tc, &(psimat[a])); } for (j = 0; j < DIMF; j++) { mult_nn(&(Lambda[j]), &(psimat[a]), &tmat); tc = trace(&tmat); CNEGATE(tc, TF[s].Flink[a].c[j]); } for (b = a + 1; b < NUMLINK; b++) { clear_mat(&(chimat)); for (j = 0; j < DIMF; j++) { mult_na(&(Lambda[j]), &Gmat, &tmat); tc = TF[s].Fplaq.c[j]; c_scalar_mult_sum_mat(&tmat, &tc, &(chimat)); } for (j = 0; j < DIMF; j++) { mult_nn(&(chimat), &(Lambda[j]), &tmat); tc = trace(&tmat); CNEGATE(tc, TF[s].Fplaq[i].c[j]); } } } // Right side of neighboring links and psis // TODO: Presumably we can convert this to a loop... s = node_index(x - 1, t); mult_na(&(lattice[s].link[0]), &Gmat, &tmat); mat_copy(&tmat, &(lattice[s].link[0])); clear_mat(&(psimat[0])); for (j = 0; j < DIMF; j++) { mult_na(&(Lambda[j]), &Gmat, &tmat); tc = TF[s].Flink[0].c[j]; c_scalar_mult_sum_mat(&tmat, &tc, &(psimat[0])); } for (j = 0; j < DIMF; j++) { mult_nn(&(psimat[0]), &(Lambda[j]), &tmat); tc = trace(&tmat); CNEGATE(tc, TF[s].Flink[0].c[j]); } s = node_index(x, t - 1); mult_na(&(lattice[s].link[3]), &Gmat, &tmat); mat_copy(&tmat, &(lattice[s].link[3])); clear_mat(&(psimat[3])); for (j = 0; j < DIMF; j++) { mult_na(&(Lambda[j]), &Gmat, &tmat); tc = TF[s].Flink[3].c[j]; c_scalar_mult_sum_mat(&tmat, &tc, &(psimat[3])); } for (j = 0; j < DIMF; j++) { mult_nn(&(psimat[3]), &(Lambda[j]), &tmat); tc = trace(&tmat); CNEGATE(tc, TF[s].Flink[3].c[j]); } // Left side of neighboring chi s = node_index(x - 1, t - 1); i = plaq_index[0][3]; clear_mat(&(chimat[i])); for (j = 0; j < DIMF; j++) { mult_nn(&Gmat, &(Lambda[j]), &tmat); tc = TF[s].Fplaq[i].c[j]; c_scalar_mult_sum_mat(&tmat, &tc, &(chimat[i])); } for (j = 0; j < DIMF; j++) { mult_nn(&(Lambda[j]), &(chimat[i]), &tmat); tc = trace(&tmat); CNEGATE(tc, TF[s].Fplaq[i].c[j]); } }
void hvy_pot(int do_det) { register int i; register site *s; int t_dist, x_dist; double wloop; complex tc; matrix tmat, tmat2; msg_tag *mtag = NULL; node0_printf("hvy_pot: MAX_T = %d, MAX_X = %d\n", MAX_T, MAX_X); // Use staple to hold product of t_dist links at each point for (t_dist = 1; t_dist <= MAX_T; t_dist++) { if (t_dist == 1) { FORALLSITES(i, s) mat_copy(&(s->link[TUP]), &(staple[i])); } else { mtag = start_gather_field(staple, sizeof(matrix), goffset[TUP], EVENANDODD, gen_pt[0]); // Be careful about overwriting staple; // gen_pt may just point to it for on-node "gathers" wait_gather(mtag); FORALLSITES(i, s) mult_nn(&(s->link[TUP]), (matrix *)gen_pt[0][i], &(tempmat2[i])); cleanup_gather(mtag); FORALLSITES(i, s) mat_copy(&(tempmat2[i]), &(staple[i])); } // Copy staple to tempmat // Will shoft at end of loop FORALLSITES(i, s) mat_copy(&(staple[i]), &(tempmat[i])); for (x_dist = 0; x_dist <= MAX_X; x_dist++) { // Evaluate potential at this separation wloop = 0.0; FORALLSITES(i, s) { // Compute the actual Coulomb gauge Wilson loop product mult_na(&(staple[i]), &(tempmat[i]), &tmat); if (do_det == 1) det_project(&tmat, &tmat2); else mat_copy(&tmat, &tmat2); tc = trace(&tmat2); wloop += tc.real; } g_doublesum(&wloop); if (do_det == 1) { // Braces fix compiler error node0_printf("D_LOOP "); } else node0_printf("POT_LOOP "); node0_printf("%d %d %.6g\n", x_dist, t_dist, wloop / volume); // As we increment x, shift in x direction shiftmat(tempmat, tempmat2, goffset[XUP]); } // x_dist } // t_dist
void AD_PatchObject::paint(float4 pos, AD_Camera *telecamera, AD_Omnilight *omnilight) { int w, ntria_generated, wl; float4 inv_accum_scale, invz; float4 envdimx, envdimy, cosalpha; AD_Matrix matrixrot_all, matrixtransform_all; AD_Matrix matrixtall_clip; AD_Vect3D camerapos_inobject, vtmp, light_vertex_vector; AD_Vertex3D *v; build_objectmatrix(pos); inv_accum_scale=1.0f/accum_scale; // si porta la camera nello spazio oggetto vect_sub(&telecamera->currentpos, ¤tpos, &vtmp); mat_mulvectaffine(&inverse_rotmatrix, &vtmp, &camerapos_inobject); camerapos_inobject.x*=inv_accum_scale; camerapos_inobject.y*=inv_accum_scale; camerapos_inobject.z*=inv_accum_scale; // si calcolano le matrici di trasformazione totali mat_mulaffine(&telecamera->currentmatrix_rot, ¤tmatrix_rot, &matrixrot_all); mat_mul(&telecamera->currentmatrix, ¤tmatrix, &matrixtall_clip); // calcolo della matrice di trasformazione con inclusi i // fattori di aspect ratio; pX e pY si trovano in render.cpp mat_copy(&matrixtall_clip, &matrixtransform_all); for (w=0; w<4; w++) { matrixtransform_all.a[0][w]=matrixtransform_all.a[0][w]*telecamera->prospettivaX; matrixtransform_all.a[1][w]=matrixtransform_all.a[1][w]*telecamera->prospettivaY; } // si portano le luci nello spazio della telecamera for (w=0; w<num_omni; w++) { vect_sub(&omnilight[w].currentpos, ¤tpos, &vtmp); mat_mulvectaffine(&inverse_rotmatrix, &vtmp, &omnilight[w].currentpos_inobject); omnilight[w].currentpos_inobject.x*=inv_accum_scale; omnilight[w].currentpos_inobject.y*=inv_accum_scale; omnilight[w].currentpos_inobject.z*=inv_accum_scale; } // trasformiamo tutti i vertici geometrici for (w=0; w<num_points; w++) { // ottengo le posizioni dei vertici dal keyframer if (vert_pos[w].numkey>0) vert_pos[w].get_data(pos, &verteces[w]); // li trasformo in camera space mat_mulvect(&matrixtransform_all, &verteces[w], &verteces_tr[w]); } // trasformiamo tutti i vettori for (w=0; w<num_vectors; w++) { // ottengo le posizioni dei vettori dal keyframer if (vect_pos[w].numkey>0) vect_pos[w].get_data(pos, &vectors[w]); // li trasformo in camera space mat_mulvect(&matrixtransform_all, &vectors[w], &vectors_tr[w]); } ntria_generated=Tassellate(); TRIA_PIPELINE_ENVRGB TRIA_PIPELINE_RGB TRIA_PIPELINE_ENVMAP TRIA_PIPELINE_ELSE }
FORALLSITES(i, s) { mat_copy((matrix *)(gen_pt[0][i]), &(src[i].Fplaq[7])); // 2, 3 mat_copy((matrix *)(gen_pt[1][i]), &(src[i].Fplaq[5])); // 1, 3 mat_copy((matrix *)(gen_pt[2][i]), &(src[i].Fplaq[4])); // 1, 2 }
MATSTACK mat_pca(MATRIX data, int pca_type) { int i, j, k, k2, m, n; MATRIX evals, im; MATSTACK tmmps0 = NULL; MATRIX symmat, symmat2; m = MatCol(data); n = MatRow(data); switch(pca_type) { case MAT_PCA_CORRELATION: tmmps0 = mat_corcol(data); symmat = tmmps0[1]; break; case MAT_PCA_COVARIANCE: tmmps0 = mat_covcol(data); symmat = tmmps0[1]; break; case MAT_PCA_SUMOFSQUARES: symmat = mat_scpcol(data); break; default: tmmps0 = mat_covcol(data); symmat = tmmps0[1]; break; } evals = mat_creat(m, 1, UNDEFINED); im = mat_creat(m, 1, UNDEFINED); symmat2 = mat_copy(symmat, NULL); mat_tred2(symmat, evals, im); mat_tqli(evals, im, symmat); for(i=0; i<n; ++i) { for(j=0; j<m; ++j) { im[j][0] = tmmps0[2][i][j]; } for(k=0; k<3; ++k) { tmmps0[2][i][k] = 0.0; for(k2=0; k2<m; ++k2) { tmmps0[2][i][k] += im[k2][0] * symmat[k2][m-k-1]; } } } for(j=0; j<m; ++j) { for(k=0; k<m; ++k) { im[k][0] = symmat2[j][k]; } for(i=0; i<3; ++i) { symmat2[j][i] = 0.0; for (k2=0; k2<m; ++k2) { symmat2[j][i] += im[k2][0] * symmat[k2][m-i-1]; } if(evals[m-i-1][0]>0.0005) symmat2[j][i] /= (mtype)sqrt(evals[m-i-1][0]); else symmat2[j][i] = 0.0; } } mat_free(evals); mat_free(im); return tmmps0; }
// 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; }