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;
}
