void VarproFunction::computeGammaSr( const gsl_matrix *Rt, gsl_matrix *PhiTRt, gsl_vector *Sr, bool regularize_gamma ) { gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, myPhi, Rt, 0, PhiTRt); myGam->calcGammaCholesky(PhiTRt, regularize_gamma ? myReggamma : 0); gsl_matrix SrMat = gsl_matrix_view_vector(Sr, getN(), getD()).matrix; gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, myMatr, PhiTRt, 0, &SrMat); }
void Testwishart(CuTest* tc) { /*set a non-trivial location matrix*/ gsl_matrix* V = gsl_matrix_alloc(DIM, DIM); gsl_matrix_set_identity(V); gsl_matrix_scale(V, V0); for(size_t i=0; i<DIM; i++) for(size_t j=i+1; j < DIM; j++) { gsl_matrix_set(V, i, j, 1.0); gsl_matrix_set(V, j, i, 1.0); } p = mcmclib_wishart_lpdf_alloc(V, P); x = gsl_vector_alloc(DIM * DIM); gsl_matrix_view X_v = gsl_matrix_view_vector(x, DIM, DIM); gsl_matrix* X = &(X_v.matrix); gsl_matrix_set_all(X, 0.2); for(size_t i=0; i<DIM; i++) gsl_matrix_set(X, i, i, 1.0); CuAssertDblEquals(tc, -7.152627, lpdf(1.0), TOL); CuAssertDblEquals(tc, -29.549142, lpdf(0.5), TOL); CuAssertDblEquals(tc, 13.380252, lpdf(2.0), TOL); mcmclib_wishart_lpdf_free(p); gsl_vector_free(x); gsl_matrix_free(V); }
void Testiwishart(CuTest* tc) { /*set a non-trivial location matrix*/ gsl_matrix* V = gsl_matrix_alloc(DIM, DIM); gsl_matrix_set_identity(V); gsl_matrix_add_constant(V, 1.0); p = mcmclib_iwishart_lpdf_alloc(V, P); x = gsl_vector_alloc(DIM * DIM); gsl_matrix_view X_v = gsl_matrix_view_vector(x, DIM, DIM); gsl_matrix* X = &(X_v.matrix); gsl_matrix_set_identity(X); gsl_matrix_add_constant(X, 1.0); /*check for side-effects*/ double tmp = lpdf(1.0); CuAssertTrue(tc, tmp == lpdf(1.0)); CuAssertDblEquals(tc, -18.188424, lpdf(1.0), TOL); CuAssertDblEquals(tc, 49.59203, lpdf(0.5), TOL); CuAssertDblEquals(tc, -88.468878, lpdf(2.0), TOL); mcmclib_iwishart_lpdf_free(p); gsl_vector_free(x); gsl_matrix_free(V); }
int SolveSVD (double a[], double b[], double x[], int neq, int nvar) { // get A gsl_matrix_view av = gsl_matrix_view_array (a, neq, nvar); if (neq < nvar) { // M < N ... do the transposed matrix gsl_matrix *atrans = gsl_matrix_alloc (nvar, neq); gsl_matrix_transpose_memcpy (atrans, &av.matrix); gsl_matrix *v = gsl_matrix_alloc (neq, neq); gsl_vector *s = gsl_vector_alloc (neq); gsl_vector *work = gsl_vector_alloc (neq); gsl_linalg_SV_decomp (atrans, v, s, work); // x = A+ b gsl_matrix *splus = gsl_matrix_calloc (neq, neq); // compute sigma_plus for (int i = 0; i < neq; i++) { double sigma; if ((sigma = gsl_vector_get (s,i)) != 0.0) gsl_matrix_set (splus, i,i, 1.0/sigma); } gsl_linalg_matmult (atrans, splus, atrans); gsl_linalg_matmult_mod (atrans, GSL_LINALG_MOD_NONE, v, GSL_LINALG_MOD_TRANSPOSE, atrans); gsl_vector_view bv = gsl_vector_view_array (b, neq); gsl_matrix_view bmv = gsl_matrix_view_vector (&bv.vector, neq, 1); gsl_matrix *xx = gsl_matrix_alloc (nvar,1); gsl_linalg_matmult (atrans, &bmv.matrix, xx); // gsl_matrix_fprintf (stdout, xx, "%g"); for (int i = 0; i < nvar; i++) { x[i] = gsl_matrix_get(xx,i,0); } gsl_matrix_free (splus); gsl_matrix_free (xx); gsl_matrix_free (atrans); gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work); } else { // M >= N gsl_matrix *v = gsl_matrix_alloc (nvar, nvar); gsl_vector *s = gsl_vector_alloc (nvar); gsl_vector *work = gsl_vector_alloc (nvar); gsl_linalg_SV_decomp (&av.matrix, v, s, work); // x = A+ b gsl_vector_view bv = gsl_vector_view_array (b, neq); gsl_vector *xx = gsl_vector_alloc (nvar); gsl_linalg_SV_solve (&av.matrix, v, s, &bv.vector, xx); // gsl_vector_fprintf (stdout, xx, "%g"); for (int i = 0; i < nvar; i++) x[i] = gsl_vector_get (xx, i); gsl_vector_free (xx); gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work); } return 1; }
int Holling2(double t, const double y[], double ydot[], void *params){ double alpha = 0.3; // respiration double lambda = 0.65; // ecologic efficiency double hand = 0.35; // handling time double beta = 0.5; // intraspecific competition double aij = 6.0; // attack rate //double migratingPop = 0.01; int i, j,l = 0; // Hilfsvariablen double rowsum = 0; //double colsum = 0; // int test = 0; // // if(test<5) // { // printf("Richtiges Holling"); // } // test++; //-- Struktur zerlegen------------------------------------------------------------------------------------------------------------------------------- struct foodweb *nicheweb = (struct foodweb *)params; // pointer cast from (void*) to (struct foodweb*) //printf("t in Holling 2=%f\n", t); gsl_vector *network = (nicheweb->network); // Inhalt: A+linksA+Y+linksY+Massen+Trophische_Level = (Rnum+S)²+1+Y²+1+(Rnum+S)+S int S = nicheweb->S; int Y = nicheweb->Y; int Rnum = nicheweb->Rnum; //double d = nicheweb->d; int Z = nicheweb->Z; //double dij = pow(10, d); double Bmigr = gsl_vector_get(network, (Rnum+S)*(S+Rnum)+1+Y*Y+1+(Rnum+S)+S); //printf("Bmigr ist %f\n", Bmigr); double nu,mu, tau; int SpeciesNumber; tau = gsl_vector_get(nicheweb->migrPara,0); mu = gsl_vector_get(nicheweb->migrPara,1); // if((int)nu!=0) // { // printf("nu ist nicht null sondern %f\n",nu); // } nu = gsl_vector_get(nicheweb->migrPara,2); SpeciesNumber = gsl_vector_get(nicheweb->migrPara,3); double tlast = gsl_vector_get(nicheweb->migrPara,4); // if(SpeciesNumber!= 0) // { // //printf("SpeciesNumber %i\n", SpeciesNumber); // } //printf("t oben %f\n",t); //int len = (Rnum+S)*(Rnum+S)+2+Y*Y+(Rnum+S)+S; gsl_vector_view A_view = gsl_vector_subvector(network, 0, (Rnum+S)*(Rnum+S)); // Fressmatrix A als Vektor gsl_matrix_view EA_mat = gsl_matrix_view_vector(&A_view.vector, (Rnum+S), (Rnum+S)); // A als Matrix_view gsl_matrix *EAmat = &EA_mat.matrix; // A als Matrix gsl_vector_view D_view = gsl_vector_subvector(network, (Rnum+S)*(Rnum+S)+1, Y*Y); // Migrationsmatrix D als Vektor gsl_matrix_view ED_mat = gsl_matrix_view_vector(&D_view.vector, Y, Y); // D als Matrixview gsl_matrix *EDmat = &ED_mat.matrix; // D als Matrix gsl_vector_view M_vec = gsl_vector_subvector(network, ((Rnum+S)*(Rnum+S))+1+(Y*Y)+1, (Rnum+S)); // Massenvektor gsl_vector *Mvec = &M_vec.vector; //-- verändere zu dem gewünschten Zeitpunkt Migrationsmatrix if( (t > tau) && (tlast < tau)) { //printf("mu ist %f\n", gsl_vector_get(nicheweb->migrPara,1)); //printf("nu ist %f\n", nu); gsl_vector_set(nicheweb->migrPara,4,t); //printf("Setze Link für gewünschte Migration\n"); // printf("t oben %f\n",t); // printf("tlast oben %f\n",tlast); gsl_matrix_set(EDmat, nu, mu, 1.); //int m; // for(l = 0; l< Y;l++) // { // for(m=0;m<Y;m++) // { // printf("%f\t",gsl_matrix_get(EDmat,l,m)); // } // printf("\n"); // } } else { gsl_matrix_set_zero(EDmat); } // printf("\ncheckpoint Holling2 I\n"); // printf("\nS = %i\n", S); // printf("\nS + Rnum = %i\n", S+Rnum); // // printf("\nSize A_view = %i\n", (int)A_view.vector.size); // printf("\nSize D_view = %i\n", (int)D_view.vector.size); // printf("\nSize M_vec = %i\n", (int)M_vec.vector.size); // for(i=0; i<(Rnum+S)*Y; i++){ // printf("\ny = %f\n", y[i]); // } // for(i=0; i<(Rnum+S)*Y; i++){ // printf("\nydot = %f\n", ydot[i]); // } //--zusätzliche Variablen anlegen------------------------------------------------------------------------------------------------------------- double ytemp[(Rnum+S)*Y]; for(i=0; i<(Rnum+S)*Y; i++) ytemp[i] = y[i]; // temp array mit Kopie der Startwerte for(i=0; i<(Rnum+S)*Y; i++) ydot[i] = 0; // Ergebnis, in das evolve_apply schreibt gsl_vector_view yfddot_vec = gsl_vector_view_array(ydot, (Rnum+S)*Y); //Notiz: vector_view_array etc. arbeiten auf den original Daten der ihnen zugeordneten Arrays/Vektoren! gsl_vector *yfddotvec = &yfddot_vec.vector; // zum einfacheren Rechnen ydot über vector_view_array ansprechen gsl_vector_view yfd_vec = gsl_vector_view_array(ytemp, (Rnum+S)*Y); gsl_vector *yfdvec = &yfd_vec.vector; // Startwerte der Populationen //-- neue Objekte zum Rechnen anlegen-------------------------------------------------------------------------------------------------------- gsl_matrix *AFgsl = gsl_matrix_calloc(Rnum+S, Rnum+S); // matrix of foraging efforts // gsl_matrix *ADgsl = gsl_matrix_calloc(Y,Y); // matrix of migration efforts gsl_matrix *Emat = gsl_matrix_calloc(Rnum+S, Rnum+S); // gsl objects for calculations of populations gsl_vector *tvec = gsl_vector_calloc(Rnum+S); gsl_vector *rvec = gsl_vector_calloc(Rnum+S); gsl_vector *svec = gsl_vector_calloc(Rnum+S); // gsl_matrix *Dmat = gsl_matrix_calloc(Y,Y); // gsl objects for calculations of migration // gsl_vector *d1vec = gsl_vector_calloc(Y); gsl_vector *d2vec = gsl_vector_calloc(Y); gsl_vector *d3vec = gsl_vector_calloc(Y); // printf("\ncheckpoint Holling2 III\n"); //-- Einzelne Patches lösen------------------------------------------------------------------------------------------------------------ for(l=0; l<Y; l++) // start of patch solving { gsl_matrix_set_zero(AFgsl); // Objekte zum Rechnen vor jedem Patch nullen gsl_matrix_set_zero(Emat); gsl_vector_set_zero(tvec); gsl_vector_set_zero(rvec); gsl_vector_set_zero(svec); gsl_vector_view ydot_vec = gsl_vector_subvector(yfddotvec, (Rnum+S)*l, (Rnum+S)); // enthält ydot von Patch l gsl_vector *ydotvec = &ydot_vec.vector; gsl_vector_view y_vec = gsl_vector_subvector(yfdvec, (Rnum+S)*l, (Rnum+S)); // enthält Startwerte der Population in l gsl_vector *yvec = &y_vec.vector; gsl_matrix_memcpy(AFgsl, EAmat); for(i=0; i<Rnum+S; i++) { gsl_vector_view rowA = gsl_matrix_row(AFgsl,i); rowsum = gsl_blas_dasum(&rowA.vector); if(rowsum !=0 ) { for(j=0; j<Rnum+S; j++) gsl_matrix_set(AFgsl, i, j, (gsl_matrix_get(AFgsl,i,j)/rowsum)); // normiere Beute Afgsl = A(Beutelinks auf 1 normiert) = f(i,j) } } gsl_matrix_memcpy(Emat, EAmat); // Emat = A gsl_matrix_scale(Emat, aij); // Emat(i,j) = a(i,j) gsl_matrix_mul_elements(Emat, AFgsl); // Emat(i,j) = a(i,j)*f(i,j) gsl_vector_memcpy(svec, yvec); // s(i) = y(i) gsl_vector_scale(svec, hand); // s(i) = y(i)*h gsl_blas_dgemv(CblasNoTrans, 1, Emat, svec, 0, rvec); // r(i) = Sum_k h*a(i,k)*f(i,k)*y(k) gsl_vector_add_constant(rvec, 1); // r(i) = 1+Sum_k h*a(i,k)*f(i,k)*y(k) gsl_vector_memcpy(tvec, Mvec); // t(i) = masse(i)^(-0.25) gsl_vector_div(tvec, rvec); // t(i) = masse(i)^(-0.25)/(1+Sum_k h*a(i,k)*f(i,k)*y(k)) gsl_vector_mul(tvec, yvec); // t(i) = masse(i)^(-0.25)*y(i)/(1+Sum_k h*a(i,k)*f(i,k)*y(k)) gsl_blas_dgemv(CblasTrans, 1, Emat, tvec, 0, rvec); // r(i) = Sum_j a(j,i)*f(j,i)*t(j) gsl_vector_mul(rvec, yvec); // r(i) = Sum_j a(j,i)*f(j,i)*t(j)*y(i) [rvec: Praedation] gsl_blas_dgemv(CblasNoTrans, lambda, Emat, yvec, 0, ydotvec); // ydot(i) = Sum_j lambda*a(i,j)*f(i,j)*y(j) gsl_vector_mul(ydotvec, tvec); // ydot(i) = Sum_j lambda*a(i,j)*f(i,j)*y(j)*t(i) gsl_vector_memcpy(svec, Mvec); gsl_vector_scale(svec, alpha); // s(i) = alpha*masse^(-0.25) [svec=Respiration bzw. Mortalitaet] gsl_vector_memcpy(tvec, Mvec); gsl_vector_scale(tvec, beta); // t(i) = beta*masse^(-0.25) gsl_vector_mul(tvec, yvec); // t(i) = beta*y(i) gsl_vector_add(svec, tvec); // s(i) = alpha*masse^(-0.25)+beta*y(i) gsl_vector_mul(svec, yvec); // s(i) = alpha*masse^(-0.25)*y(i)+beta*y(i)*y(i) gsl_vector_add(svec, rvec); // [svec: Respiration, competition und Praedation] gsl_vector_sub(ydotvec, svec); // ydot(i) = Fressen-Respiration-Competition-Praedation for(i=0; i<Rnum; i++) gsl_vector_set(ydotvec, i, 0.0); // konstante Ressourcen }// Ende Einzelpatch, Ergebnis steht in ydotvec // printf("\ncheckpoint Holling2 IV\n"); //-- Migration lösen--------------------------------------------------------------------------------------------------------- gsl_vector *ydottest = gsl_vector_calloc(Y); double ydotmigr = gsl_vector_get(nicheweb->migrPara, 5); // int count=0,m; // for(l = 0; l< Y;l++) // { // for(m=0;m<Y;m++) // { // count += gsl_matrix_get(EDmat,l,m); // } // } // if(count!=0) // { // //printf("count %i\n",count); // //printf("t unten %f\n",t); // //printf("tau %f\n",tau); // for(l = 0; l< Y;l++) // { // for(m=0;m<Y;m++) // { // printf("%f\t",gsl_matrix_get(EDmat,l,m)); // } // printf("\n"); // } // } double max = gsl_matrix_max(EDmat); for(l = Rnum; l< Rnum+S; l++) // start of migration solving { if(l == SpeciesNumber+Rnum && max !=0 ) { //printf("max ist %f\n",max); //printf("l ist %i\n",l); // gsl_matrix_set_zero(ADgsl); // reset gsl objects for every patch // gsl_matrix_set_zero(Dmat); // gsl_vector_set_zero(d1vec); gsl_vector_set_zero(d2vec); gsl_vector_set_zero(d3vec); gsl_vector_set_zero(ydottest); // Untervektor von yfddot (enthält ydot[]) mit offset l (Rnum...Rnum+S) und Abstand zwischen den Elementen (stride) von Rnum+S. // Dies ergibt gerade die Größe einer Spezies in jedem Patch in einem Vektor gsl_vector_view dydot_vec = gsl_vector_subvector_with_stride(yfddotvec, l, (Rnum+S), Y); // ydot[] gsl_vector *dydotvec = &dydot_vec.vector; /* gsl_vector_view dy_vec = gsl_vector_subvector_with_stride(yfdvec, l, (Rnum+S), Y); // Startgrößen der Spezies pro Patch gsl_vector *dyvec = &dy_vec.vector; */ // gsl_matrix_memcpy(ADgsl, EDmat); // ADgsl = D // // if(nicheweb->M == 1) // umschalten w: patchwise (Abwanderung aus jedem Patch gleich), sonst linkwise (Abwanderung pro link gleich) // { // for(i=0; i<Y; i++) // { // gsl_vector_view colD = gsl_matrix_column(ADgsl, i); // Spalte i aus Migrationsmatrix // colsum = gsl_blas_dasum(&colD.vector); // if(colsum!=0) // { // for(j=0;j<Y;j++) // gsl_matrix_set(ADgsl,j,i,(gsl_matrix_get(ADgsl,j,i)/colsum)); // ADgsl: D mit normierten Links // } // } // } // // gsl_matrix_memcpy(Dmat, EDmat); // Dmat = D // gsl_matrix_scale(Dmat, dij); // Dmat(i,j) = d(i,j) (Migrationsstärke) // gsl_matrix_mul_elements(Dmat, ADgsl); // Dmat(i,j) = d(i,j)*xi(i,j) (skalierte und normierte Migrationsmatrix) // // gsl_vector_set_all(d1vec, 1/gsl_vector_get(Mvec, l)); // d1(i)= m(l)^0.25 // gsl_vector_mul(d1vec, dyvec); // d1(i)= m(l)^0.25*y(i) // gsl_blas_dgemv(CblasNoTrans, 1, Dmat, d1vec, 0, d2vec); // d2(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(j) // // gsl_vector_set_all(d1vec, 1); // d1(i)= 1 // gsl_blas_dgemv(CblasTrans, 1, Dmat, d1vec, 0, d3vec); // d3(i)= Sum_j d(i,j)*xi(i,j) // gsl_vector_scale(d3vec, 1/gsl_vector_get(Mvec,l)); // d3(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25 // gsl_vector_mul(d3vec, dyvec); // d3(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(i) // gsl_vector_set(d2vec,nu,Bmigr); gsl_vector_set(d3vec,mu,Bmigr); gsl_vector_add(ydottest,d2vec); gsl_vector_sub(ydottest,d3vec); //printf("d2vec ist %f\n",gsl_vector_get(d2vec,0)); //printf("d3vec ist %f\n",gsl_vector_get(d3vec,0)); //if(gsl_vector_get(ydottest,mu)!=0) //{ ydotmigr += gsl_vector_get(ydottest,nu); // printf("ydotmigr ist %f\n",ydotmigr); gsl_vector_set(nicheweb->migrPara,5,ydotmigr); // if(ydotmigr !=0) // { // printf("ydottest aufaddiert ist %f\n",ydotmigr); // printf("ydottest aufaddiert ist %f\n",gsl_vector_get(nicheweb->migrPara,5)); // } gsl_vector_add(dydotvec, d2vec); // gsl_vector_sub(dydotvec, d3vec); // Ergebnis in dydotvec (also ydot[]) = Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(j) - Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(i) } }// Patch i gewinnt das was aus allen j Patches zuwandert und verliert was von i auswandert //printf("ydot ist %f\n",gsl_vector_get(ydottest,0)); //printf("\ncheckpoint Holling2 V\n"); /* for(i=0; i<(Rnum+S)*Y; i++){ printf("\ny = %f\tydot=%f\n", y[i], ydot[i]); } */ //--check for fixed point attractor----------------------------------------------------------------------------------- if(t>7800){ gsl_vector_set(nicheweb->fixpunkte, 0, 0); gsl_vector_set(nicheweb->fixpunkte, 1, 0); gsl_vector_set(nicheweb->fixpunkte, 2, 0); int fix0 = (int)gsl_vector_get(nicheweb->fixpunkte, 0); int fix1 = (int)gsl_vector_get(nicheweb->fixpunkte, 1); int fix2 = (int)gsl_vector_get(nicheweb->fixpunkte, 2); //printf("t unten = %f\n", t); for(i=0; i<(Rnum+S)*Y; i++) { if(y[i] <= 0) { fix0++; fix1++; fix2++; } else { if((ydot[i]/y[i]<0.0001) || (ydot[i]<0.0001)) fix0++; if(ydot[i]/y[i]<0.0001) fix1++; if(ydot[i]<0.0001) fix2++; } } if(fix0==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 3, 1); if(fix1==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 4, 1); if(fix2==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 5, 1); } //--Speicher leeren----------------------------------------------------------------------------------------------------- gsl_matrix_free(Emat); // gsl_matrix_free(Dmat); gsl_matrix_free(AFgsl); // gsl_matrix_free(ADgsl); gsl_vector_free(tvec); gsl_vector_free(rvec); gsl_vector_free(svec); // gsl_vector_free(d1vec); gsl_vector_free(d2vec); gsl_vector_free(d3vec); gsl_vector_free(ydottest); // printf("\nCheckpoint Holling2 VI\n"); return GSL_SUCCESS; }
double* intraguildPred(struct foodweb nicheweb, const double y[], double* intraPred) { int i,j,l; int S = nicheweb.S; int Y = nicheweb.Y; int Rnum = nicheweb.Rnum; gsl_vector *network = nicheweb.network; // Inhalt: A+linksA+Y+linksY+Massen+Trophische_Level = (Rnum+S)²+1+Y²+1+(Rnum+S)+S double lambda = nicheweb.lambda; double aij = nicheweb.aij; double hand = nicheweb.hand; /* Massen rausholen */ gsl_vector_view A_view = gsl_vector_subvector(network, 0, (Rnum+S)*(Rnum+S)); // Fressmatrix A als Vektor gsl_matrix_view EA_mat = gsl_matrix_view_vector(&A_view.vector, (Rnum+S), (Rnum+S)); // A als Matrix_view gsl_matrix *EAmat = &EA_mat.matrix; // A als Matrix gsl_vector_view M_vec = gsl_vector_subvector(network, ((Rnum+S)*(Rnum+S))+1+(Y*Y)+1, (Rnum+S)); // Massenvektor gsl_vector *Mvec = &M_vec.vector; // massvector: M(i)=m^(-0.25) double ytemp[(Rnum+S)*Y]; // tempvector for populations and efforts for(i=0;i<(Rnum+S)*Y;i++) ytemp[i]=y[i]; /* Alles view_array */ /* Auslesen von ytemp = y[]; sind Population */ gsl_vector_view yfd_vec=gsl_vector_view_array(ytemp,(Rnum+S)*Y); gsl_vector *yfdvec=&yfd_vec.vector; // populations and efforts for later use /* Initialisierungen */ gsl_matrix *AFgsl=gsl_matrix_calloc(Rnum+S, Rnum+S); // matrix of foraging efforts gsl_matrix *Emat=gsl_matrix_calloc(Rnum+S, Rnum+S); // gsl objects for calculations of populations gsl_vector *tvec=gsl_vector_calloc(Rnum+S); gsl_vector *rvec=gsl_vector_calloc(Rnum+S); gsl_vector *svec=gsl_vector_calloc(Rnum+S); gsl_vector *intraPredTemp=gsl_vector_calloc(Rnum+S); for(l=0;l<Y;l++) // start of patch solving { /* Initialisierungen */ gsl_matrix_set_zero(AFgsl); // reset gsl objects for every patch gsl_matrix_set_zero(Emat); gsl_vector_set_zero(tvec); gsl_vector_set_zero(rvec); gsl_vector_set_zero(svec); /* Je Vektoren von (Res+S) Elementen */ /* yfdvec enthält die Population */ gsl_vector_view y_vec=gsl_vector_subvector(yfdvec,(Rnum+S)*l,(Rnum+S)); gsl_vector *yvecint=&y_vec.vector; /* Kopie von EAmat erstellen */ gsl_matrix_memcpy(AFgsl,EAmat); for(i=0;i<Rnum+S;i++) { /* Nehme i-te Zeile aus A */ gsl_vector_view tempp=gsl_matrix_row(AFgsl,i); /* Summiere Absolutwerte der Zeile */ double temp1; temp1=gsl_blas_dasum(&tempp.vector); if(temp1!=0) { /* Teile die Einträge, die nicht- Null sind durch Anzahl an nicht-Nullen in dieser Zeile*/ /* und setzte diesen Wert dann an den entsprechenden Platz */ /* Man erhält also eine prozentuale Verbindung */ for(j=0;j<Rnum+S;j++) gsl_matrix_set(AFgsl,i,j,(gsl_matrix_get(AFgsl,i,j)/temp1)); } } /* aij ist Attackrate; AFgsl ist jetzt normiert- also fij */ gsl_matrix_memcpy(Emat,EAmat); gsl_matrix_scale(Emat,aij); // Emat(i,j) = a(i,j) gsl_matrix_mul_elements(Emat,AFgsl); // Emat(i,j) = a(i,j)*f(i,j) /* hand = handling time */ /* Berechnung wie aus Paper */ gsl_vector_set(yvecint,0,0); printf("y: %f\n",gsl_vector_get(yvecint,0)); gsl_vector_memcpy(svec,yvecint); // s(i)=y(i) gsl_vector_scale(svec, hand); // s(i)=y(i)*h gsl_blas_dgemv(CblasNoTrans,1,Emat,svec,0,rvec); // r(i)=Sum_k h*a(i,k)*f(i,k)*y(k) gsl_vector_add_constant(rvec,1); // r(i)=1+Sum_k h*a(i,k)*f(i,k)*y(k) gsl_vector_memcpy(tvec,Mvec); // t(i)=masse(i)^(-0.25) gsl_vector_div(tvec,rvec); // t(i)=masse(i)^(-0.25)/(1+Sum_k h*a(i,k)*f(i,k)*y(k)) gsl_vector_mul(tvec,yvecint); // t(i)=masse(i)^(-0.25)*y(i)/(1+Sum_k h*a(i,k)*f(i,k)*y(k)) gsl_blas_dgemv(CblasNoTrans,lambda,Emat,yvecint,0,intraPredTemp); // ydot(i)=Sum_j lambda*a(i,j)*f(i,j)*y(j) gsl_vector_mul(intraPredTemp,tvec); intraPred[l] = gsl_blas_dasum(intraPredTemp); } /* Speicher befreien */ gsl_matrix_free(Emat); gsl_matrix_free(AFgsl); gsl_vector_free(tvec); gsl_vector_free(rvec); gsl_vector_free(svec); gsl_vector_free(intraPredTemp); return 0; }
int main() { srand(time(NULL)); int states = 6; int unfiltered_states = 3; gsl_matrix *state_mean = gsl_matrix_calloc(states,1); gsl_matrix_set(state_mean, 0,0, 3); gsl_matrix_set(state_mean, 1,0, 3); gsl_matrix *state_covariance = gsl_matrix_calloc(states,states); gsl_matrix *observation_mean = gsl_matrix_calloc(states,1); gsl_matrix *observation_covariance = gsl_matrix_calloc(states,states); //gsl_matrix_set(observation_covariance, 0, 0, hdop); //gsl_matrix_set(observation_covariance, 1, 1, hdop); //gsl_matrix_set(observation_covariance, 2, 2, vert_err); //gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2)); gsl_matrix *observation_transformation = gsl_matrix_calloc(states,states); gsl_matrix *estimate_mean = gsl_matrix_calloc(states,1); gsl_matrix *estimate_covariance = gsl_matrix_calloc(states,states); gsl_matrix *kalman_gain = gsl_matrix_calloc(states,states); gsl_matrix *temp21a = gsl_matrix_calloc(states,1); gsl_matrix *temp21b = gsl_matrix_calloc(states,1); gsl_matrix *temp22a = gsl_matrix_calloc(states,states); gsl_matrix *temp22b = gsl_matrix_calloc(states,states); gsl_matrix *predict = gsl_matrix_calloc(states,states); //gsl_matrix_set(predict, 0, 0, 1); //gsl_matrix_set(predict, 1, 1, 1); //gsl_matrix_set(predict, 2, 2, 1); gsl_matrix_set(predict, 3, 3, 1); gsl_matrix_set(predict, 4, 4, 1); gsl_matrix_set(predict, 5, 5, 1); //gsl_matrix_set(predict, 0, 3, timestep); //gsl_matrix_set(predict, 1, 4, timestep); //gsl_matrix_set(predict, 2, 5, timestep); gsl_matrix *control = gsl_matrix_calloc(states, unfiltered_states); //gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2)); //gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2)); //gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2)); //gsl_matrix_set(control, 3, 0, timestep); //gsl_matrix_set(control, 4, 1, timestep); //gsl_matrix_set(control, 5, 2, timestep); gsl_vector *acceleration = gsl_vector_calloc(unfiltered_states); gsl_vector *velocity = gsl_vector_calloc(unfiltered_states); gsl_vector *location = gsl_vector_calloc(unfiltered_states); gsl_vector *delta_location = gsl_vector_calloc(unfiltered_states); gsl_vector *temp_location = gsl_vector_calloc(unfiltered_states); double timestep; int hdop; int vert_err; int s; double error; for (int t = 1; t < 1000000; t++) { timestep = 1; gsl_vector_set(acceleration, 0, (rand()%101)-52); gsl_vector_set(acceleration, 1, (rand()%101)-46); gsl_vector_set(acceleration, 2, (rand()%101)-54); gsl_vector_add(velocity, acceleration); gsl_vector_add(location, velocity); gsl_vector_memcpy(delta_location, location); gsl_vector_sub(delta_location, temp_location); gsl_vector_memcpy(temp_location, location); gsl_matrix_set(predict, 0, 3, timestep); gsl_matrix_set(predict, 1, 4, timestep); gsl_matrix_set(predict, 2, 5, timestep); gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2)); gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2)); gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2)); gsl_matrix_set(control, 3, 0, timestep); gsl_matrix_set(control, 4, 1, timestep); gsl_matrix_set(control, 5, 2, timestep); hdop = 5; vert_err = 5; gsl_matrix_set(observation_covariance, 0, 0, hdop); gsl_matrix_set(observation_covariance, 1, 1, hdop); gsl_matrix_set(observation_covariance, 2, 2, vert_err); gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2)); //observation_transformation = observation_mean[k] * pseudoinverse(state_mean[k-1]) gsl_matrix_set_zero(temp21a); gsl_matrix_set(temp21a, 0, 0, gsl_vector_get(delta_location,0)); gsl_matrix_set(temp21a, 1, 0, gsl_vector_get(delta_location,1)); gsl_matrix_set(temp21a, 2, 0, gsl_vector_get(delta_location,2)); gsl_matrix_set(temp21a, 3, 0, gsl_vector_get(velocity,0)); gsl_matrix_set(temp21a, 4, 0, gsl_vector_get(velocity,1)); gsl_matrix_set(temp21a, 5, 0, gsl_vector_get(velocity,2)); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, temp21a, pseudo_inverse(state_mean), 0, observation_transformation); //observation_mean[k] = observation_transformation * state_mean[k-1] gsl_matrix_memcpy(temp21a, state_mean); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp21a, 0, observation_mean); //observation_covariance[k] = observation_transformation * state_covariance * transpose(observation_transformation) /* gsl_matrix_set_zero(temp22a); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, observation_transformation, 0, temp22a); //notice observation_transformation is transposed gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp22a, 0, observation_covariance); */ gsl_matrix_set(observation_covariance, 0, 3, timestep*3); gsl_matrix_set(observation_covariance, 3, 0, timestep*3); gsl_matrix_set(observation_covariance, 1, 4, timestep*5); gsl_matrix_set(observation_covariance, 4, 1, timestep*5); gsl_matrix_set(observation_covariance, 2, 5, timestep*6); gsl_matrix_set(observation_covariance, 5, 2, timestep*6); //estimate_mean = predict * state_mean + control * acceleration; gsl_matrix_set_zero(estimate_mean); gsl_matrix_set_zero(temp21a); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, state_mean, 0, estimate_mean); gsl_matrix_view test = gsl_matrix_view_vector(acceleration, unfiltered_states, 1); gsl_matrix * tmp = &test.matrix; gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, control, tmp, 0, temp21a); gsl_matrix_add(estimate_mean, temp21a); //estimate_covariance = predict * state_covariance * transpose(predict) + noise; gsl_matrix_set_zero(estimate_covariance); gsl_matrix_set_zero(temp22a); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, predict, 0, temp22a); //notice predict is transposed gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, temp22a, 0, estimate_covariance); gsl_matrix_add_constant(estimate_covariance, 0.1); //adxl345 noise, this is completely wrong //kalman_gain = estimate_covariance * pseudoinverse(estimate_covariance + observation_covariance); gsl_matrix_set_zero(kalman_gain); gsl_matrix_set_zero(temp22a); gsl_matrix_memcpy(temp22a, observation_covariance); gsl_matrix_add(temp22a, estimate_covariance); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, estimate_covariance, pseudo_inverse(temp22a), 0, kalman_gain); //state_mean = estimate_mean + kalman_gain * ( observation_mean - estimate_mean ); gsl_matrix_set_zero(state_mean); gsl_matrix_set_zero(temp21a); gsl_matrix_memcpy(temp21a, observation_mean); gsl_matrix_sub(temp21a, estimate_mean); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, temp21a, 0, state_mean); gsl_matrix_add(state_mean, estimate_mean); //state_covariance = estimate_covariance - kalman_gain * ( estimate_covariance ); gsl_matrix_set_zero(state_covariance); gsl_matrix_set_zero(temp22a); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, estimate_covariance, 0, temp22a); gsl_matrix_add(state_covariance, estimate_covariance); gsl_matrix_sub(state_covariance, temp22a); printf("state_mean:"); gsl_matrix_fprintf(stdout, state_mean, "%f"); //printf("state_covariance:"); //gsl_matrix_fprintf(stdout, state_covariance, "%f"); printf("observation_mean:"); gsl_matrix_fprintf(stdout, observation_mean, "%f"); //printf("observation_covariance:"); //gsl_matrix_fprintf(stdout, observation_covariance, "%f"); printf("estimate_mean:"); gsl_matrix_fprintf(stdout, estimate_mean, "%f"); //printf("estimate_covariance:"); //gsl_matrix_fprintf(stdout, estimate_covariance, "%f"); gsl_matrix_set_zero(temp21a); gsl_matrix_memcpy(temp21a, observation_mean); gsl_matrix_sub(temp21a, state_mean); gsl_matrix_div_elements(temp21a, observation_mean); gsl_matrix_mul_elements(temp21a, temp21a); for (int i = states-1; i >= 0; i--) { error += gsl_matrix_get(temp21a, i, 0); } printf("error: %f\n", error); printf("error/time: %f\n", error/t); printf("\n"); usleep(1000000); } }