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
}
