void MadgwickAHRS::update(const float *gyro, const float *acc, const float *mag, Quaternion<float> *Q, const float dT){ chDbgCheck(true == ready, "AHRS not ready"); Vector3d<float> Gyroscope(gyro); Vector3d<float> Accelerometer(acc); Vector3d<float> Magnetometer(mag); Quaternion<float> step(1,0,0,0); Accelerometer.normalize(); Magnetometer.normalize(); Quaternion<float> tmp; Quaternion<float> qDot; Quaternion<float> qcon(1,0,0,0); /* complex conjunction to main quaternion */ //h = quatmult(q, quatmult([0 Magnetometer].', quatcon(q))); Quaternion<float> qmag(0, Magnetometer(0), Magnetometer(1), Magnetometer(2)); Q->ccon(&qcon); qmag.mul(&qcon, &tmp); Q->mul(&tmp, &h); //b = [norm([m_n m_e]) m_u 0]; //% Reference direction of Earth's G feild //d = [0 1 0]; b(0) = sqrtf(h(1)*h(1) + h(3)*h(3)); b(1) = h(2); b(2) = 0; //Gradient decent algorithm corrective step F_m(Q, &d, &Accelerometer, &Ftop); F_m(Q, &b, &Magnetometer, &Fdown); J_m(Q, &d, &Jtop); J_m(Q, &b, &Jdown); J.transpose(&JT); JT.mul(&F, &step); step.normalize(); Gyroscope -= &Wb; //Compute rate of change of quaternion //qDot_w = 0.5*quatmult(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)].'); //qDot = qDot_w - obj.Beta * step; //obj.Q_w = q_w/norm(q_w); tmp(0) = 0; tmp(1) = gyro[0]; tmp(2) = gyro[1]; tmp(3) = gyro[2]; Q->mul(&tmp, &qDot); qDot *= 0.5f; step *= *beta; qDot -= &step; //Integrate to yield quaternion //q = q + qDot*obj.dT; //obj.Q = q/norm(q); % normalise quaternion qDot *= dT; *Q += &qDot; Q->normalize(); //delta_wb = 2*obj.Zeta*obj.dT * quatmult(quatcon(q), step); Q->ccon(&qcon); qcon.mul(&step, &tmp); tmp *= *zeta * 2 * dT; //Wb = wb+delta_wb(2:4); Wb(0) = tmp(1); Wb(1) = tmp(2); Wb(2) = tmp(3); }
/** * Compute the MAn for subsets of cardinality 2 to up to p as well as * the global statistic I * * @param U pseudo-observations * @param n sample size * @param p number of random vectors * @param b vector of vector dimensions * @param m max. cardinality of subsets of {1,...,p} * @param MA0 simulated values of TA under independence (size: N * (sb - p - 1)) * @param I0 simulated values of I under independence * @param N number of repetitions (nrows MA0, fisher0, tippett0) * @param subset subsets of {1,...,p} in binary notation (int) whose card. is * between 2 and m in "natural" order * @param MA test statistics (size: sum.bin. - p - 1) * @param I global test statistic * @param pval p-values corresponding to MA (size = sum.bin. - p - 1) * @param fisher pvalue à la Fisher * @param tippett pvalue à la Tippett * @param Ipval pvalue of I * @author Ivan Kojadinovic */ void empirical_copula_test_rv(double *U, int *n, int *p, int *b, int *m, double *MA0, double *I0, int *N, int *subset, double *MA, double *I, double *pval, double *fisher, double *tippett, double *Ipval) { int i, j, k, count, sb = (int)sum_binom(*p,*m); size_t max_size = (size_t)-1,// C99 has SIZE_MAX n_ = (size_t)(*n); double J_size = ((double)n_) * n_ * (*p); if(J_size > max_size) error(_("** empirical_copula.._rv(): n and/or p too large: n^2*p = %12.0g > %12.0g = max(size_t)\n"), J_size, (double)max_size); double *fisher0 = Calloc(*N, double); double *tippett0 = Calloc(*N, double); double *J = Calloc((size_t) J_size, double); double *K = Calloc(n_ * (*p), double); double *L = Calloc(*p, double); int *R = Calloc(n_ * (*p), int); double pvalue; /* generate identity selection within the blocks */ /* used for the computation of the test statistics */ for (j=0;j<*p;j++) for (i=0;i<*n;i++) R[(*n) * j + i] = i; /* compute W à la Fisher and à la Tippett from MA0*/ for (k=0;k<*N;k++) { fisher0[k] = 0.0; tippett0[k] = 1.0; for (i=0;i<sb-*p-1;i++) { /* p-value */ count = 0; for (j=0;j<*N;j++) if (MA0[j + (*N) * i] >= MA0[k + (*N) * i]) count ++; pvalue = (double)(count + 0.5)/(*N + 1.0); fisher0[k] -= 2*log(pvalue); tippett0[k] = fmin2(tippett0[k],pvalue); } } /* compute W from the current data */ *fisher = 0.0; *tippett = 1.0; /* compute arrays J, K, L */ J_m(*n, *p, b, U, R, J); K_array(*n, *p, J, K); L_array(*n, *p, K, L); /* for subsets i of cardinality greater than 1 */ for (i=0;i<sb-*p-1;i++) { MA[i] = M_A_n(*n, *p, J, K, L, subset[i]); /* p-value */ count = 0; for (k=0;k<*N;k++) if (MA0[k + (*N) * i] >= MA[i]) count ++; pval[i] = (double)(count + 0.5)/(*N + 1.0); *fisher -= 2*log(pval[i]); *tippett = fmin2(*tippett,pval[i]); } /* p-values of the Fisher and Tippett statistics */ count = 0; for (k=0;k<*N;k++) if (fisher0[k] >= *fisher) count ++; *fisher = (double)(count + 0.5)/(*N + 1.0); count = 0; for (k=0;k<*N;k++) if (tippett0[k] <= *tippett) count ++; *tippett = (double)(count + 0.5)/(*N + 1.0); /* compute In from the current data and the corresponding pvalue*/ *I = I_n(*n, *p, J, K, L); count = 0; for (k=0;k<*N;k++) if (I0[k] >= *I) count ++; *Ipval = (double)(count + 0.5)/(*N + 1.0); Free(fisher0); Free(tippett0); Free(J); Free(K); Free(L); Free(R); }
/** * Bootstrap of the MAn, up to subsets of cardinality p, * and of In * * @param n sample size * @param N number of repetitions * @param p number of random vectors * @param b vector of vector dimensions * @param U pseudo-observations * @param m max. card. of A * @param MA0 bootstrap values of MAn under independence (N repetitions) * @param I0 bootstrap values of In under independence (N repetitions) * @param subset subsets of {1,...,p} in binary notation (int) whose card. is * between 2 and m in "natural" order * @param subset_char similar, for printing * @param verbose display progress bar if > 0 * @author Ivan Kojadinovic */ void bootstrap_MA_I(int *n, int *N, int *p, int *b, double *U, int *m, double *MA0, double *I0, int *subset, char **subset_char, int *verbose) { int i, j, k, sb[1]; size_t max_size = (size_t)-1,// C99 has SIZE_MAX n_ = (size_t)(*n); double J_size = ((double)n_) * n_ * (*p); if(J_size > max_size) error(_("** bootstrap_MA_I(): n and/or p too large: n^2*p = %12.0g > %12.0g = max(size_t)\n"), J_size, (double)max_size); int *R = Calloc(n_ * (*p), int); double *J = Calloc((size_t) J_size, double); double *K = Calloc(n_ * (*p), double); double *L = Calloc(*p, double); /* number of subsets */ *sb = (int)sum_binom(*p,*m); /* partial power set in "natural" order */ k_power_set(p, m, subset); /* convert partial power set to char for printing */ k_power_set_char(p, sb, subset, subset_char); /* Simulate the distribution of TA under independence */ GetRNGstate(); /* N repetitions */ for (k=0; k<*N; k++) { /* generate row selection within the blocks */ /* for (j=0;j<*p;j++) for (i=0;i<*n;i++) R[(*n) * j + i] = (int)( unif_rand() * (*n) ); */ /* generate row selection */ for (j=0;j<*p;j++) { for (i=0;i<*n;i++) R[i + (*n) * j] = i; /* random permutation */ for (i= *n-1; i >= 0; i--) { int t1 = R[j * (*n) + i], t2 = (int)((i + 1) * unif_rand()); R[j * (*n) + i] = R[j * (*n) + t2]; R[j * (*n) + t2] = t1; } } /* compute arrays J, K, L */ J_m(*n, *p, b, U, R, J); K_array(*n, *p, J, K); L_array(*n, *p, K, L); /* for subsets i of cardinality greater than 1 and lower than m */ for (i=*p+1;i<*sb;i++) MA0[k + (*N) * (i - *p - 1)] = M_A_n(*n, *p, J, K, L, subset[i]); /* global statistic */ I0[k] = I_n(*n, *p, J, K, L); if (*verbose) progressBar(k, *N, 70); } PutRNGstate(); Free(R); Free(J); Free(K); Free(L); }
double PhotoFarquhar::assimilate (const Units& units, const double ABA, const double psi_c, const double ec /* Canopy Vapour Pressure [Pa] */, const double gbw_ms /* Boundary layer [m/s] */, const double CO2_atm, const double O2_atm, const double Ptot /* [Pa] */, const double, const double Tc, const double Tl, const double cropN, const std::vector<double>& PAR, const std::vector<double>& PAR_height, const double PAR_LAI, const std::vector<double>& fraction, const double, CanopyStandard& canopy, Phenology& development, Treelog& msg) { const double h_x = std::fabs (psi_c) * 1.0e-4 /* [MPa/cm] */; // MPa // sugar production [gCH2O/m2/h] by canopy photosynthesis. const PLF& LAIvsH = canopy.LAIvsH; const double DS = development.DS; // One crop: daisy_assert (approximate (canopy.CAI, bioclimate.CAI ())); if (!approximate (LAIvsH (canopy.Height), canopy.CAI)) { std::ostringstream tmp; tmp << "Bug: CAI below top: " << LAIvsH (canopy.Height) << " Total CAI: " << canopy.CAI << "\n"; canopy.CanopyStructure (DS); tmp << "Adjusted: CAI below top: " << LAIvsH (canopy.Height) << " Total CAI: " << canopy.CAI; msg.error (tmp.str ()); } // CAI (total) below the current leaf layer. double prevLA = LAIvsH (PAR_height[0]); // Assimilate produced by canopy photosynthesis double Ass_ = 0.0; // Accumulated CAI, for testing purposes. double accCAI =0.0; // Number of computational intervals in the canopy. const int No = PAR.size () - 1; daisy_assert (No > 0); daisy_assert (No == PAR_height.size () - 1); // N-distribution and photosynthetical capacity std::vector<double> rubisco_Ndist (No, 0.0); std::vector<double> crop_Vm_total (No, 0.0); // Photosynthetic capacity (for logging) while (Vm_vector.size () < No) Vm_vector.push_back (0.0); // Potential electron transport rate (for logging) while (Jm_vector.size () < No) Jm_vector.push_back (0.0); // Photosynthetic N-leaf distribution (for logging) while (Nleaf_vector.size () < No) Nleaf_vector.push_back (0.0); // Brutto assimilate production (for logging) while (Ass_vector.size () < No) Ass_vector.push_back (0.0); // LAI (for logging) while (LAI_vector.size () < No) LAI_vector.push_back (0.0); rubiscoNdist->rubiscoN_distribution (units, PAR_height, prevLA, DS, rubisco_Ndist/*[mol/m²leaf]*/, cropN /*[g/m²area]*/, msg); crop_Vmax_total (rubisco_Ndist, crop_Vm_total); // Net photosynthesis (for logging) while (pn_vector.size () < No) pn_vector.push_back (0.0);// // Stomata CO2 pressure (for logging) while (ci_vector.size () < No) ci_vector.push_back (0.0);//[Pa] // Leaf surface CO2 pressure (for logging) while (cs_vector.size () < No) cs_vector.push_back (0.0);// // Leaf surface relative humidity (for logging) while (hs_vector.size () < No) hs_vector.push_back (0.0);//[] // Stomata conductance (for logging) while (gs_vector.size () < No) gs_vector.push_back (0.0);//[m/s] // Photosynthetic effect of Xylem ABA and crown water potential. Gamma = Arrhenius (Gamma25, Ea_Gamma, Tl); // [Pa] const double estar = FAO::SaturationVapourPressure (Tl); // [Pa] daisy_assert (gbw_ms >= 0.0); gbw = Resistance::ms2molly (Tl, Ptot, gbw_ms); daisy_assert (gbw >= 0.0); // CAI in each interval. const double dCAI = PAR_LAI / No; for (int i = 0; i < No; i++) { const double height = PAR_height[i+1]; daisy_assert (height < PAR_height[i]); // Leaf Area index for a given leaf layer const double LA = prevLA - LAIvsH (height); daisy_assert (LA >= 0.0); prevLA = LAIvsH (height); accCAI += LA; if (LA * fraction [i] > 0) { // PAR in mol/m2/s = PAR in W/m2 * 0.0000046 const double dPAR = (PAR[i] - PAR[i+1])/dCAI * 0.0000046; //W/m2->mol/m²leaf/s if (dPAR < 0) { std::stringstream tmp; tmp << "Negative dPAR (" << dPAR << " [mol/m^2 leaf/h])" << " PAR[" << i << "] = " << PAR[i] << " PAR[" << i+1 << "] = " << PAR[i+1] << " dCAI = " << dCAI << " LA = " << LA << " fraction[" << i << "] = " << fraction [i]; msg.debug (tmp.str ()); continue; } // log variable PAR_ += dPAR * dCAI * 3600.0; //mol/m²area/h/fraction // Photosynthetic rubisco capacity const double vmax25 = crop_Vm_total[i]*fraction[i];//[mol/m²leaf/s/fracti.] daisy_assert (vmax25 >= 0.0); // leaf respiration const double rd = respiration_rate(vmax25, Tl); daisy_assert (rd >= 0.0); //solving photosynthesis and stomatacondctance model for each layer double& pn = pn_vector[i]; double ci = 0.5 * CO2_atm;//first guess for ci, [Pa] double& hs = hs_vector[i]; hs = 0.5; // first guess of hs [] double& cs = cs_vector[i]; //first gues for stomatal cond,[mol/s/m²leaf] double gsw = Stomatacon->minimum () * 2.0; // double gsw = 2 * b; // old value const int maxiter = 150; int iter = 0; double lastci; double lasths; double lastgs; do { lastci = ci; //Stomata CO2 pressure lasths = hs; lastgs = gs; //Calculating ci and "net"photosynthesis CxModel(CO2_atm, O2_atm, Ptot, pn, ci, dPAR /*[mol/m²leaf/s]*/, gsw, gbw, Tl, vmax25, rd, msg);//[mol/m²leaf/s/fraction] // Vapour pressure at leaf surface. [Pa] const double es = (gsw * estar + gbw * ec) / (gsw + gbw); // Vapour defecit at leaf surface. [Pa] const double Ds = bound (0.0, estar - es, estar); // Relative humidity at leaf surface. [] hs = es / estar; const double hs_use = bound (0.0, hs, 1.0); // Boundary layer resistance. [s*m2 leaf/mol] daisy_assert (gbw >0.0); const double rbw = 1./gbw; //[s*m2 leaf/mol] // leaf surface CO2 [Pa] // We really should use CO2_canopy instead of CO2_atm // below. Adding the resitence from canopy point to // atmostphere is not a good workaround, as it will // ignore sources such as the soil and stored CO2 from // night respiration. cs = CO2_atm - (1.4 * pn * Ptot * rbw); //[Pa] daisy_assert (cs > 0.0); //stomatal conductance gsw = Stomatacon->stomata_con (ABA /*g/cm^3*/, h_x /* MPa */, hs_use /*[]*/, pn /*[mol/m²leaf/s]*/, Ptot /*[Pa]*/, cs /*[Pa]*/, Gamma /*[Pa]*/, Ds/*[Pa]*/, msg); //[mol/m²leaf/s] iter++; if(iter > maxiter) { std::ostringstream tmp; tmp << "total iterations in assimilation model exceed " << maxiter; msg.warning (tmp.str ()); break; } } // while (std::fabs (lastci-ci)> 0.01); while (std::fabs (lastci-ci)> 0.01 || std::fabs (lasths-hs)> 0.01 || std::fabs (lastgs-gs)> 0.01); // Leaf brutto photosynthesis [gCO2/m2/h] /*const*/ double pn_ = (pn+rd) * molWeightCO2 * 3600.0;//mol CO2/m²leaf/s->g CO2/m²leaf/h const double rd_ = (rd) * molWeightCO2 * 3600.0; //mol CO2/m²/s->g CO2/m²/h const double Vm_ = V_m(vmax25, Tl); //[mol/m² leaf/s/fraction] const double Jm_ = J_m(vmax25, Tl); //[mol/m² leaf/s/fraction] if (pn_ < 0.0) { std::stringstream tmp; tmp << "Negative brutto photosynthesis (" << pn_ << " [g CO2/m²leaf/h])" << " pn " << pn << " rd " << rd << " CO2_atm " << CO2_atm << " O2_atm " << O2_atm << " Ptot " << Ptot << " pn " << pn << " ci " << ci << " dPAR " << dPAR << " gsw " << gsw << " gbw " << gbw << " Tl " << Tl << " vmax25 " << vmax25 << " rd " << rd; msg.error (tmp.str ()); pn_ = 0.0; } Ass_ += LA * pn_; // [g/m²area/h] Res += LA * rd_; // [g/m²area/h] daisy_assert (Ass_ >= 0.0); //log variables: Ass_vector[i]+= pn_* (molWeightCH2O / molWeightCO2) * LA;//[g CH2O/m²area/h] Nleaf_vector[i]+= rubisco_Ndist[i] * LA * fraction[i]; //[mol N/m²area]OK gs_vector[i]+= gsw /* * LA * fraction[i] */; //[mol/m² area/s] ci_vector[i]+= ci /* * fraction[i] */; //[Pa] OK Vm_vector[i]+= Vm_ * 1000.0 * LA * fraction[i]; //[mmol/m² area/s]OK Jm_vector[i]+= Jm_ * 1000.0 * LA * fraction[i]; //[mmol/m² area/s]OK LAI_vector[i] += LA * fraction[i];//OK ci_middel += ci * fraction[i]/(No + 0.0);// [Pa] OK gs += LA * gsw * fraction[i]; Ass += LA * pn_ * (molWeightCH2O / molWeightCO2);//[g CH2O/m2 area/h] OK LAI += LA * fraction[i];//OK Vmax += 1000.0 * LA * fraction[i] * Vm_; //[mmol/m² area/s] jm += 1000.0 * LA * fraction[i] * Jm_; //[mmol/m² area/s] leafPhotN += rubisco_Ndist[i] * LA *fraction[i]; //[mol N/m²area]; fraction_total += fraction[i]/(No + 0.0); } } daisy_assert (approximate (accCAI, canopy.CAI)); daisy_assert (Ass_ >= 0.0); // Omregning af gs(mol/(m2s)) til gs_ms (m/s) foretages ved // gs_ms = gs * (R * T)/P: gs_ms = Resistance::molly2ms (Tl, Ptot, gs); return (molWeightCH2O / molWeightCO2)* Ass_; // Assimilate [g CH2O/m2/h] }