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