自学教程：C++ sq函数代码示例

// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute// Do not reset vertical velocity using GPS as there is baro alt available to constrain driftvoid NavEKF2_core::ResetVelocity(void){    // Store the position before the reset so that we can record the reset delta    velResetNE.x = stateStruct.velocity.x;    velResetNE.y = stateStruct.velocity.y;    if (PV_AidingMode != AID_ABSOLUTE) {        stateStruct.velocity.zero();    } else {        // reset horizontal velocity states to the GPS velocity        stateStruct.velocity.x  = gpsDataNew.vel.x; // north velocity from blended accel data        stateStruct.velocity.y  = gpsDataNew.vel.y; // east velocity from blended accel data    }    for (uint8_t i=0; i<imu_buffer_length; i++) {        storedOutput[i].velocity.x = stateStruct.velocity.x;        storedOutput[i].velocity.y = stateStruct.velocity.y;    }    outputDataNew.velocity.x = stateStruct.velocity.x;    outputDataNew.velocity.y = stateStruct.velocity.y;    outputDataDelayed.velocity.x = stateStruct.velocity.x;    outputDataDelayed.velocity.y = stateStruct.velocity.y;    // Calculate the position jump due to the reset    velResetNE.x = stateStruct.velocity.x - velResetNE.x;    velResetNE.y = stateStruct.velocity.y - velResetNE.y;    // store the time of the reset    lastVelReset_ms = imuSampleTime_ms;    // reset the corresponding covariances    zeroRows(P,3,4);    zeroCols(P,3,4);    // set the variances to the measurement variance    P[4][4] = P[3][3] = sq(frontend->_gpsHorizVelNoise);}

