void close_nav(void){
//free memory space
mat_free(F);
mat_free(PHI);
mat_free(P);
mat_free(G);
mat_free(Rw);
mat_free(Q);
mat_free(Qw);
mat_free(R);
mat_free(eul);
mat_free(x);
mat_free(y);
mat_free(Rbodtonav);
mat_free(grav);
mat_free(f_b);
mat_free(om_ib);
mat_free(nr);
mat_free(K);
mat_free(H);
mat_free(C_N2B);
mat_free(C_B2N);
mat_free(pos_ref);
mat_free(pos_ins_ecef);
mat_free(pos_ins_ned);
mat_free(pos_gps);
mat_free(pos_gps_ecef);
mat_free(pos_gps_ned);
mat_free(I15);
mat_free(I3);
mat_free(ImKH);
mat_free(KRKt);
mat_free(dx);
mat_free(a_temp31);
mat_free(b_temp31);
mat_free(temp33);
mat_free(atemp33);
mat_free(temp1515);
mat_free(temp615);
mat_free(temp66);
mat_free(atemp66);
mat_free(temp156);
mat_free(temp1512);
}
matrix_t * mpi_mat_rand(
idx_t const mode,
idx_t const nfactors,
permutation_t const * const perm,
rank_info * const rinfo)
{
idx_t const localdim = rinfo->mat_end[mode] - rinfo->mat_start[mode];
matrix_t * mymat = mat_alloc(localdim, nfactors);
MPI_Status status;
/* figure out buffer sizes */
idx_t maxlocaldim = localdim;
if(rinfo->rank == 0) {
MPI_Reduce(MPI_IN_PLACE, &maxlocaldim, 1, SPLATT_MPI_IDX, MPI_MAX, 0,
rinfo->comm_3d);
} else {
MPI_Reduce(&maxlocaldim, NULL, 1, SPLATT_MPI_IDX, MPI_MAX, 0,
rinfo->comm_3d);
}
/* root rank does the heavy lifting */
if(rinfo->rank == 0) {
/* allocate buffers */
idx_t * loc_perm = splatt_malloc(maxlocaldim * sizeof(*loc_perm));
val_t * vbuf = splatt_malloc(maxlocaldim * nfactors * sizeof(*vbuf));
/* allocate initial factor */
matrix_t * full_factor = mat_rand(rinfo->global_dims[mode], nfactors);
/* copy root's own matrix to output */
#pragma omp parallel for schedule(static)
for(idx_t i=0; i < localdim; ++i) {
idx_t const gi = rinfo->mat_start[mode] + perm->iperms[mode][i];
for(idx_t f=0; f < nfactors; ++f) {
mymat->vals[f + (i*nfactors)] = full_factor->vals[f+(gi*nfactors)];
}
}
/* communicate! */
for(int p=1; p < rinfo->npes; ++p) {
/* first receive layer start and permutation info */
idx_t layerstart;
idx_t nrows;
MPI_Recv(&layerstart, 1, SPLATT_MPI_IDX, p, 0, rinfo->comm_3d, &status);
MPI_Recv(&nrows, 1, SPLATT_MPI_IDX, p, 1, rinfo->comm_3d, &status);
MPI_Recv(loc_perm, nrows, SPLATT_MPI_IDX, p, 2, rinfo->comm_3d, &status);
/* fill buffer */
#pragma omp parallel for schedule(static)
for(idx_t i=0; i < nrows; ++i) {
idx_t const gi = layerstart + loc_perm[i];
for(idx_t f=0; f < nfactors; ++f) {
vbuf[f + (i*nfactors)] = full_factor->vals[f+(gi*nfactors)];
}
}
/* send to rank p */
MPI_Send(vbuf, nrows * nfactors, SPLATT_MPI_VAL, p, 3, rinfo->comm_3d);
}
mat_free(full_factor);
splatt_free(loc_perm);
splatt_free(vbuf);
/* other ranks just send/recv */
} else {
/* send permutation info to root */
MPI_Send(&(rinfo->layer_starts[mode]), 1, SPLATT_MPI_IDX, 0, 0, rinfo->comm_3d);
MPI_Send(&localdim, 1, SPLATT_MPI_IDX, 0, 1, rinfo->comm_3d);
MPI_Send(perm->iperms[mode] + rinfo->mat_start[mode], localdim,
SPLATT_MPI_IDX, 0, 2, rinfo->comm_3d);
/* receive factor */
MPI_Recv(mymat->vals, mymat->I * mymat->J, SPLATT_MPI_VAL, 0, 3,
rinfo->comm_3d, &status);
}
return mymat;
}
void mpi_write_mats(
matrix_t ** mats,
permutation_t const * const perm,
rank_info const * const rinfo,
char const * const basename,
idx_t const nmodes)
{
char * fname;
idx_t const nfactors = mats[0]->J;
MPI_Status status;
idx_t maxdim = 0;
idx_t maxlocaldim = 0;
matrix_t * matbuf = NULL;
val_t * vbuf = NULL;
idx_t * loc_iperm = NULL;
for(idx_t m=0; m < nmodes; ++m) {
maxdim = SS_MAX(maxdim, rinfo->global_dims[m]);
maxlocaldim = SS_MAX(maxlocaldim, mats[m]->I);
}
/* get the largest local dim */
if(rinfo->rank == 0) {
MPI_Reduce(MPI_IN_PLACE, &maxlocaldim, 1, SPLATT_MPI_IDX, MPI_MAX, 0,
rinfo->comm_3d);
} else {
MPI_Reduce(&maxlocaldim, NULL, 1, SPLATT_MPI_IDX, MPI_MAX, 0,
rinfo->comm_3d);
}
if(rinfo->rank == 0) {
matbuf = mat_alloc(maxdim, nfactors);
loc_iperm = (idx_t *) splatt_malloc(maxdim * sizeof(idx_t));
vbuf = (val_t *) splatt_malloc(maxdim * nfactors * sizeof(val_t));
}
for(idx_t m=0; m < nmodes; ++m) {
/* root handles the writing */
if(rinfo->rank == 0) {
asprintf(&fname, "%s%"SPLATT_PF_IDX".mat", basename, m+1);
matbuf->I = rinfo->global_dims[m];
/* copy root's matrix to buffer */
for(idx_t i=0; i < mats[m]->I; ++i) {
idx_t const gi = rinfo->layer_starts[m] + perm->iperms[m][i];
for(idx_t f=0; f < nfactors; ++f) {
matbuf->vals[f + (gi*nfactors)] = mats[m]->vals[f+(i*nfactors)];
}
}
/* receive matrix from each rank */
for(int p=1; p < rinfo->npes; ++p) {
idx_t layerstart;
idx_t nrows;
MPI_Recv(&layerstart, 1, SPLATT_MPI_IDX, p, 0, rinfo->comm_3d, &status);
MPI_Recv(&nrows, 1, SPLATT_MPI_IDX, p, 0, rinfo->comm_3d, &status);
MPI_Recv(vbuf, nrows * nfactors, SPLATT_MPI_VAL, p, 0, rinfo->comm_3d,
&status);
MPI_Recv(loc_iperm, nrows, SPLATT_MPI_IDX, p, 0, rinfo->comm_3d, &status);
/* permute buffer and copy into matbuf */
for(idx_t i=0; i < nrows; ++i) {
idx_t const gi = layerstart + loc_iperm[i];
for(idx_t f=0; f < nfactors; ++f) {
matbuf->vals[f + (gi*nfactors)] = vbuf[f+(i*nfactors)];
}
}
}
/* write the factor matrix to disk */
mat_write(matbuf, fname);
/* clean up */
free(fname);
} else {
/* send matrix to root */
MPI_Send(&(rinfo->layer_starts[m]), 1, SPLATT_MPI_IDX, 0, 0, rinfo->comm_3d);
MPI_Send(&(mats[m]->I), 1, SPLATT_MPI_IDX, 0, 0, rinfo->comm_3d);
MPI_Send(mats[m]->vals, mats[m]->I * mats[m]->J, SPLATT_MPI_VAL, 0, 0,
rinfo->comm_3d);
MPI_Send(perm->iperms[m] + rinfo->mat_start[m], mats[m]->I, SPLATT_MPI_IDX,
0, 0, rinfo->comm_3d);
}
} /* foreach mode */
if(rinfo->rank == 0) {
mat_free(matbuf);
free(vbuf);
free(loc_iperm);
}
}
void assembly_run(Params *p, double nfert, double **regpool, int poolsize, char *folder, char *replicate_identity)
{
// Define and initialize the history and ecosystem lists
//------------------------------------------------------
sll history;
sll_init(&history,(void*)free_seq);
sll ecosys;
sll_init(&ecosys,(void*)free_species);
// create the basal resources
//---------------------------
basal(p,&ecosys,nfert);
// create and initialize the presence matrix (ne pas oublier de mettre les espèces au minimum pour l'invasion)
//------------------------------------------
unsigned int **presence = (unsigned int**)mat_alloc(2,poolsize,sizeof(unsigned int));
for(int i = 0 ; i < poolsize ; ++i)
{
presence[0][i] = regpool[i][1];
presence[1][i] = 0;
}
// get the initial values of the sequence
//---------------------------------------
seqlevel *seq0 = fill_seq(&ecosys,0,0);
seq0->success = 0;
sll_add_head(&history,seq0);
// assemble until any species can install (or all are already installed)
//----------------------------------------------------------------------
int a = poolsize + EAM_NBNUT + EAM_NBDET; /* expected ecosystem size */
int stop_endcycles = poolsize * EAM_STOP_ENDCYCLE; /* variable to stop the sequence if infinite */
int endcycles = 0; /* 0 if endpoint / 1 if endcycle */
int unsucc = EAM_STOP; /* check the success of the invader */
int invasion = 1; /* invasion number */
while(ecosys.length != a)
{
int b = get_int(p->rng,0,poolsize); /* select randomly the number of the species in the regional pool */
if(presence[1][b] == 0) /* if the species is not already in the ecosystem */
{
ParamSP *sp = (ParamSP*)malloc(sizeof(ParamSP));
tr_sp_struct(regpool[b],sp);
install_process(p,&ecosys,sp); /* install the species */
seqlevel *seq = fill_seq(&ecosys,invasion,presence[0][b]); /* create and initialize a sequence node */
sll_rm(&ecosys,extinct_condition); /* remove the extincted species */
presence_absence(&ecosys,presence,poolsize); /* fill the presence-absence matrix */
unsucc = (presence[1][b] == 0) ? unsucc-1 : EAM_STOP; /* upgrade the counter of unsuccessfull installations */
seq->success = presence[1][b]; /* fill the success of invader installation in the sequence node */
sll_add_head(&history,seq); /* add the sequence node to the assembly history */
++invasion; /* upgrade the invasion number */
--stop_endcycles; /* upgrade the endcycle detector */
}
if(unsucc == 0) break;
if(stop_endcycles == 0) /* break the loop if its an endcycle */
{
endcycles = 1;
break;
}
}
// print the outputs : subpool / final community / sequence / abundances history / identity history
//------------------
print_final_community(&ecosys,folder,replicate_identity);
print_id_history(&history,poolsize,folder,replicate_identity);
print_final_biomasses_history(&history,poolsize,folder,replicate_identity);
print_sequence(&history,folder,replicate_identity);
char *buffer = (char*)malloc(100);
sprintf(buffer, "%s/%s_endcycle.txt",folder,replicate_identity);
FILE *out = fopen(buffer,"w");
fprintf(out,"%d",endcycles);
fclose(out);
// free the memory
//----------------
mat_free((void**)presence,2);
sll_rm_all(&history);
sll_rm_all(&ecosys);
free(buffer);
}
/*
*-----------------------------------------------------------------------------
* funct: mat_inv
* desct: find inverse of a matrix
* given: a = square matrix a, and C, return for inv(a)
* retrn: square matrix Inverse(A), C
* NULL = fails, singular matrix
*-----------------------------------------------------------------------------
*/
MATRIX mat_inv( MATRIX a , MATRIX C)
{
MATRIX A, B, P;
int i, n;
#ifdef CONFORM_CHECK
if(MatCol(a)!=MatCol(C))
{
error("\nUnconformable matrices in routine mat_inv(): Col(A)!=Col(B)\n");
}
if(MatRow(a)!=MatRow(C))
{
error("\nUnconformable matrices in routine mat_inv(): Row(A)!=Row(B)\n");
}
#endif
n = MatCol(a);
A = mat_creat(MatRow(a), MatCol(a), UNDEFINED);
A = mat_copy(a, A);
B = mat_creat( n, 1, UNDEFINED );
P = mat_creat( n, 1, UNDEFINED );
/*
* - LU-decomposition -
* also check for singular matrix
*/
if (mat_lu(A, P) == -1)
{
mat_free(A);
mat_free(B);
mat_free(P);
return (NULL);
}
else
{
/* Bug??? was still mat_backsubs1 even when singular??? */
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);
return (C);
}
}
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);
}
// make sure C is 3 x 3
MATRIX mat_T321 (double pitch, double roll, double yaw, MATRIX C)
{
MATRIX TI2,T23,T3B,t1;
double cphi,sphi,ctheta,stheta,cpsi,spsi;
// if dimensions of C is wrong
if ( MatCol(C) != 3 || MatRow(C) != 3 ) {
printf("mat_T321 error: incompatible output matrix size\n");
_exit(-1);
// if dimensions of C is correct
} else {
if ((TI2 = mat_creat( 3, 3, UNDEFINED )) == NULL)
return (NULL);
if ((T23 = mat_creat( 3, 3, UNDEFINED )) == NULL)
return (NULL);
if ((T3B = mat_creat( 3, 3, UNDEFINED )) == NULL)
return (NULL);
cphi = cos(roll);
sphi = sin(roll);
ctheta = cos(pitch);
stheta = sin(pitch);
cpsi = cos(yaw);
spsi = sin(yaw);
TI2[0][0] = spsi;
TI2[0][1] = cpsi;
TI2[0][2] = 0;
TI2[1][0] = cpsi;
TI2[1][1] = -spsi;
TI2[1][2] = 0;
TI2[2][0] = 0;
TI2[2][1] = 0;
TI2[2][2] = -1;
T23[0][0] = ctheta;
T23[0][1] = 0;
T23[0][2] = -stheta;
T23[1][0] = 0;
T23[1][1] = 1;
T23[1][2] = 0;
T23[2][0] = stheta;
T23[2][1] = 0;
T23[2][2] = ctheta;
T3B[0][0] = 1;
T3B[0][1] = 0;
T3B[0][2] = 0;
T3B[1][0] = 0;
T3B[1][1] = cphi;
T3B[1][2] = sphi;
T3B[2][0] = 0;
T3B[2][1] = -sphi;
T3B[2][2] = cphi;
// C = T3B*T23*TI2;
if ( ( t1 = mat_creat(3,3,UNDEFINED) ) == NULL )
return (NULL);
mat_mul(T23,TI2,t1);
mat_mul(T3B,t1, C);
}
mat_free(T3B);
mat_free(T23);
mat_free(TI2);
mat_free(t1);
return(C);
}
/* Subtree version of score test */
void ff_score_tests_sub(TreeModel *mod, MSA *msa, GFF_Set *gff, mode_type mode,
double *feat_pvals, double *feat_null_scales,
double *feat_derivs, double *feat_sub_derivs,
double *feat_teststats, FILE *logf) {
int i;
FeatFitData *d, *d2;
Vector *grad = vec_new(2);
Matrix *fim = mat_new(2, 2);
double lnl, teststat;
FimGrid *grid;
List *inside=NULL, *outside=NULL;
TreeModel *modcpy = tm_create_copy(mod); /* need separate copy of tree model
with different internal scaling
data for supertree/subtree case */
/* init FeatFitData -- one for null model, one for alt */
d = ff_init_fit_data(modcpy, msa, ALL, NNEUT, FALSE);
d2 = ff_init_fit_data(mod, msa, SUBTREE, NNEUT, FALSE);
/* mod has the subtree info, modcpy
does not */
/* precompute Fisher information matrices for a grid of scale values */
grid = col_fim_grid_sub(mod);
/* prepare lists of leaves inside and outside root, for use in
checking for informative substitutions */
if (mod->subtree_root != NULL) {
inside = lst_new_ptr(mod->tree->nnodes);
outside = lst_new_ptr(mod->tree->nnodes);
tr_partition_leaves(mod->tree, mod->subtree_root, inside, outside);
}
/* iterate through features */
for (i = 0; i < lst_size(gff->features); i++) {
checkInterrupt();
d->feat = lst_get_ptr(gff->features, i);
/* first check for informative substitution data in feature; if none,
don't waste time computing likelihoods */
if (!ff_has_data_sub(mod, msa, d->feat, inside, outside)) {
teststat = 0;
vec_zero(grad);
}
else {
vec_set(d->cdata->params, 0, d->cdata->init_scale);
opt_newton_1d(ff_likelihood_wrapper_1d, &d->cdata->params->data[0], d,
&lnl, SIGFIGS, d->cdata->lb->data[0], d->cdata->ub->data[0],
logf, NULL, NULL);
/* turns out to be faster to use numerical rather than exact
derivatives (judging by col case) */
d2->feat = d->feat;
d2->cdata->mod->scale = d->cdata->params->data[0];
d2->cdata->mod->scale_sub = 1;
tm_set_subst_matrices(d2->cdata->mod);
ff_scale_derivs_subtree(d2, grad, NULL, d2->cdata->fels_scratch);
fim = col_get_fim_sub(grid, d2->cdata->mod->scale);
mat_scale(fim, d->feat->end - d->feat->start + 1);
/* scale column-by-column FIM by length of feature (expected
values are additive) */
teststat = grad->data[1]*grad->data[1] /
(fim->data[1][1] - fim->data[0][1]*fim->data[1][0]/fim->data[0][0]);
if (teststat < 0) {
fprintf(stderr, "WARNING: teststat < 0 (%f)\n", teststat);
teststat = 0;
}
if ((mode == ACC && grad->data[1] < 0) ||
(mode == CON && grad->data[1] > 0))
teststat = 0; /* derivative points toward boundary;
truncate at 0 */
mat_free(fim);
}
if (feat_pvals != NULL) {
if (mode == NNEUT || mode == CONACC)
feat_pvals[i] = chisq_cdf(teststat, 1, FALSE);
else
feat_pvals[i] = half_chisq_cdf(teststat, 1, FALSE);
/* assumes 50:50 mix of chisq and point mass at zero */
if (feat_pvals[i] < 1e-20)
feat_pvals[i] = 1e-20;
/* approx limit of eval of tail prob; pvals of 0 cause problems */
if (mode == CONACC && grad->data[1] > 0)
feat_pvals[i] *= -1; /* mark as acceleration */
}
/* store scales and log likelihood ratios if necessary */
if (feat_null_scales != NULL) feat_null_scales[i] = d->cdata->params->data[0];
if (feat_derivs != NULL) feat_derivs[i] = grad->data[0];
if (feat_sub_derivs != NULL) feat_sub_derivs[i] = grad->data[1];
if (feat_teststats != NULL) feat_teststats[i] = teststat;
}
ff_free_fit_data(d);
ff_free_fit_data(d2);
vec_free(grad);
modcpy->estimate_branchlens = TM_BRANCHLENS_ALL;
/* have to revert for tm_free to work
correctly */
tm_free(modcpy);
col_free_fim_grid(grid);
if (inside != NULL) lst_free(inside);
if (outside != NULL) lst_free(outside);
}
void treatments(void *param, unsigned long s)
{
//------------//
// LOCAL COPY //
//------------//
Params *P = (Params*)param;
//------------------------------------------------------//
// OPEN A NEW FOLDER AND SAVE THE REGIONAL POOL SPECIES //
//------------------------------------------------------//
char *buffer = (char*)malloc(100);
sprintf(buffer, "SIMU_%lu",s);
mkdir(buffer, S_IRWXU); // S_IRWXU is the mode, it give read/write/search access to the user.
print_global_parameters(P, s,buffer);
//-----------------//
// TREATMENT LOOPS //
//-----------------//
char buffer2[100];
for(int i = 0 ; i < P->nsize ; ++i) // SIZE POOL LOOP
{
for(int j = 0 ; j < P->nfert ; ++j) // FERTILITY LOOP
{
for(int k = 1 ; k <= P->nrep ; ++k) // REPLICATE LOOP
{
// create a subset of the regional pool
//-------------------------------------
double **regpool = (double**)mat_alloc((P->sizevalues)[i],21,sizeof(double));
if(P->pooltype)
{
create_assembled_pool(P,regpool,(P->sizevalues)[i],(P->fertvalues)[j]);
}
else
{
create_random_pool(P,regpool,(P->sizevalues)[i]);
interaction_random_pool(regpool,(P->sizevalues)[i]);
}
// print the regional species pool
//--------------------------------
char filename1[100];
sprintf(filename1, "%s/S%d_F%.2f_REP%d_regpool.txt",buffer,(P->sizevalues)[i],(P->fertvalues)[j],k);
print_regional_pool(regpool,(P->sizevalues)[i],21,filename1);
// run the assembly sequence replication (with print outputs)
//-----------------------------------------------------------
for(int seq = 1 ; seq <= P->nseq ; ++seq)
{
// create the filename parameter part
//-----------------------------------
char filename2[100];
sprintf(filename2,"%s/S%d_F%.2f_REP%d_SEQ%d",buffer,(P->sizevalues)[i],(P->fertvalues)[j],k,seq);
// run the assembly sequence (and print the outputs)
//--------------------------------------------------
assembly_run(P,(P->fertvalues)[j],regpool,(P->sizevalues)[i],filename2);
}
// Free the memory
//----------------
mat_free((void**)regpool,(P->sizevalues)[i]);
} // END REPLICATE LOOP
} // END FERTILITY LOOP
} // END SIZE POOL LOOP
//-----------------//
// Free the memory //
//-----------------//
free(buffer);
}
int main()
{
unsigned r1, c1, r2, c2, ch;
mat m1, m2, t;
puts("Enter row and column numbers of matrices 1 and 2:");
scanf(" %u %u %u %u%*c", &r1, &c1, &r2, &c2);
putchar('\n');
m1=mat_alloc(r1, c1);
m2=mat_alloc(r2, c2);
printf("Enter value of Matrix 1 (%ux%u):\n", r1, c1);
mat_read(m1);
putchar('\n');
printf("Enter value of Matrix 2 (%ux%u):\n", r2, c2);
mat_read(m2);
putchar('\n');
do{
puts("What would you like to do?");
puts(" ( 0) Exit");
puts(" ( 1) Display");
puts(" ( 2) Transpose");
puts(" ( 3) Sum");
puts(" ( 4) Difference");
puts(" ( 5) Product");
puts(" ( 6) Greatest Element of Rows");
puts(" ( 7) Sum of Major Diagonal");
puts(" ( 8) Sum of Minor Diagonal");
puts(" ( 9) Check Symmetricity");
puts(" (10) Upper-Triangular Matrix");
puts(" (11) Lower-Triangular Matrix");
scanf(" %u%*c", &ch);
switch(ch){
case 0:
puts("Bye!");
break;
case 1:
puts("Matrix 1:");
mat_write(m1);
putchar('\n');
puts("Matrix 2:");
mat_write(m2);
break;
case 2:
t=mat_trn(m1);
mat_write(t);
mat_free(t);
break;
case 3:
if((t=mat_add(m1, m2)).r){
mat_write(t);
mat_free(t);
}
else puts("Not Possible");
break;
case 4:
if((t=mat_sub(m1, m2)).r){
mat_write(t);
mat_free(t);
}
else puts("Not Possible");
break;
case 5:
if((t=mat_mul(m1, m2)).r){
mat_write(t);
mat_free(t);
}
else puts("Not Possible");
break;
case 6:
row_great(m1);
break;
case 7:
add_major(m1);
break;
case 8:
add_minor(m1);
break;
case 9:
if(issymm(m1)) puts("Symmetric");
else puts("Unsymmetric");
break;
case 10:
up_tri(m1);
break;
case 11:
lo_tri(m1);
break;
default:
puts("Incorrect Choice!");
break;
}
putchar('\n');
} while(ch);
mat_free(m1);
mat_free(m2);
return 0;
}
void EcefToLatLonAlt(MATRIX vector)
{
int i;
double x, y, z, q, p, sinlat, sinlat2;
double a, b, d, radius, lat, alt, dummy;
double E_WGS84, E2_WGS84, ONE_MIN_E2, A_WGS84;
MATRIX lla;
lla = mat_creat(3,1,ZERO_MATRIX);
E_WGS84 = ECCENTRICITY; /* Earth ellipse ecc - unitless */
E2_WGS84 = E_WGS84*E_WGS84; /* Earth's ellipse ecc^2 - unitless */
ONE_MIN_E2 = 1.0 - E2_WGS84;
A_WGS84 = EARTH_RADIUS; /* Earth's ellipse semi-major axis - meters */
x = vector[0][0];
y = vector[1][0];
z = vector[2][0];
lla[1][0] = atan2(y, x); /* Longitude */
p = sqrt((x * x) + (y * y)); /* Latitude and Altitude */
if (p < 0.1)
{
p = 0.1;
}
q = z / p;
alt = 0.0;
lat = atan(q * (1.0 / ONE_MIN_E2));
a = 1.0;
i = 0;
while ((a > 0.2) && (i < 20))
{
sinlat = sin(lat);
sinlat2 = sinlat * sinlat;
dummy =sqrt((1.0 - (E2_WGS84 * sinlat2))*(1.0 - (E2_WGS84 * sinlat2)));
radius = A_WGS84 / sqrt(dummy);
d = alt;
alt = (p / cos(lat)) - radius;
a = q * (radius + alt);
b = (ONE_MIN_E2 * radius) + alt;
lat = atan2(a, b);
a = sqrt((alt - d)*(alt - d));
i = i + 1;
}
lla[0][0] = lat;
lla[2][0] = alt;
for (i = 0; i < 3; i++)
{
vector[i][0] = lla[i][0];
}
mat_free(lla);
}
// アレする
int mat_solve(matrix *mat1, matrix mat2, matrix mat3){
int i, j, k, l;
double multiplier, divisor;
matrix a, b;
if(check_square(mat2) != 0 || mat2.row != mat3.row || mat1->row != mat3.row || mat1->col != mat3.col ){
return -1;
}
mat_alloc(&a, mat2.row, mat2.col);
mat_alloc(&b, mat3.row, mat3.col);
mat_copy(&a, mat2);
mat_copy(&b, mat3);
mat_print(a);
mat_print(b);
// Gaussの消去法
for(i=0; i<a.row; i++){
// i+1行目以降のi列目を消す（i=0のとき、2行目〜最終行の1列目を消去）
for(j=i+1; j<a.row; j++){
// j行目に、i行目を何倍したら、i列目が消えるか（例えば、i=0のとき、2行目以降の行全体に、何倍すれば、1列目が0になるか）
multiplier = a.element[j][i] / a.element[i][i];
for(k=0; k<a.col; k++){
a.element[j][k] = a.element[j][k] - a.element[i][k] * multiplier;
}
for(l=0; l<b.col; l++){
b.element[j][l] = b.element[j][l] - b.element[i][l] * multiplier;
}
}
}
// 後退代入
// 最終行以前の行に対して
for(i=a.row-1; i>=0; i--){
// 例えば、今2行目なら、3列目〜最終列に対して処理を行う
for(j=a.row-1; j>i; j--){
divisor = a.element[i][j];
a.element[i][j] = a.element[i][j] - divisor * a.element[j][j];
for(l=0; l<b.col; l++){
b.element[i][l] = b.element[i][l] - divisor * b.element[j][l];
}
}
for(l=0; l<b.col; l++){
b.element[i][l] = b.element[i][l] / a.element[i][i];
}
a.element[i][i] = 1.0;
}
mat_copy(mat1, b);
mat_free(&a);
mat_free(&b);
return 0;
}
//---------------------------------------------------------------------------
// Compute determinant of the matrix
NaReal NaMatrix::det () const
{
#ifdef __matrix_h
if(dim_rows() != dim_cols()){
throw(na_size_mismatch);
}
NaReal vDet = 0;
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);
}
}
vDet = mat_det(mat);
mat_free(mat);
return vDet;
#else
throw(na_not_implemented);
#endif/* __matrix_h */
#if 0
unsigned i, j, k, n = dim_rows();
// Predefined det()
switch(n){
case 1:
vDet = get(0, 0);
break;
case 2:
vDet = get(0, 0) * get (1, 1) - get(0, 1) * get(1, 0);
break;
default:{
// Allocate space for minor matrix
NaMatrix mOfMinor(n - 1, n - 1);
for(i = 0; i < n; ++i){
// Compose minor
for(j = 0; j < n - 1; ++j){
for(k = 0; k < n - 1; ++k){
if(k < i){
mOfMinor[j][k] = get(1 + j, k);
}else if(k >= i){
mOfMinor[j][k] = get(1 + j, 1 + k);
}
}
}// minor composition
//NaPrintLog("Minor(%d,%d):\n", i, n - 1);
//mOfMinor.print_contents();
// Summarize determinant
if(i % 2){
vDet -= get(0, i) * mOfMinor.det();
}else{
vDet += get(0, i) * mOfMinor.det();
}
}
}break;
}// end of switch for different cases
//NaPrintLog("det=%g\n", vDet);
return vDet;
#endif
}
extern void close_ss02_control(void){
//free memory space
mat_free(M);
mat_free(u);
mat_free(y);
}
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;
}
mat mars(Agraph_t* g, struct marsopts opts)
{
int i, j, n = agnnodes(g), k = MIN(n, MAX(opts.k, 2)), iter = 0;
mat dij, u, u_trans, q, r, q_t, tmp, tmp2, z;
double* s = (double*) malloc(sizeof(double)*k);
double* ones = (double*) malloc(sizeof(double)*n);
double* d;
int* anchors = (int*) malloc(sizeof(int)*k);
int* clusters = NULL;
double change = 1, old_stress = -1;
dij = mat_new(k, n);
u = mat_new(n,k);
tmp = mat_new(n,k);
darrset(ones,n,-1);
select_anchors(g, dij, anchors, k);
if(opts.color) {
for(i = 0; i < k; i++) {
Agnode_t* anchor = get_node(anchors[i]);
agset(anchor, "color", "red");
}
}
if(opts.power != 1) {
clusters = graph_cluster(g,dij,anchors);
}
singular_vectors(g, dij, opts.power, u, s);
vec_scalar_mult(s, k, -1);
u_trans = mat_trans(u);
d = mat_mult_for_d(u, s, u_trans, ones);
for(i = 0; i < u->c; i++) {
double* col = mat_col(u,i);
double* b = inv_mul_ax(d,col,u->r);
for(j = 0; j < u->r; j++) {
tmp->m[mindex(j,i,tmp)] = b[j];
}
free(b);
free(col);
}
tmp2 = mat_mult(u_trans,tmp);
for(i = 0; i < k; i++) {
tmp2->m[mindex(i,i,tmp2)] += (1.0/s[i]);
}
q = mat_new(tmp2->r, tmp2->c);
r = mat_new(tmp2->c, tmp2->c);
qr_factorize(tmp2,q,r);
q_t = mat_trans(q);
if(opts.given) {
z = get_positions(g, opts.dim);
} else {
z = mat_rand(n, opts.dim);
}
translate_by_centroid(z);
if(opts.viewer) {
init_viewer(g, opts.max_iter);
append_layout(z);
}
old_stress = stress(z, dij, anchors, opts.power);
while(change > EPSILON && iter < opts.max_iter) {
mat right_side;
double new_stress;
if(opts.power == 1) {
right_side = barnes_hut(z);
} else {
right_side = barnes_hut_cluster(z, dij, clusters, opts.power);
}
for(i = 0; i < opts.dim; i++) {
double sum = 0;
double* x;
double* b = mat_col(right_side,i);
for(j = 0; j < right_side->r; j++) {
sum += b[j];
}
x = inv_mul_full(d, b, right_side->r, u, u_trans, q_t, r);
for(j = 0; j < z->r; j++) {
z->m[mindex(j,i,z)] = x[j] - sum/right_side->r;
}
free(x);
free(b);
}
adjust_anchors(g, anchors, k, z);
update_anchors(z, dij, anchors, opts.power);
translate_by_centroid(z);
if(opts.viewer) {
append_layout(z);
}
new_stress = stress(z, dij, anchors, opts.power);
change = fabs(new_stress-old_stress)/old_stress;
old_stress = new_stress;
mat_free(right_side);
iter++;
}
mat_free(dij);
mat_free(u);
mat_free(u_trans);
mat_free(q);
mat_free(r);
mat_free(q_t);
mat_free(tmp);
mat_free(tmp2);
free(s);
free(ones);
free(d);
free(anchors);
free(clusters);
return z;
}
void blockwise_closed_form (double ** ytwo, double ** ztwo, double ** wtwo, double rho, double* lambda, int N) {
// STEP ONE: compute the optimal solution for truncated problem
double ** wbar = mat_init (N, N);
mat_zeros (wbar, N, N);
mat_dot (rho, ztwo, wbar, N, N); // wbar = rho * z_2
mat_sub (wbar, ytwo, wbar, N, N); // wbar = rho * z_2 - y_2
mat_dot (1.0/rho, wbar, wbar, N, N); // wbar = (rho * z_2 - y_2) / rho
// STEP TWO: find the closed-form solution for second subproblem
for (int j = 0; j < N; j ++) {
// 1. bifurcate the set of values
vector< pair<int,double> > alpha_vec;
for (int i = 0; i < N; i ++) {
double value = wbar[i][j];
/*if( wbar[i][j] < 0 ){
cerr << "wbar[" << i << "][" << j << "]" << endl;
exit(0);
}*/
alpha_vec.push_back (make_pair(i, abs(value)));
}
// 2. sorting
std::sort (alpha_vec.begin(), alpha_vec.end(), pairComparator);
/*
for (int i = 0; i < N; i ++) {
if (alpha_vec[i].second != 0)
cout << alpha_vec[i].second << endl;
}
*/
// 3. find mstar
int mstar = 0; // number of elements support the sky
double separator;
double max_term = -INF, new_term;
double sum_alpha = 0.0;
for (int i = 0; i < N; i ++) {
sum_alpha += alpha_vec[i].second;
new_term = (sum_alpha - lambda[j]) / (i + 1.0);
if ( new_term > max_term ) {
separator = alpha_vec[i].second;
max_term = new_term;
mstar = i;
}
}
// 4. assign closed-form solution to wtwo
if( max_term < 0 ) {
for(int i=0; i<N; i++)
wtwo[i][j] = 0.0;
continue;
}
for (int i = 0; i < N; i ++) {
// harness vector of pair
double value = wbar[i][j];
if ( abs(value) >= separator ) {
wtwo[i][j] = max_term;
} else {
// its ranking is above m*, directly inherit the wbar
wtwo[i][j] = max(wbar[i][j],0.0);
}
}
}
// compute value of objective function
double penalty = second_subproblem_obj (ytwo, ztwo, wtwo, rho, N, lambda);
// report the #iter and objective function
/*cout << "[Blockwise] second_subproblem_obj: " << penalty << endl;
cout << endl;*/
// STEP THREE: recollect temporary variable - wbar
mat_free (wbar, N, N);
}
int main (int argc, char ** argv) {
if (argc < 5) {
cerr << "Usage: " << endl;
cerr << "\tHDP_MEDOIDS [dataFile] [nRuns] [lambda_global] [lambda_local]" << endl;
exit(-1);
}
// PARSE arguments
char* dataFile = argv[1];
int nRuns = atoi(argv[2]);
vector<double> LAMBDAs (2, 0.0);
LAMBDAs[0] = atof(argv[3]); // lambda_global
LAMBDAs[1] = atof(argv[4]); // lambda_local
objmin_trace << "time objective" << endl;
// read in data
int FIX_DIM;
Parser parser;
vector<Instance*>* pdata;
vector<Instance*> data;
pdata = parser.parseSVM(dataFile, FIX_DIM);
data = *pdata;
// init lookup_tables
vector< pair<int,int> > doc_lookup;
get_doc_lookup (data, doc_lookup);
Lookups lookup_tables;
lookup_tables.doc_lookup = &doc_lookup;
lookup_tables.nWords = data.size();
lookup_tables.nDocs = lookup_tables.doc_lookup->size();
int seed = time(NULL);
srand (seed);
cerr << "###########################################" << endl;
cerr << "nDocs = " << lookup_tables.nDocs << endl; // # documents
cerr << "nWords = " << lookup_tables.nWords << endl; // # words
cerr << "lambda_global = " << LAMBDAs[0] << endl;
cerr << "lambda_local = " << LAMBDAs[1] << endl;
cerr << "TRIM_THRESHOLD = " << TRIM_THRESHOLD << endl;
cerr << "seed = " << seed << endl;
cerr << "###########################################" << endl;
// Run sparse convex clustering
int N = lookup_tables.nWords;
int D = lookup_tables.nDocs;
double** W = mat_init (N, N);
// dist_mat computation and output
dist_func df = L2norm;
double** dist_mat = mat_init (N, N);
compute_dist_mat (data, dist_mat, N, FIX_DIM, df, true);
start_time = omp_get_wtime();
double min_obj = INF;
vector< vector<double> > min_means;
for (int j = 0; j < nRuns; j ++) {
vector< vector<double> > means;
// inner-doc shuffle
for (int d = 0; d < D; d++) {
int begin_i = doc_lookup[d].first;
int end_i = doc_lookup[d].second;
random_shuffle(data.begin()+begin_i, data.begin()+end_i);
}
// between-doc shuffle
vector<pair<int,int> > s_doc_lookup (doc_lookup);
random_shuffle(s_doc_lookup.begin(), s_doc_lookup.end());
vector<Instance*> s_data (N, NULL);
int p = 0;
for (int d = 0; d < D; d ++) {
for (int i = s_doc_lookup[d].first; i < s_doc_lookup[d].second; i ++) {
s_data[p] = data[i];
p ++;
}
}
lookup_tables.doc_lookup = &s_doc_lookup;
double obj = HDP_MEDOIDS (s_data, means, &lookup_tables, LAMBDAs, df, FIX_DIM);
lookup_tables.doc_lookup = &doc_lookup;
cerr << "###################################################" << endl;
if (obj < min_obj) {
min_obj = obj;
min_means = means;
}
}
/* Output objective */
output_objective (min_obj);
ofstream model_out ("opt_model");
for (int i = 0; i < min_means.size(); i ++) {
model_out << "mean[" << i << "] ";
for (int j = 0; j < min_means[i].size(); j ++) {
model_out << min_means[i][j] << " " ;
}
model_out << endl;
}
model_out.close();
/* Output cluster centroids */
// output_model (W, &lookup_tables);
/* Output assignment */
// output_assignment (W, &lookup_tables);
/* reallocation */
mat_free (W, N, N);
mat_free (dist_mat, N, N);
objmin_trace.close();
}
// entry main function
int main (int argc, char ** argv) {
// exception control: illustrate the usage if get input of wrong format
if (argc < 5) {
cerr << "Usage: cvx_clustering [dataFile] [fw_max_iter] [max_iter] [lambda] " << endl;
cerr << "Note: dataFile must be scaled to [0,1] in advance." << endl;
exit(-1);
}
// parse arguments
char * dataFile = argv[1];
int fw_max_iter = atoi(argv[2]);
int max_iter = atoi(argv[3]);
double lambda_base = atof(argv[4]);
char * dmatFile = argv[5];
// vector<Instance*> data;
// readFixDim (dataFile, data, FIX_DIM);
// read in data
int FIX_DIM;
Parser parser;
vector<Instance*>* pdata;
vector<Instance*> data;
pdata = parser.parseSVM(dataFile, FIX_DIM);
data = *pdata;
// vector<Instance*> data;
// readFixDim (dataFile, data, FIX_DIM);
// explore the data
int dimensions = -1;
int N = data.size(); // data size
for (int i = 0; i < N; i++) {
vector< pair<int,double> > * f = &(data[i]->fea);
int last_index = f->size()-1;
if (f->at(last_index).first > dimensions) {
dimensions = f->at(last_index).first;
}
}
assert (dimensions == FIX_DIM);
int D = dimensions;
cerr << "D = " << D << endl; // # features
cerr << "N = " << N << endl; // # instances
cerr << "lambda = " << lambda_base << endl;
cerr << "r = " << r << endl;
int seed = time(NULL);
srand (seed);
cerr << "seed = " << seed << endl;
//create lambda with noise
double* lambda = new double[N];
for(int i=0; i<N; i++) {
lambda[i] = lambda_base + noise();
}
// pre-compute distance matrix
dist_func df = L2norm;
double ** dist_mat = mat_init (N, N);
// double ** dist_mat = mat_read (dmatFile, N, N);
mat_zeros (dist_mat, N, N);
compute_dist_mat (data, dist_mat, N, D, df, true);
ofstream dist_mat_out ("dist_mat");
dist_mat_out << mat_toString(dist_mat, N, N);
dist_mat_out.close();
// Run sparse convex clustering
double ** W = mat_init (N, N);
mat_zeros (W, N, N);
cvx_clustering (dist_mat, fw_max_iter, max_iter, D, N, lambda, W);
ofstream W_OUT("w_out");
W_OUT<< mat_toString(W, N, N);
W_OUT.close();
// Output cluster
output_objective(clustering_objective (dist_mat, W, N));
/* Output cluster centroids */
output_model (W, N);
/* Output assignment */
output_assignment (W, data, N);
/* reallocation */
mat_free (dist_mat, N, N);
mat_free (W, N, N);
}
static void parse_gps( struct gps *gpsData_ptr ){
double old_GPS_TOW;
double sig_X, sig_Y, sig_Z, sig_VX, sig_VY, sig_VZ;
uint8_t finesteering_ok, solution_ok;
MATRIX ecef_mat = mat_creat(3,1,ZERO_MATRIX);
MATRIX lla_mat = mat_creat(3,1,ZERO_MATRIX);
MATRIX ned_mat = mat_creat(3,1,ZERO_MATRIX);
switch(localBuffer[5]*256 + localBuffer[4] ){
case 241: // parse BESTXYZ
endian_swap(localBuffer,14,2); // GPS Week No
endian_swap(localBuffer,16,4); // GPS_TOW
endian_swap(localBuffer,28,4); // Solution Status
endian_swap(localBuffer,28+8,8); // X
endian_swap(localBuffer,28+16,8); // Y
endian_swap(localBuffer,28+24,8); // Z
endian_swap(localBuffer,28+32,4); // stdevXe
endian_swap(localBuffer,28+36,4); // stdevYe
endian_swap(localBuffer,28+40,4); // stdevZe
endian_swap(localBuffer,28+52,8); // Vx
endian_swap(localBuffer,28+60,8); // Vy
endian_swap(localBuffer,28+68,8); // Vz
endian_swap(localBuffer,28+76,4); // stdevVx
endian_swap(localBuffer,28+80,4); // stdevVy
endian_swap(localBuffer,28+84,4); // stdevVz
gpsData_ptr->GPS_week = *((uint16_t *)(&localBuffer[14]));
gpsData_ptr->satVisible = (uint16_t)(localBuffer[28+105]);
old_GPS_TOW = gpsData_ptr->GPS_TOW;
gpsData_ptr->GPS_TOW = (double) *((uint32_t *)(&localBuffer[16])) / 1000.0;
// convert positions from ECEF to LLA
gpsData_ptr->Xe = *((double *)(&localBuffer[28+8]));
gpsData_ptr->Ye = *((double *)(&localBuffer[28+16]));
gpsData_ptr->Ze = *((double *)(&localBuffer[28+24]));
ecef_mat[0][0] = gpsData_ptr->Xe;
ecef_mat[1][0] = gpsData_ptr->Ye;
ecef_mat[2][0] = gpsData_ptr->Ze;
if(sqrt(gpsData_ptr->Xe*gpsData_ptr->Xe + gpsData_ptr->Ye*gpsData_ptr->Ye + gpsData_ptr->Ze*gpsData_ptr->Ze) < 1e-3) {
lla_mat[0][0] = 0.0;
lla_mat[1][0] = 0.0;
lla_mat[2][0] = 0.0;
} else {
lla_mat = ecef2lla(ecef_mat,lla_mat);
}
gpsData_ptr->lat = lla_mat[0][0]*R2D;
gpsData_ptr->lon = lla_mat[1][0]*R2D;
gpsData_ptr->alt = lla_mat[2][0];
// convert velocities from ECEF to NED
gpsData_ptr->Ue = *((double *)(&localBuffer[28+52]));
gpsData_ptr->Ve = *((double *)(&localBuffer[28+60]));
gpsData_ptr->We = *((double *)(&localBuffer[28+68]));
ecef_mat[0][0] = gpsData_ptr->Ue;
ecef_mat[1][0] = gpsData_ptr->Ve;
ecef_mat[2][0] = gpsData_ptr->We;
ned_mat = ecef2ned(ecef_mat,ned_mat,lla_mat);
gpsData_ptr->vn = ned_mat[0][0];
gpsData_ptr->ve = ned_mat[1][0];
gpsData_ptr->vd = ned_mat[2][0];
// convert stdev position from ECEF to NED
sig_X = (double) *((float *)(&localBuffer[28+32]));
sig_Y = (double) *((float *)(&localBuffer[28+36]));
sig_Z = (double) *((float *)(&localBuffer[28+40]));
ecef_mat[0][0] = sig_X;
ecef_mat[1][0] = sig_Y;
ecef_mat[2][0] = sig_Z;
ned_mat = ecef2ned(ecef_mat,ned_mat,lla_mat);
gpsData_ptr->sig_N = ned_mat[0][0];
gpsData_ptr->sig_E = ned_mat[1][0];
gpsData_ptr->sig_D = ned_mat[2][0];
// convert stdev velocities from ECEF to NED
sig_VX = (double) *((float *)(&localBuffer[28+76]));
sig_VY = (double) *((float *)(&localBuffer[28+80]));
sig_VZ = (double) *((float *)(&localBuffer[28+84]));
ecef_mat[0][0] = sig_VX;
ecef_mat[1][0] = sig_VY;
ecef_mat[2][0] = sig_VZ;
ned_mat = ecef2ned(ecef_mat,ned_mat,lla_mat);
gpsData_ptr->sig_vn = ned_mat[0][0];
gpsData_ptr->sig_ve = ned_mat[1][0];
gpsData_ptr->sig_vd = ned_mat[2][0];
//fprintf(stderr,"Stat:%d \n",(*((uint32_t *)(&localBuffer[28]))));
//fprintf(stderr,"Time:%d \n",(*((uint8_t *)(&localBuffer[13]))));
//endian_swap(localBuffer,20,4);
//fprintf(stderr,"Rx:%08X \n",(*((uint32_t *)(&localBuffer[20]))));
solution_ok = ((*((uint32_t *)(&localBuffer[28]))) == 0);
//finesteering_ok = ( (*((uint8_t *)(&localBuffer[13]))) == 160 ) || ( (*((uint8_t *)(&localBuffer[13]))) == 170 ) || ( (*((uint8_t *)(&localBuffer[13]))) == 180 );
finesteering_ok=1;
if(solution_ok & finesteering_ok) {
if(fabs(gpsData_ptr->GPS_TOW - old_GPS_TOW) > 1e-3) {
// Check that this is a new data (no GPS outage)
gpsData_ptr->navValid = 0; // Light the GPS LOCK in Ground Station
gpsData_ptr->newData = 1; // Execute measurement update
}
else gpsData_ptr->navValid = 1; // Turn off the GPS LOCK light in Ground station
} // Also, no measurement update will occur since newData is not set to 1
else gpsData_ptr->navValid = 1; // Turn off the GPS LOCK light in Ground station
// Also, no measurement update will occur since newData is not set to 1
break;
}
mat_free(ecef_mat);
mat_free(lla_mat);
mat_free(ned_mat);
}
