/* * Penalizes total jerk. */ double total_jerk_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose) { double cost = 0.0; double* s = traj->s_coeffs; double t = traj->T; double s_dot[5]; differentiate(s_dot, s, 5); double s_d_dot[4]; differentiate(s_d_dot, s_dot, 4); double jerk[3]; differentiate(jerk, s_d_dot, 3); double total_jerk = 0.0; double dt = T / 100.0; double j = 0.0; for (uint8_t i = 0; i < 100; i++) { t = dt * i; j = calculate(jerk, t, 3); total_jerk += abs(j*dt); } double jerk_per_second = total_jerk / T; cost = logistic(jerk_per_second / EXPECTED_JERK_IN_ONE_SEC ); if (verbose == true) { printf("[COST][INFO] %24s : %f\n", __func__, cost); } return cost; }
/* * Penalizes max jerk. */ double max_jerk_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose) { double cost = 0.0; double* s = traj->s_coeffs; double t = traj->T; double s_dot[5]; differentiate(s_dot, s, 5); double s_d_dot[4]; differentiate(s_d_dot, s_dot, 4); double jerk[3]; differentiate(jerk, s_d_dot, 3); std::vector<double> all_jerks; for (uint8_t i = 0; i < 100; i++) { all_jerks.push_back(calculate(jerk, T/100 * i, 4)); } double max_jerk = *std::max_element(all_jerks.begin(), all_jerks.end()); if (abs(max_jerk) > MAX_JERK) { cost = 1.0; } else { cost = logistic(MAX_JERK/(MAX_JERK-abs(max_jerk))); } if (verbose == true) { printf("[COST][INFO] %24s : %f\n", __func__, cost); } return cost; }
void BirdEye::updateFlightStatus() { double roll,pitch,yaw; try{ listener->lookupTransform("/world", vicon, ros::Time(0), *transform); } catch (tf::TransformException ex){ ROS_ERROR("%s",ex.what()); } //get angles in ZYX tf::Matrix3x3(transform->getRotation()).getEulerYPR(yaw,pitch,roll); //transform from euler zyx to euler zxy transformZYXtoZXY(yaw,pitch,roll); //getting positions x,y,z currentPoint.x = transform->getOrigin().x()*1000; currentPoint.y = transform->getOrigin().y()*1000; currentPoint.z = transform->getOrigin().z()*1000; flightLog(); //calculate velocity using one step differentiate, can be improved by moving average method. viconLostHandle(); currentVel = differentiate(pointLog); velLog.push_back(currentVel); currentAcc = differentiate(velLog); accLog.push_back(currentAcc); //update target position targetPosClient.call(srv_getTargetPos); targetVelClient.call(srv_getTargetVel); targetPos.x = srv_getTargetPos.response.x; targetPos.y = srv_getTargetPos.response.y; targetPos.z = srv_getTargetPos.response.z; targetVel.x = srv_getTargetVel.response.x; targetVel.y = srv_getTargetVel.response.y; targetVel.z = srv_getTargetVel.response.z; ROS_DEBUG("eye target %.3f %.3f %.3f", targetVel.x, targetVel.y, targetVel.z); //targetPos = zeroVector; //targetVel = zeroVector; targetLog.push_back(targetPos); }
int main (void) { int poly_degree=0; while (poly_degree>-1)//while polynomial size is greater than -1 { //set the size of the polynomial printf("Please enter the maximum degree of the polynomial: "); scanf("%d", &poly_degree); if (poly_degree>-1) { size=poly_degree; //set up the memory for the values in coefficients coefficients=new int [size+1]; signage=new char[size+1]; //now input the coefficient input_coefficients (); //print the polynomial output_polynomial (POLY); //differentiate differentiate(); //print the diferential output_polynomial (DIFF); //delete assigned memory delete [] coefficients; delete [] signage; } } return 0; }
Polinom Polinom:: operator-- (int) { Polinom temp = *this; differentiate(); return temp; }
RicoPtr Rico::differentiate(RicoPtr This, RicoPtr Wrt) const { // Translate generic RicoPtr to BasicRV id, if possible. if(!Wrt->isBasicRV()) { cout << "ERROR: Cannot differentiate with respect to non-BasicRV" << endl; return This; } return differentiate(This, id()); }
RicoPtr Rico::differentiate_single(RicoPtr This) const { set<RicoID> rv_set; collectBasicRVs(rv_set); if(rv_set.size() != 1) cout << "WARNING: Rico::differentiate_single found " << rv_set.size() << " variables" << endl; set<RicoID>::iterator ii = rv_set.begin(); // choose first one only! Ignore others. return differentiate(This, *ii); }
const vRicoPtr Rico::differentiate(RicoPtr This) const { set<RicoID> rv_set; collectBasicRVs(rv_set); vRicoPtr drv; for(set<RicoID>::iterator ii = rv_set.begin(); ii != rv_set.end(); ++ii) { drv.push_back(differentiate(This, *ii)); } return drv; }
/* * Tests for keratinocyte_differentiate() * */ void test_differentiate_stem_z() { //initialise(); char_array * id_temp; id_temp = init_char_array(); add_char(id_temp, '1'); add_keratinocyte_agent(id_temp, K_TYPE_STEM, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); current_xmachine = *p_xmachine; differentiate(); CU_ASSERT_EQUAL(get_type(), K_TYPE_TA); }
void audio2midi::execute(AudioSampleBuffer* in, MidiBuffer* out, int SR, float threshold, float offtreshold, int* nummessagessent, int mindt, float keyscaling) { buffersize = in->getNumSamples(); incopy = *in; //peeks = *in; //rmsbuf = *in; //diff1 = *in; //diff2 = *in; peeks.setSize(1, buffersize, false, true, false); rmsbuf.setSize(1, buffersize, false, true, false); diff1.setSize(1, buffersize, false, true, false); diff2.setSize(1, buffersize, false, true, false); peeks.clear(); rmsbuf.clear(); diff1.clear(); diff2.clear(); bandpassfilter::setup(4, SR, note); bandpassfilter::process(buffersize, incopy.getArrayOfChannels()); incopy.applyGain(2); if (vorige.getNumSamples() == buffersize) { //onset.peeks(&buffer, &peeks, &vorige); rms(&incopy, &rmsbuf, &vorige, 10); differentiate(&rmsbuf, &diff1, &vorige); differentiate(&diff1, &diff2, &vorige); difftomidi(&diff2, &incopy, out, note, 1 , threshold, offtreshold, nummessagessent, mindt, keyscaling); } vorige = incopy; }
/* * Penalizes exceeds speed limit. */ double exceeds_speed_limit_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose) { double cost = 0.0; double* s = traj->s_coeffs; double t = traj->T; double s_dot[5]; differentiate(s_dot, s, 5); double speed = calculate(s_dot, t, 5); if (speed > SPEED_LIMIT/2.24) { cost = 1.0 + logistic((speed-SPEED_LIMIT/2.24)/(SPEED_LIMIT/2.24));; } else { cost = logistic((SPEED_LIMIT/2.24-speed)/(SPEED_LIMIT/2.24)); } if (verbose == true) { printf("[COST][INFO] %24s : %f\n", __func__, cost); } return cost; }
int main() { struct node *p1,*p2,*p3,*p4,*p5,*p6; int x,ch; do { printf("\n\t\t\t---------Polynomial----------\n"); printf("\n\n\t1.Create\n\t2.Add\n\t3.Subtract\n\t4.Multiply\n\t5.Differentiate\n\t6.Exit"); printf("\nEnter choice : "); scanf("%d",&ch); switch(ch) { case 1: p1=(struct node*) malloc(sizeof(struct node)); p2=(struct node*) malloc(sizeof(struct node)); p1=NULL; p2=NULL; p1=create(p1); x=count(p1); sort(p1,x); p2=create(p2); x=count(p2); sort(p2,x); display(p1); display(p2); break; case 2: p3=add(p1,p2); display(p1); display(p2); printf("\nSum is : \n"); display(p3); break; case 3: p3=subtract(p1,p2); display(p1); display(p2); printf("\nDifference is : \n"); display(p3); break; case 4: p3=multiply(p1,p2); display(p1); display(p2); printf("\nProduct is : \n"); display(p3); break; case 5: p3=differentiate(p1); display(p1); printf("\nDifferential is : \n"); display(p3); p3=differentiate(p2); display(p2); printf("\nDifferential is : \n"); display(p3); break; case 6: return 0; } }while(5); }
int main(int argc, char **argv) { cwp_String mode; /* display: real, imag, amp, arg */ int imode=AMP; /* integer abbrev. for mode in switch */ register complex *ct; /* complex trace */ int nt; /* number of points on input trace */ float dt; /* sample spacing */ float *data; /* array of data from each trace */ float *hdata; /* array of Hilbert transformed data */ float unwrap; /* PI/unwrap=min dphase assumed to by wrap*/ int wint; /* n time sampling to window */ cwp_Bool seismic; /* is this seismic data? */ int ntout; /* Initialize */ initargs(argc, argv); requestdoc(1); /* Get info from first trace */ if (!gettr(&tr)) err("can't get first trace"); nt = tr.ns; dt = ((double) tr.dt)/1000000.0; ntout = nt + nt -1; /* check to see if data type is seismic */ seismic = ISSEISMIC(tr.trid); if (!seismic) warn("input is not seismic data, trid=%d", tr.trid); /* Get mode; note that imode is initialized to AMP */ if (!getparstring("mode", &mode)) mode = "amp"; if (STREQ(mode, "phase")) imode = ARG; else if (STREQ(mode, "freq")) imode = FREQ; else if (STREQ(mode, "bandwidth")) imode = BANDWIDTH; else if (STREQ(mode, "normamp")) imode = NORMAMP; else if (STREQ(mode, "freqw")) imode = FREQW; else if (STREQ(mode, "thin")) imode = THIN; else if (STREQ(mode, "fdenv")) imode = FENV; else if (STREQ(mode, "sdenv")) imode = SENV; else if (STREQ(mode, "q")) imode = Q; else if (!STREQ(mode, "amp")) err("unknown mode=\"%s\", see self-doc", mode); /* getpar value of unwrap */ switch(imode) { case FREQ: if (!getparfloat("unwrap", &unwrap)) unwrap=1; break; case Q: if (!getparfloat("unwrap", &unwrap)) unwrap=1; break; case FREQW: if (!getparfloat("unwrap", &unwrap)) unwrap=1; if (!getparint("wint", &wint)) wint=3; break; case THIN: if (!getparfloat("unwrap", &unwrap)) unwrap=1; if (!getparint("wint", &wint)) wint=3; break; case ARG: if (!getparfloat("unwrap", &unwrap)) unwrap=0; break; } /* allocate space for data and hilbert transformed data, cmplx trace */ data = ealloc1float(nt); hdata = ealloc1float(nt); ct = ealloc1complex(nt); /* Loop over traces */ do { register int i; /* Get data from trace */ for (i = 0; i < nt; ++i) data[i] = tr.data[i]; /* construct quadrature trace with hilbert transform */ hilbert(nt, data, hdata); /* build the complex trace */ for (i = 0; i < nt; ++i) ct[i] = cmplx(data[i],hdata[i]); /* Form absolute value, phase, or frequency */ switch(imode) { case AMP: for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; tr.data[i] = sqrt(re*re + im*im); } /* set trace id */ tr.trid = ENVELOPE; break; case ARG: { float *phase = ealloc1float(nt); for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; if (re*re+im*im) phase[i] = atan2(im, re); else phase[i] = 0.0; } /* phase unwrapping */ /* default unwrap=0 for this mode */ if (unwrap!=0) unwrap_phase(nt, unwrap, phase); /* write phase values to tr.data */ for (i = 0; i < nt; ++i) tr.data[i] = phase[i]; /* set trace id */ tr.trid = INSTPHASE; } break; case FREQ: { float *phase = ealloc1float(nt); float fnyq = 0.5 / dt; for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; if (re*re+im*im) { phase[i] = atan2(im, re); } else { phase[i] = 0.0; } } /* unwrap the phase */ if (unwrap!=0) unwrap_phase(nt, unwrap, phase); /* compute freq(t)=dphase/dt */ differentiate(nt, 2.0*PI*dt, phase); /* correct values greater nyquist frequency */ for (i=0 ; i < nt; ++i) { if (phase[i] > fnyq) phase[i] = 2 * fnyq - phase[i]; } /* write freq(t) values to tr.data */ for (i=0 ; i < nt; ++i) tr.data[i] = phase[i]; /* set trace id */ tr.trid = INSTFREQ; } break; case FREQW: { float fnyq = 0.5 / dt; float *freqw = ealloc1float(nt); float *phase = ealloc1float(nt); float *envelop = ealloc1float(nt); float *envelop2 = ealloc1float(nt); for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; if (re*re+im*im) { phase[i] = atan2(im, re); } else { phase[i] = 0.0; } envelop[i] = sqrt(re*re + im*im); } /* unwrap the phase */ if (unwrap!=0) unwrap_phase(nt, unwrap, phase); /* compute freq(t)=dphase/dt */ differentiate(nt, 2.0*PI*dt, phase); /* correct values greater nyquist frequency */ for (i=0 ; i < nt; ++i) { if (phase[i] > fnyq) phase[i] = 2 * fnyq - phase[i]; envelop2[i]=envelop[i]*phase[i]; } twindow(nt, wint, envelop); twindow(nt, wint, envelop2); /* correct values greater nyquist frequency */ for (i=0 ; i < nt; ++i) { freqw[i] = (envelop[i] == 0.0) ? 0.0 :envelop2[i]/envelop[i]; } /* write freq(t) values to tr.data */ for (i=0 ; i < nt; ++i) tr.data[i] = freqw[i]; /* set trace id */ tr.trid = INSTFREQ; } break; case THIN: { float fnyq = 0.5 / dt; float *phase = ealloc1float(nt); float *freqw = ealloc1float(nt); float *phase2 = ealloc1float(nt); for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; if (re*re+im*im) { phase[i] = atan2(im, re); } else { phase[i] = 0.0; } } /* unwrap the phase */ if (unwrap!=0) unwrap_phase(nt, unwrap, phase); /* compute freq(t)=dphase/dt */ differentiate(nt, 2.0*PI*dt, phase); /* correct values greater nyquist frequency */ for (i=0 ; i < nt; ++i) { if (phase[i] > fnyq) phase[i] = 2 * fnyq - phase[i]; phase2[i]= 2 * fnyq - phase[i]; } /* Do windowing for Average Ins . Freq over wint*/ twindow(nt, wint, phase2); for (i=0 ; i < nt; ++i) { freqw[i] = phase[i] - phase2[i]; /* if (abs(freqw[i]) > fnyq) freqw[i] = 2 * fnyq - freqw[i]; */ /* write Thin-Bed(t) values to tr.data */ tr.data[i] = freqw[i]; } /* set trace id */ tr.trid = INSTFREQ; } break; case BANDWIDTH: { float *envelop = ealloc1float(nt); float *envelop2 = ealloc1float(nt); /* Bandwidth (Barnes 1992) |d(envelope)/dt| band =abs |--------------| |2 PI envelope | */ for (i = 0; i < nt; ++i) { float er = ct[i].r; float em = ct[i].i; envelop[i] = sqrt(er*er + em*em); envelop2[i]=sqrt(er*er + em*em); } differentiate(nt, dt, envelop); for (i = 0; i < ntout; ++i) { if (2.0*PI*envelop2[i]!=0.0) { tr.data[i] = ABS(envelop[i]/(2.0*PI*envelop2[i])); } else { tr.data[i]=0.0; } } tr.trid = ENVELOPE; } break; case NORMAMP: { float phase; float *na = ealloc1float(nt); for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; if (re*re+im*im) phase = atan2(im, re); else phase = 0.0; na[i] = cos(phase); } for (i=0 ; i < nt; ++i) tr.data[i] = na[i]; /* set trace id */ tr.trid = INSTPHASE; } break; case FENV: { float *amp = ealloc1float(nt); for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; amp[i] = sqrt(re*re + im*im); } /*conv(nt, 0, envelop, nt, 0, time, ntout, 0, ouput);*/ differentiate(nt, 2.0*PI*dt, amp); for (i=0 ; i < nt; ++i) tr.data[i] = amp[i]; /* set trace id */ tr.trid = ENVELOPE; } break; case SENV: { float *amp = ealloc1float(nt); for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; amp[i] = sqrt(re*re + im*im); } differentiate(nt, 2.0*PI*dt, amp); differentiate(nt, 2.0*PI*dt, amp); for (i=0 ; i < nt; ++i) tr.data[i] = amp[i]; /* set trace id */ tr.trid = ENVELOPE; } break; case Q: { float *envelop = ealloc1float(nt); float *envelop2 = ealloc1float(nt); float *phase = ealloc1float(nt); float fnyq = 0.5 / dt; /* Banswith (Barnes 1992) -PI Freq(t) d(envelope)/dt band = -------------------------- envelope(t) */ for (i = 0; i < nt; ++i) { float re = ct[i].r; float im = ct[i].i; envelop[i] = sqrt(re*re + im*im); envelop2[i]=sqrt(re*re + im*im); if (re*re+im*im) { phase[i] = atan2(im, re); } else { phase[i] = 0.0; } } /* get envelope diff */ differentiate(nt, dt, envelop); /* unwrap the phase */ if (unwrap!=0) unwrap_phase(nt, unwrap, phase); /* compute freq(t)=dphase/dt */ differentiate(nt, 2.0*PI*dt, phase); for (i=0 ; i < nt; ++i) { if (phase[i] > fnyq) phase[i] = 2 * fnyq - phase[i]; } for (i = 0; i < ntout; ++i) { if (envelop[i]!=0.0) tr.data[i] = -1*PI*phase[i]*envelop2[i]/envelop[i]; else tr.data[i]=0.0; } tr.trid = INSTFREQ; } break; default: err("%s: mysterious mode=\"%s\"", __LINE__, mode); } tr.d1 = dt; /* for graphics */ puttr(&tr); } while (gettr(&tr)); return(CWP_Exit()); }
Polinom& Polinom:: operator-- () { differentiate(); return *this; }
* Public test 13 (public13.c) * * Tests differentiating a polynomial. */ #include <stdio.h> #include "polynomial.h" int main() { Polynomial poly1= new(),s; add_term(&poly1, 6, 4); add_term(&poly1, -3, 3); add_term(&poly1, 3.5, 2); printf("poly1 is: "); print(poly1); printf("\n"); s= differentiate(poly1); printf("poly1' is: "); print(s); printf("\n"); clear(&poly1); clear(&s); return 0; }
surface::instance_t make(const description_t& description) { BOOST_LOG_TRIVIAL(debug) << "Make surface sor"; const auto& points = description.points; spline_t spline = make_spline(points.begin(), points.end()); derivations_t derivations; rtree_t rtree; for (std::size_t i = 0; i < spline.size(); ++i) { const spline_segment_t& segment = spline[i]; const polynomial5_t derivation = differentiate(std::get<2>(segment) * std::get<2>(segment)); const float delta = std::get<1>(segment) - std::get<0>(segment); float max = std::max ( evaluate(std::get<2>(segment), 0.0f), evaluate(std::get<2>(segment), delta) ); std::array<float, 5> roots; const auto end = solve(derivation, roots.begin()); for (auto root = roots.begin(); root != end; ++root) if (*root >= 0.0f && *root <= delta) max = std::max ({ max, evaluate(std::get<2>(segment), *root) }); const box_t box ( point_t(-max, std::get<0>(segment), -max), point_t(+max, std::get<1>(segment), +max) ); derivations.emplace_back(derivation); rtree.insert(value_t(box, i)); } /* const box_t box = transform ( description.transformation, box_t// TODO: puke => ( vector_t { geo::get<X>(rtree.bounds().min_corner()), geo::get<Y>(rtree.bounds().min_corner()), geo::get<Z>(rtree.bounds().min_corner()) }, vector_t { geo::get<X>(rtree.bounds().max_corner()), geo::get<Y>(rtree.bounds().max_corner()), geo::get<Z>(rtree.bounds().max_corner()) } ) ); BOOST_LOG_TRIVIAL(trace) << "Box: min = " << box.min_corner() << ", max = " << box.max_corner() << std::endl; const box_t box// TODO: puke => ( vector_t { geo::get<X>(rtree.bounds().min_corner()), geo::get<Y>(rtree.bounds().min_corner()), geo::get<Z>(rtree.bounds().min_corner()) }, vector_t { geo::get<X>(rtree.bounds().max_corner()), geo::get<Y>(rtree.bounds().max_corner()), geo::get<Z>(rtree.bounds().max_corner()) } ); BOOST_LOG_TRIVIAL(trace) << "Box: min = " << box.min_corner() << ", max = " << box.max_corner() << std::endl; */ model_t model ( std::move(spline), std::move(derivations), std::move(rtree), points.front()[X], points.back()[X], points.front()[Y], points.back()[Y] // box ); return std::make_shared<instance_impl_t<model_t>>(description.transformation, std::move(model)); // return boost::make_tuple // ( // primitive::make<model> // ( // description.transformation, // std::move(spline), // std::move(derivations), // std::move(rtree), // points.front()[X], // points.back()[X], // points.front()[Y], // points.back()[Y] // ), // box, // 3 * spline.size() + 2 // ); }