/*  calculate a new aerodynamic_load_factor and limit nav_roll_cd to  ensure that the load factor does not take us below the sustainable  airspeed */void Plane::update_load_factor(void){    float demanded_roll = fabsf(nav_roll_cd*0.01f);    if (demanded_roll > 85) {        // limit to 85 degrees to prevent numerical errors        demanded_roll = 85;    }    aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));    if (!aparm.stall_prevention) {        // stall prevention is disabled        return;    }    if (fly_inverted()) {        // no roll limits when inverted        return;    }    float max_load_factor = smoothed_airspeed / aparm.airspeed_min;    if (max_load_factor <= 1) {        // our airspeed is below the minimum airspeed. Limit roll to        // 25 degrees        nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500);    } else if (max_load_factor < aerodynamic_load_factor) {        // the demanded nav_roll would take us past the aerodymamic        // load limit. Limit our roll to a bank angle that will keep        // the load within what the airframe can handle. We always        // allow at least 25 degrees of roll however, to ensure the        // aircraft can be maneuvered with a bad airspeed estimate. At        // 25 degrees the load factor is 1.1 (10%)        int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100;        if (roll_limit < 2500) {            roll_limit = 2500;        }        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);    }}

Csf SpinAdapted::CSFUTIL::applyTensorOp(const TensorOp& newop, int spinL){  //for (int k=0; k<newop.Szops[newop.Szops.size()-1-newop.Spin].size(); k++) {  map<Slater, double> m;  SpinQuantum sq(newop.optypes.size(), SpinSpace(newop.Spin), IrrepSpace(newop.irrep));   for (int k=0; k<newop.Szops[spinL].size(); k++) {    if (fabs(newop.Szops[spinL][k]) > 1e-10) {      std::vector<bool> occ_rep(Slater().size(), 0);      Slater s(occ_rep,1);      for (int k2=newop.opindices[k].size()-1; k2>=0; k2--) {	s.c(newop.opindices[k][k2]);      }      if (s.isempty()) {	continue;      }      map<Slater, double>::iterator itout = m.find(s);      if (itout == m.end()) {	int sign = s.alpha.getSign();	s.setSign(1);	m[s] = sign*newop.Szops[spinL][k];      }	      else	m[s] += s.alpha.getSign()*newop.Szops[spinL][k];    }  }  int irreprow = spinL/(newop.Spin+1);   int sz = -2*(spinL%(newop.Spin+1)) + newop.Spin;    Csf csf = Csf(m,sq.particleNumber, sq.totalSpin, sz, IrrepVector(sq.get_symm().getirrep(),irreprow)) ;   if (!csf.isempty() && fabs(csf.norm()) > 1e-14)    csf.normalize();    return csf;}

doubleeval_nb_dfda(   double a,   const tab_t *tab){   double retval;   double prev;   // Convenience variables.   const unsigned int *val = tab->val;   const unsigned int *num = tab->num;   size_t nobs = num[0];   double mean = num[0] * val[0];   prev = trigamma(a + val[0]);   retval = num[0] * prev;   // Iterate over the occurrences and compute the new value   // of trigamma either by the recurrence relation, or by   // a new call to 'trigamma()', whichever is faster.   for (size_t i = 1 ; i < tab->size ; i++) {      nobs += num[i];      mean += num[i] * val[i];      prev = (val[i] - val[i-1] == 1) ?         prev - 1.0 / sq(a-1 +val[i]) :         trigamma(a + val[i]);      retval += num[i] * prev;   }   mean /= nobs;   retval += nobs*(mean/(a*(a+mean)) - trigamma(a));   return retval;}

/** * Morgan SCARA Inverse Kinematics. Results in delta[]. * * See http://forums.reprap.org/read.php?185,283327 * * Maths and first version by QHARLEY. * Integrated into Marlin and slightly restructured by Joachim Cerny. */void inverse_kinematics(const float (&raw)[XYZ]) {  static float C2, S2, SK1, SK2, THETA, PSI;  float sx = raw[X_AXIS] - SCARA_OFFSET_X,  // Translate SCARA to standard X Y        sy = raw[Y_AXIS] - SCARA_OFFSET_Y;  // With scaling factor.  if (L1 == L2)    C2 = HYPOT2(sx, sy) / L1_2_2 - 1;  else    C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2);  S2 = SQRT(1 - sq(C2));  // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End  SK1 = L1 + L2 * C2;  // Rotated Arm2 gives the distance from Arm1 to Arm2  SK2 = L2 * S2;  // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow  THETA = ATAN2(SK1, SK2) - ATAN2(sx, sy);  // Angle of Arm2  PSI = ATAN2(S2, C2);  delta[A_AXIS] = DEGREES(THETA);        // theta is support arm angle  delta[B_AXIS] = DEGREES(THETA + PSI);  // equal to sub arm angle (inverted motor)  delta[C_AXIS] = raw[Z_AXIS];  /*    DEBUG_POS("SCARA IK", raw);    DEBUG_POS("SCARA IK", delta);    SERIAL_ECHOLNPAIR("  SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI);  //*/}

void main(){	int i,n,m;	float a,x0,x1,epsilon,delta,relativeerror;	clrscr();	cout<<"Enter any number: ";	cin>>a;	cout<<"Enter the value of n: ";	cin>>m;	cout<<"Enter the initial approximation: ";	cin>>x0;	cout<<"Enter the number of iterations: ";	cin>>epsilon;	cout<<"Enter the lower boundary: ";	cin>>delta;	for(i=1;i<n;i++)	{		if(fabs(dfsq(x0,m))<=delta)		{			cout<<"Slope too small!!/n";			exit(0);		}	}	x1=(float)(x0-(sq(x0,a,m)/dfsq(x0,m)));	relativeerror=fabs((x1-x0)/x1);	x0=x1;	if(relativeerror>=epsilon)	{		cout<<"Root of the given number is: "<<setpricision(6)<<x1;		exit(0);	}	cout<<"Solution does not converge in "<<n<<" iterations"<<endl;	getch();}

void rotate_m_axyz(double *mat, double angle, double x, double y, double z ) {    int i;    double m[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };    double s = sin( deg2rad(angle) );    double c = cos( deg2rad(angle) );    double mag = sqrt(sq(x) + sq(y) + sq(z));    if (mag <= 1.0e-4) { return; } // no rotation, leave mat as-is    x /= mag; y /= mag; z /= mag;    double one_c = 1.0 - c;#define M(row,col)  m[col*4+row]    M(0,0) = (one_c * sq(x)) + c;    M(0,1) = (one_c * x*y) - z*s;    M(0,2) = (one_c * z*x) + y*s;    M(1,0) = (one_c * x*y) + z*s;    M(1,1) = (one_c * sq(y)) + c;    M(1,2) = (one_c * y*z) - x*s;    M(2,0) = (one_c * z*x) - y*s;    M(2,1) = (one_c * y*z) + x*s;    M(2,2) = (one_c * sq(z)) + c;#undef M    for (i = 0; i < 4; i++) {#define A(row,col)  mat[(col<<2)+row]#define B(row,col)  m[(col<<2)+row]        double ai0=A(i,0), ai1=A(i,1), ai2=A(i,2), ai3=A(i,3);        A(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);        A(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);        A(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);        A(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);#undef A#undef B    }}

/**Contains algorithm to pixelate and image with given params*Parameters:*pixAmount: the amount to pixelate our image by*img: a pointer to our image in memory.**return 0 if no errors were encountered, 1 otherwise.*/int pixelate(Image* img, int pixAmount) {	//if we don't have a file in memory, complain.	if(img->data == NULL) {	fprintf(stderr, "Error, no file currently in memory/n");		return 1;	}	//ints that we can change without messing with our image	int numCols = img->colNum;	int numRows = img->rowNum;	//this sets the variables above to ones evenly divisible by pixAmount	while( ((numCols % pixAmount) != 0) || ((numRows % pixAmount) != 0)) {		if((numCols % pixAmount) != 0) {			numCols--;		}		if((numRows % pixAmount) != 0) {			numRows--;		}	}	//we crop our image, and lose the part that we can't easily pixelate. 	cropImage(0,  numCols, 0, numRows, img);	//set our number of pixels in our pixelated image	numCols = numCols/pixAmount;	numRows = numRows/pixAmount;	//variables to compute what each pixel should be	int pixSumR = 0;	int pixSumG = 0;	int pixSumB = 0;	// the number of pixels in one "square" of our image	int numPix = (int)sq(pixAmount);	//for loop that loops through all the rows & cols of our pix'd image	for(int j=0; j<numRows; j++) {		for(int i=0; i<numCols; i++) {			//these loops loop through each pixel of our image in memory, and change the pixSums as needed.			for(int y=0; y<pixAmount; y++) {				for(int x=0; x<pixAmount; x++) {					pixSumR+=img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].r;					pixSumG+=img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].g;					pixSumB+=img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].b;				}			}		//averages it for each color		pixSumR = (pixSumR/numPix);		pixSumG = (pixSumG/numPix);		pixSumB = (pixSumB/numPix);			//turns our image into a pixelated one with the (almost) same resolution as the original.		for(int y=0; y<pixAmount; y++) {				for(int x=0; x<pixAmount; x++) {					img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].r =pixSumR;					img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].g =pixSumG;					img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].b =pixSumB;				}			}		//reset the sums		pixSumR = 0;		pixSumG = 0;		pixSumB = 0;		}	}		return 0; }

/**computes the gaussian value for a matrix given params*Parameters:*sigma: the sigma to use in the equation.*dx: x distance from the center of the matrix*dy: y distance from the center of the matrix**/double gaussian(double sigma, int dx, int dy) {	//equation to comput the gaussian value	double g = (1.0 / (2.0 * PI * sq(sigma))) * exp( -(sq(dx) + sq(dy)) / (2 * sq(sigma)));	return g;		}

/* calculate best fit from i+.5 to j+.5.  Assume i<j (cyclically).   Return 0 and set badness and parameters (alpha, beta), if   possible. Return 1 if impossible. */static int opti_penalty(privpath_t *pp, int i, int j, opti_t *res, double opttolerance, int *convc, double *areac) {  int m = pp->curve.n;  int k, k1, k2, conv, i1;  double area, alpha, d, d1, d2;  dpoint_t p0, p1, p2, p3, pt;  double A, R, A1, A2, A3, A4;  double s, t;  /* check convexity, corner-freeness, and maximum bend < 179 degrees */  if (i==j) {  /* sanity - a full loop can never be an opticurve */    return 1;  }  k = i;  i1 = mod(i+1, m);  k1 = mod(k+1, m);  conv = convc[k1];  if (conv == 0) {    return 1;  }  d = ddist(pp->curve.vertex[i], pp->curve.vertex[i1]);  for (k=k1; k!=j; k=k1) {    k1 = mod(k+1, m);    k2 = mod(k+2, m);    if (convc[k1] != conv) {      return 1;    }    if (sign(cprod(pp->curve.vertex[i], pp->curve.vertex[i1], pp->curve.vertex[k1], pp->curve.vertex[k2])) != conv) {      return 1;    }    if (iprod1(pp->curve.vertex[i], pp->curve.vertex[i1], pp->curve.vertex[k1], pp->curve.vertex[k2]) < d * ddist(pp->curve.vertex[k1], pp->curve.vertex[k2]) * COS179) {      return 1;    }  }  /* the curve we're working in: */  p0 = pp->curve.c[mod(i,m)][2];  p1 = pp->curve.vertex[mod(i+1,m)];  p2 = pp->curve.vertex[mod(j,m)];  p3 = pp->curve.c[mod(j,m)][2];  /* determine its area */  area = areac[j] - areac[i];  area -= dpara(pp->curve.vertex[0], pp->curve.c[i][2], pp->curve.c[j][2])/2;  if (i>=j) {    area += areac[m];  }  /* find intersection o of p0p1 and p2p3. Let t,s such that o =     interval(t,p0,p1) = interval(s,p3,p2). Let A be the area of the     triangle (p0,o,p3). */  A1 = dpara(p0, p1, p2);  A2 = dpara(p0, p1, p3);  A3 = dpara(p0, p2, p3);  /* A4 = dpara(p1, p2, p3); */  A4 = A1+A3-A2;        if (A2 == A1) {  /* this should never happen */    return 1;  }  t = A3/(A3-A4);  s = A2/(A2-A1);  A = A2 * t / 2.0;    if (A == 0.0) {  /* this should never happen */    return 1;  }  R = area / A;	 /* relative area */  alpha = 2 - sqrt(4 - R / 0.3);  /* overall alpha for p0-o-p3 curve */  res->c[0] = interval(t * alpha, p0, p1);  res->c[1] = interval(s * alpha, p3, p2);  res->alpha = alpha;  res->t = t;  res->s = s;  p1 = res->c[0];  p2 = res->c[1];  /* the proposed curve is now (p0,p1,p2,p3) */  res->pen = 0;  /* calculate penalty */  /* check tangency with edges */  for (k=mod(i+1,m); k!=j; k=k1) {    k1 = mod(k+1,m);    t = tangent(p0, p1, p2, p3, pp->curve.vertex[k], pp->curve.vertex[k1]);    if (t<-.5) {      return 1;    }    pt = bezier(t, p0, p1, p2, p3);    d = ddist(pp->curve.vertex[k], pp->curve.vertex[k1]);    if (d == 0.0) {  /* this should never happen */      return 1;//.........这里部分代码省略.........

real SimpleStats::var() const {  real v = (_mean2 - sq(_mean));  return max(v, (real)0); // make sure the result is >= 0}

// Detect if we are in flight or on groundvoid NavEKF2_core::detectFlight(){    /*        If we are a fly forward type vehicle (eg plane), then in-air status can be determined through a combination of speed and height criteria.        Because of the differing certainty requirements of algorithms that need the in-flight / on-ground status we use two booleans where        onGround indicates a high certainty we are not flying and inFlight indicates a high certainty we are flying. It is possible for        both onGround and inFlight to be false if the status is uncertain, but they cannot both be true.        If we are a plane as indicated by the assume_zero_sideslip() status, then different logic is used        TODO - this logic should be moved out of the EKF and into the flight vehicle code.    */    if (assume_zero_sideslip()) {        // To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change        float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);        bool highGndSpd = false;        bool highAirSpd = false;        bool largeHgtChange = false;        // trigger at 8 m/s airspeed        if (_ahrs->airspeed_sensor_enabled()) {            const AP_Airspeed *airspeed = _ahrs->get_airspeed();            if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) {                highAirSpd = true;            }        }        // trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors        if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {            highGndSpd = true;        }        // trigger if more than 10m away from initial height        if (fabsf(baroDataDelayed.hgt) > 10.0f) {            largeHgtChange = true;        }        // Determine to a high certainty we are flying        if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) {            onGround = false;            inFlight = true;        }        // if is possible we are in flight, set the time this condition was last detected        if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) {            airborneDetectTime_ms = imuSampleTime_ms;            onGround = false;        }        // Determine if is is possible we are on the ground        if (highGndSpd || highAirSpd || largeHgtChange) {            inFlight = false;        }        // Determine to a high certainty we are not flying        // after 5 seconds of not detecting a possible flight condition or we are disarmed, we transition to on-ground mode        if(!motorsArmed || ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {            onGround = true;            inFlight = false;        }    } else {        // Non fly forward vehicle, so can only use height and motor arm status        // If the motors are armed then we could be flying and if they are not armed then we are definitely not flying        if (motorsArmed) {            onGround = false;        } else {            inFlight = false;            onGround = true;        }        // If height has increased since exiting on-ground, then we definitely are flying        if (!onGround && ((stateStruct.position.z - posDownAtTakeoff) < -1.5f)) {            inFlight = true;        }        // If rangefinder has increased since exiting on-ground, then we definitely are flying        if (!onGround && ((rngMea - rngAtStartOfFlight) > 0.5f)) {            inFlight = true;        }    }    // store current on-ground  and in-air status for next time    prevOnGround = onGround;    prevInFlight = inFlight;    // Store vehicle height and range prior to takeoff for use in post takeoff checks    if (!onGround && !prevOnGround) {        // store vertical position at start of flight to use as a reference for ground relative checks        posDownAtTakeoff = stateStruct.position.z;        // store the range finder measurement which will be used as a reference to detect when we have taken off        rngAtStartOfFlight = rngMea;    }}

void calcTargetDistanceAndHeading(geoCoordinate_t *tracker, geoCoordinate_t *target) {  int16_t dLat = tracker->lat - target->lat;  int16_t dLon = (tracker->lon - target->lon);// * lonScale;  target->distance = uint16_t(sqrt(sq(dLat) + sq(dLon)) * 1.113195f);  target->heading = uint16_t(atan2(dLon, dLat) * 572.90f);}

double LogisticNormalLogPdf::f(double mu, double sigma, double x){    return -fastlog(sigma) - fastlog(sqrt(2*M_PI)) -           sq(fastlog(x / (1 - x)) - mu) / (2 * sq(sigma)) -           fastlog(x) - fastlog(1-x);}

float cov(const vector<float>& x) {    float ssd = sum(sq(add(-mean(x), x)));    return ssd / x.size();}

double LogNormalLogPdf::df_dx(double mu, double sigma, double x){    return (mu - fastlog(x)) / (x * sq(sigma)) - 1.0 / x;}

// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to// avoid unnecessary operationsvoid NavEKF2_core::setWindMagStateLearningMode(){    // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states    bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);    if (!inhibitWindStates && setWindInhibit) {        inhibitWindStates = true;    } else if (inhibitWindStates && !setWindInhibit) {        inhibitWindStates = false;        // set states and variances        if (yawAlignComplete && useAirspeed()) {            // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading            // which assumes the vehicle has launched into the wind             Vector3f tempEuler;            stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);            float windSpeed =  sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;            stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);            stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);            // set the wind sate variances to the measurement uncertainty            for (uint8_t index=22; index<=23; index++) {                P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f));            }        } else {            // set the variances using a typical wind speed            for (uint8_t index=22; index<=23; index++) {                P[index][index] = sq(5.0f);            }        }    }    // determine if the vehicle is manoevring    if (accNavMagHoriz > 0.5f) {        manoeuvring = true;    } else {        manoeuvring = false;    }    // Determine if learning of magnetic field states has been requested by the user    uint8_t magCal = effective_magCal();    bool magCalRequested =            ((magCal == 0) && inFlight) || // when flying            ((magCal == 1) && manoeuvring)  || // when manoeuvring            ((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete            (magCal == 4); // all the time    // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,    // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)    bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);    // Inhibit the magnetic field calibration if not requested or denied    bool setMagInhibit = !magCalRequested || magCalDenied;    if (!inhibitMagStates && setMagInhibit) {        inhibitMagStates = true;    } else if (inhibitMagStates && !setMagInhibit) {        inhibitMagStates = false;        if (magFieldLearned) {            // if we have already learned the field states, then retain the learned variances            P[16][16] = earthMagFieldVar.x;            P[17][17] = earthMagFieldVar.y;            P[18][18] = earthMagFieldVar.z;            P[19][19] = bodyMagFieldVar.x;            P[20][20] = bodyMagFieldVar.y;            P[21][21] = bodyMagFieldVar.z;        } else {            // set the variances equal to the observation variances            for (uint8_t index=18; index<=21; index++) {                P[index][index] = sq(frontend->_magNoise);            }            // set the NE earth magnetic field states using the published declination            // and set the corresponding variances and covariances            alignMagStateDeclination();        }        // request a reset of the yaw and magnetic field states if not done before        if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {            magYawResetRequest = true;        }    }    // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed    // because we want it re-done for each takeoff    if (onGround) {        finalInflightYawInit = false;        finalInflightMagInit = false;    }    // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations    // if we are not using those states    if (inhibitMagStates && inhibitWindStates) {        stateIndexLim = 15;    } else if (inhibitWindStates) {        stateIndexLim = 21;    } else {        stateIndexLim = 23;    }}

void PitchTracker::run() {    for (;;) {        busy = false;        sem_wait(&m_trig);        if (error) {            continue;        }        float sum = 0.0;        for (int k = 0; k < m_buffersize; ++k) {            sum += fabs(m_input[k]);        }        float threshold = (m_audioLevel ? signal_threshold_off : signal_threshold_on);        m_audioLevel = (sum / m_buffersize >= threshold);        if ( m_audioLevel == false ) {	    if (m_freq != 0) {		m_freq = 0;		new_freq();	    }            continue;        }        memcpy(m_fftwBufferTime, m_input, m_buffersize * sizeof(*m_fftwBufferTime));        memset(m_fftwBufferTime+m_buffersize, 0, (m_fftSize - m_buffersize) * sizeof(*m_fftwBufferTime));        fftwf_execute(m_fftwPlanFFT);        for (int k = 1; k < m_fftSize/2; k++) {            m_fftwBufferFreq[k] = sq(m_fftwBufferFreq[k]) + sq(m_fftwBufferFreq[m_fftSize-k]);            m_fftwBufferFreq[m_fftSize-k] = 0.0;        }        m_fftwBufferFreq[0] = sq(m_fftwBufferFreq[0]);        m_fftwBufferFreq[m_fftSize/2] = sq(m_fftwBufferFreq[m_fftSize/2]);        fftwf_execute(m_fftwPlanIFFT);        double sumSq = 2.0 * static_cast<double>(m_fftwBufferTime[0]) / static_cast<double>(m_fftSize);        for (int k = 0; k < m_fftSize - m_buffersize; k++) {            m_fftwBufferTime[k] = m_fftwBufferTime[k+1] / static_cast<float>(m_fftSize);        }        int count = (m_buffersize + 1) / 2;        for (int k = 0; k < count; k++) {            sumSq  -= sq(m_input[m_buffersize-1-k]) + sq(m_input[k]);            // dividing by zero is very slow, so deal with it seperately            if (sumSq > 0.0) {                m_fftwBufferTime[k] *= 2.0 / sumSq;            } else {                m_fftwBufferTime[k] = 0.0;            }        }	const float thres = 0.99; // was 0.6        int maxAutocorrIndex = findsubMaximum(m_fftwBufferTime, count, thres);        float x = 0.0;        if (maxAutocorrIndex >= 0) {            parabolaTurningPoint(m_fftwBufferTime[maxAutocorrIndex-1],                                 m_fftwBufferTime[maxAutocorrIndex],                                 m_fftwBufferTime[maxAutocorrIndex+1],                                 maxAutocorrIndex+1, &x);            x = m_sampleRate / x;            if (x > 999.0) {  // precision drops above 1000 Hz                x = 0.0;            }        }	if (m_freq != x) {	    m_freq = x;	    new_freq();	}    }}

/*  update the state of the airspeed calibration - needs to be called  once a second  On an AVR2560 this costs 1.9 milliseconds per call */float Airspeed_Calibration::update(float airspeed, const Vector3f &vg){    // Perform the covariance prediction    // Q is a diagonal matrix so only need to add three terms in    // C code implementation    // P = P + Q;    P.a.x += Q0;    P.b.y += Q0;    P.c.z += Q1;        // Perform the predicted measurement using the current state estimates    // No state prediction required because states are assumed to be time    // invariant plus process noise    // Ignore vertical wind component    float TAS_pred = state.z * pythagorous3(vg.x - state.x, vg.y - state.y, vg.z);    float TAS_mea  = airspeed;        // Calculate the observation Jacobian H_TAS    float SH1 = sq(vg.y - state.y) + sq(vg.x - state.x);    if (SH1 < 0.000001f) {        // avoid division by a small number        return state.z;    }    float SH2 = 1/sqrtf(SH1);    // observation Jacobian    Vector3f H_TAS(        -(state.z*SH2*(2*vg.x - 2*state.x))/2,        -(state.z*SH2*(2*vg.y - 2*state.y))/2,        1/SH2);        // Calculate the fusion innovaton covariance assuming a TAS measurement    // noise of 1.0 m/s    // S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1]    Vector3f PH = P * H_TAS;    float S = H_TAS * PH + 1.0f;        // Calculate the Kalman gain    // [3 x 3] * [3 x 1] / [1 x 1]    Vector3f KG = PH / S;         // Update the states    state += KG*(TAS_mea - TAS_pred); // [3 x 1] + [3 x 1] * [1 x 1]        // Update the covariance matrix    Vector3f HP2 = H_TAS * P;    P -= KG.mul_rowcol(HP2);        // force symmetry on the covariance matrix - necessary due to rounding    // errors	float P12 = 0.5f * (P.a.y + P.b.x);	float P13 = 0.5f * (P.a.z + P.c.x);	float P23 = 0.5f * (P.b.z + P.c.y);	P.a.y = P.b.x = P12;	P.a.z = P.c.x = P13;	P.b.z = P.c.y = P23;    // Constrain diagonals to be non-negative - protects against rounding errors    P.a.x = max(P.a.x, 0.0f);    P.b.y = max(P.b.y, 0.0f);    P.c.z = max(P.c.z, 0.0f);    state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max);    state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max);    state.z = constrain_float(state.z, 0.5f, 1.0f);    return state.z;}

void kins_inv(bot_t* bot) {    double nx = -bot->t[0], ny =  bot->t[2];    double ox =  bot->t[8], oy = -bot->t[10];    double ax = -bot->t[4], ay =  bot->t[6],  az = -bot->t[5];    double px = bot->t[12];    double pz = bot->t[13];    double py = bot->t[14];    double th1;    if (py == 0 && px == 0) {     // point on the Z0 axis        if (ay == 0 && ax == 0) { // wrist pointing straight up/down            th1 = 0;        } else {            th1 = atan2(ay, ax);        }    } else {        th1 = atan2(py, px);    }    double c1 = cos(th1);    double s1 = sin(th1);    double t234 = atan2(c1*ax + s1*ay, -az);    double c234 = cos(t234);    double s234 = sin(t234);    // joint 3 - elbow    double tp1 = c1*px + s1*py - bot->d5*s234;    double tp2 = pz - bot->d1 + bot->d5*c234;    double c3  = (sq(tp1) + sq(tp2) - sq(bot->a3) - sq(bot->a2)) / (2*bot->a2*bot->a3);    double s3  = -sqrt(1 - sq(c3));    double th3 = atan2(s3, c3);    // joint 2 - shoulder    double num = tp2*(bot->a3*c3 + bot->a2) - bot->a3*s3*tp1;    double den = tp1*(bot->a3*c3 + bot->a2) + bot->a3*s3*tp2;    double th2 = atan2(num, den);    // joint 4 - pitch    double th4 = (t234 - th3 - th2);    // joint 5 - roll    double th5 = atan2(s1*nx - c1*ny, s1*ox - c1*oy);    char msg[5][256];    double th[] = {th1, th2, th3, th4, th5};    int i;    bot->err = 0;    for (i = 0; i < 5; i++) {#ifdef KINS_INV_IGNORE_LIMITS        if (!isnan(th[i])) {#else            if (!isnan(th[i]) && th[i] >= deg2rad(bot->j[i].min) && th[i] <= deg2rad(bot->j[i].max)) {#endif                bot->j[i].pos = rad2deg(th[i]);            } else {                bot->err |= (1 << i);            }            // pretty print results            if (isnan(th[i])) {                snprintf(msg[i], sizeof(msg[i]), "%7s ", "nan");            } else if (th[i] < deg2rad(bot->j[i].min)) {                snprintf(msg[i], sizeof(msg[i]), "%7.2fv", rad2deg(th[i]));            } else if (th[i] > deg2rad(bot->j[i].max)) {                snprintf(msg[i], sizeof(msg[i]), "%7.2f^", rad2deg(th[i]));            } else {                snprintf(msg[i], sizeof(msg[i]), "%7.2f ", rad2deg(th[i]));            }        }        snprintf(bot->msg, sizeof(bot->msg), "kin_inv(%d): %s %s %s %s %s", bot->err, msg[0], msg[1], msg[2], msg[3], msg[4]);    }#ifdef HAVE_SDLvoid cross(float th, float l) {    glLineWidth(th);    glBegin(GL_LINES);    glColor3f(1, 0, 0); glVertex3f(0, 0, 0); glVertex3f(l, 0, 0);    glColor3f(0, 1, 0); glVertex3f(0, 0, 0); glVertex3f(0, l, 0);    glColor3f(0, 0, 1); glVertex3f(0, 0, 0); glVertex3f(0, 0, l);    glEnd();    glLineWidth(1);}

static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,                                bool useAcc, float ax, float ay, float az,                                bool useMag, float mx, float my, float mz,                                bool useYaw, float yawError){    static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki    float recipNorm;    float hx, hy, bx;    float ex = 0, ey = 0, ez = 0;    float qa, qb, qc;    // Calculate general spin rate (rad/s)    float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));    // Use raw heading error (from GPS or whatever else)    if (useYaw) {        while (yawError >  M_PIf) yawError -= (2.0f * M_PIf);        while (yawError < -M_PIf) yawError += (2.0f * M_PIf);        ez += sin_approx(yawError / 2.0f);    }    // Use measured magnetic field vector    recipNorm = sq(mx) + sq(my) + sq(mz);    if (useMag && recipNorm > 0.01f) {        // Normalise magnetometer measurement        recipNorm = invSqrt(recipNorm);        mx *= recipNorm;        my *= recipNorm;        mz *= recipNorm;        // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).        // This way magnetic field will only affect heading and wont mess roll/pitch angles        // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)        // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)        hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;        hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;        bx = sqrtf(hx * hx + hy * hy);        // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)        float ez_ef = -(hy * bx);        // Rotate mag error vector back to BF and accumulate        ex += rMat[2][0] * ez_ef;        ey += rMat[2][1] * ez_ef;        ez += rMat[2][2] * ez_ef;    }    // Use measured acceleration vector    recipNorm = sq(ax) + sq(ay) + sq(az);    if (useAcc && recipNorm > 0.01f) {        // Normalise accelerometer measurement        recipNorm = invSqrt(recipNorm);        ax *= recipNorm;        ay *= recipNorm;        az *= recipNorm;        // Error is sum of cross product between estimated direction and measured direction of gravity        ex += (ay * rMat[2][2] - az * rMat[2][1]);        ey += (az * rMat[2][0] - ax * rMat[2][2]);        ez += (ax * rMat[2][1] - ay * rMat[2][0]);    }    // Compute and apply integral feedback if enabled    if(imuRuntimeConfig->dcm_ki > 0.0f) {        // Stop integrating if spinning beyond the certain limit        if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {            float dcmKiGain = imuRuntimeConfig->dcm_ki;            integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki            integralFBy += dcmKiGain * ey * dt;            integralFBz += dcmKiGain * ez * dt;        }    }    else {        integralFBx = 0.0f;    // prevent integral windup        integralFBy = 0.0f;        integralFBz = 0.0f;    }    // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster    float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();    // Apply proportional and integral feedback    gx += dcmKpGain * ex + integralFBx;    gy += dcmKpGain * ey + integralFBy;    gz += dcmKpGain * ez + integralFBz;    // Integrate rate of change of quaternion    gx *= (0.5f * dt);    gy *= (0.5f * dt);    gz *= (0.5f * dt);    qa = q0;    qb = q1;    qc = q2;    q0 += (-qb * gx - qc * gy - q3 * gz);    q1 += (qa * gx + qc * gz - q3 * gy);    q2 += (qa * gy - qb * gz + q3 * gx);    q3 += (qa * gz + qb * gy - qc * gx);//.........这里部分代码省略.........

void do_mysql_creds(std::fstream &pf, std::unordered_map<std::string, std::string> &dbinfo){	dbinfo["type"] = "mysql";	bool prompt = true, serve = false, ssl = false, sock = false;	std::string dbname, user, pass, host, server, port, sslca, sslcert, servestr, sslstr, socktest, sockstr;	while(prompt){		std::cout << "Enter the name of the database: ";		std::getline(std::cin, dbname);		dbinfo["mysql_db_name"] = dbname; 		std::cout << "Enter the username: ";		std::getline(std::cin, user);		dbinfo["mysql_user"] = user;		std::cout << "Enter the password: ";		std::getline(std::cin, pass);		dbinfo["mysql_pass"] = pass;		std::cout << "Do you use a custom socket?(y/n)";		std::getline(std::cin, socktest);		if(socktest == "y" || socktest == "yes" || socktest == "true"){			std::cout << "Enter socket: ";			std::getline(std::cin, sockstr);			dbinfo["mysql_sock"] = sockstr;			sock = true;		}		std::cout << "Is this hosted on a separate server?(y/n) ";		std::getline(std::cin, servestr);		if(servestr == "y" || servestr == "yes" || servestr == "true"){			serve = true;			dbinfo["mysql_external_server"] = "true";		}		if(serve){			std::cout << "Enter server address: ";			std::getline(std::cin, server);			dbinfo["mysql_host"] = server;			std::cout << "Enter port: ";			std::getline(std::cin, port);			dbinfo["mysql_port"] = port;			std::cout << "Do you use ssl? (y/n) ";			std::getline(std::cin, sslstr);			if(sslstr == "y" || sslstr == "yes" || sslstr == "true"){				ssl = true;			}			if(ssl){				std::cout << "CA: ";				std::getline(std::cin, sslca);				dbinfo["mysql_sslca"] = sslca;				std::cout << "Cert path: ";				std::getline(std::cin, sslcert);				dbinfo["mysql_cert_path"] = sslcert;			}		}		std::cout << "Attempting login..." << std::endl;		std::string connstr = utils::db::build_db_connstr(dbinfo, false);		try{			soci::session sq(connstr);			prompt = false;		} catch(soci::mysql_soci_error const &e){			std::cout << e.what() << std::endl;			prompt = true;		} catch(soci::soci_error const &e){			std::cout << e.what() << std::endl;			prompt = true;		}	}	std::cout << "Login OK!" << std::endl << "Storing Credentials";	pf << "type:mysql" << std::endl << "mysql_db_name:" << dbname << std::endl << "mysql_user:" << user << std::endl		<< "mysql_pass:" << pass;	if(serve){		pf << "mysql_server:" << server << std::endl << "mysql_port:" << port;		if(ssl){			pf << "mysql_sslca:" << sslca << std::endl << "mysql_cert_path:" << sslcert;		}	}	if(sock){		pf << "mysql_sock:" << sockstr << std::endl;	}}

 // Evaluation  double eval(const X::Vector& x) const {     return sq(x[0]-Real(3.))+sq(x[1]-Real(2.)); }

/* calculate distance between two points */static inline double ddist(dpoint_t p, dpoint_t q) {  return sqrt(sq(p.x-q.x)+sq(p.y-q.y));}

//.........这里部分代码省略.........            Location target_loc = rover.current_loc;            if (!pos_ignore) {                switch (packet.coordinate_frame) {                case MAV_FRAME_BODY_NED:                case MAV_FRAME_BODY_OFFSET_NED: {                    // rotate from body-frame to NE frame                    const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw();                    const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw();                    // add offset to current location                    location_offset(target_loc, ne_x, ne_y);                    }                    break;                case MAV_FRAME_LOCAL_OFFSET_NED:                    // add offset to current location                    location_offset(target_loc, packet.x, packet.y);                    break;                default:                    // MAV_FRAME_LOCAL_NED interpret as an offset from home                    target_loc = rover.ahrs.get_home();                    location_offset(target_loc, packet.x, packet.y);                    break;                }            }            float target_speed = 0.0f;            float target_yaw_cd = 0.0f;            // consume velocity and convert to target speed and heading            if (!vel_ignore) {                const float speed_max = rover.control_mode->get_speed_default();                // convert vector length into a speed                target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);                // convert vector direction to target yaw                target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;                // rotate target yaw if provided in body-frame                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {                    target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);                }            }            // consume yaw heading            if (!yaw_ignore) {                target_yaw_cd = ToDeg(packet.yaw) * 100.0f;                // rotate target yaw if provided in body-frame                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {                    target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);                }            }            // consume yaw rate            float target_turn_rate_cds = 0.0f;            if (!yaw_rate_ignore) {                target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;            }            // handling case when both velocity and either yaw or yaw-rate are provided            // by default, we consider that the rover will drive forward            float speed_dir = 1.0f;            if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {                // Note: we are using the x-axis velocity to determine direction even though                // the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED                if (is_negative(packet.vx)) {                    speed_dir = -1.0f;                }

static int adjust_vertices(privpath_t *pp) {  int m = pp->m;  int *po = pp->po;  int n = pp->len;  point_t *pt = pp->pt;  int x0 = pp->x0;  int y0 = pp->y0;  dpoint_t *ctr = NULL;      /* ctr[m] */  dpoint_t *dir = NULL;      /* dir[m] */  quadform_t *q = NULL;      /* q[m] */  double v[3];  double d;  int i, j, k, l;  dpoint_t s;  int r;  SAFE_MALLOC(ctr, m, dpoint_t);  SAFE_MALLOC(dir, m, dpoint_t);  SAFE_MALLOC(q, m, quadform_t);  r = privcurve_init(&pp->curve, m);  if (r) {    goto malloc_error;  }    /* calculate "optimal" point-slope representation for each line     segment */  for (i=0; i<m; i++) {    j = po[mod(i+1,m)];    j = mod(j-po[i],n)+po[i];    pointslope(pp, po[i], j, &ctr[i], &dir[i]);  }  /* represent each line segment as a singular quadratic form; the     distance of a point (x,y) from the line segment will be     (x,y,1)Q(x,y,1)^t, where Q=q[i]. */  for (i=0; i<m; i++) {    d = sq(dir[i].x) + sq(dir[i].y);    if (d == 0.0) {      for (j=0; j<3; j++) {	for (k=0; k<3; k++) {	  q[i][j][k] = 0;	}      }    } else {      v[0] = dir[i].y;      v[1] = -dir[i].x;      v[2] = - v[1] * ctr[i].y - v[0] * ctr[i].x;      for (l=0; l<3; l++) {	for (k=0; k<3; k++) {	  q[i][l][k] = v[l] * v[k] / d;	}      }    }  }  /* now calculate the "intersections" of consecutive segments.     Instead of using the actual intersection, we find the point     within a given unit square which minimizes the square distance to     the two lines. */  for (i=0; i<m; i++) {    quadform_t Q;    dpoint_t w;    double dx, dy;    double det;    double min, cand; /* minimum and candidate for minimum of quad. form */    double xmin, ymin;	/* coordinates of minimum */    int z;    /* let s be the vertex, in coordinates relative to x0/y0 */    s.x = pt[po[i]].x-x0;    s.y = pt[po[i]].y-y0;    /* intersect segments i-1 and i */    j = mod(i-1,m);    /* add quadratic forms */    for (l=0; l<3; l++) {      for (k=0; k<3; k++) {	Q[l][k] = q[j][l][k] + q[i][l][k];      }    }        while(1) {      /* minimize the quadratic form Q on the unit square */      /* find intersection */#ifdef HAVE_GCC_LOOP_BUG      /* work around gcc bug #12243 */      free(NULL);#endif            det = Q[0][0]*Q[1][1] - Q[0][1]*Q[1][0];      if (det != 0.0) {	w.x = (-Q[0][2]*Q[1][1] + Q[1][2]*Q[0][1]) / det;	w.y = ( Q[0][2]*Q[1][0] - Q[1][2]*Q[0][0]) / det;	break;      }//.........这里部分代码省略.........