std::vector<float> CosineHcdf::getRateOfChange(const Chromagram& ch, const Parameters& params){ unsigned int hops = ch.getHops(); unsigned int bins = ch.getBins(); unsigned int gaussianSize = params.getHcdfGaussianSize(); float gaussianSigma = params.getHcdfGaussianSigma(); unsigned int padding = 0; // as opposed to gaussianSize/2 std::vector<float> cosine(hops+padding); for (unsigned int hop = 0; hop < hops; hop++){ float top = 0.0; float bottom = 0.0; for (unsigned int bin = 0; bin < bins; bin++){ float mag = ch.getMagnitude(hop, bin); top += mag; bottom += pow(mag,2); } float cos; if(bottom > 0.0) // divzero cos = top / sqrt(bottom) * sqrt(bins * sqrt(2)); else cos = 0.0; cosine[hop] = cos; } // gaussian std::vector<float> gaussian(gaussianSize); for (unsigned int i=0; i<gaussianSize; i++){ gaussian[i] = exp(-1 * (pow(i - gaussianSize/2 , 2) / (2 * gaussianSigma * gaussianSigma))); } std::vector<float> smoothed(hops); for (unsigned int hop = padding; hop < cosine.size(); hop++){ float conv = 0.0; for (unsigned int k=0; k<gaussianSize; k++){ int frm = hop - (gaussianSize/2) + k; if(frm >= 0 && frm < (signed)cosine.size()){ conv += cosine[frm] * gaussian[k]; } } smoothed[hop-padding] = conv; } // rate of change of hcdf signal; look at all hops except first. std::vector<float> rateOfChange(hops); for (unsigned int hop=1; hop<hops; hop++){ float change = (smoothed[hop] - smoothed[hop-1]) / 90.0; change = (change >= 0 ? change : change * -1.0); change = change / 0.16; // very cheeky magic number; for display purposes in KeyFinder GUI app rateOfChange[hop] = change; } // fudge first rateOfChange[0] = rateOfChange[1]; return rateOfChange; }
double RobotPosition::linInterp(double time, DataArray<DataPoint <double> >& attr) { int previous = attr.firstIndBefore(time); if (previous == 0) // If the time is after our latest information { return attr[0].value(); } else //Do linear interpolation { double last = attr[previous].value(); double slope = rateOfChange((previous-1), attr); double dT = time-attr[previous].time(); return last + (slope*dT); } //TODO add code to ensure that there are at least two values available or state as assumption //There may need to be code written for the case in which }