//public enum NDerivative
//{ first, second}
public Tuple <double, long> GetNDerivative(NDerivative nd)
{
for (int i = 0; i < windowSize; i++)
{
if (rawPositionData[i].Item1 == double.NaN)
{
return(null);
}
}
//averageTimeSpan
long sum = 0;
long hj = 0;
double h;
double NDerivativeVal = 0;
for (int i = 0; i < windowSize - 1; i++)
{
sum += rawPositionData[i + 1].Item2 - rawPositionData[i].Item2;
}
hj = sum / (windowSize - 1);
h = (double)hj * (1.0 / TimeSpan.TicksPerSecond);
if (windowSize == 5)
{
if (nd == NDerivative.second)
{
NDerivativeVal = (1.0 / (7.0 * h * h)) * (2.0 * rawPositionData[0].Item1 - rawPositionData[1].Item1 - 2.0 * rawPositionData[2].Item1 - rawPositionData[3].Item1 + 2.0 * rawPositionData[4].Item1);
}
else if (nd == NDerivative.first)
{
NDerivativeVal = (1.0 / (12.0 * h)) * (rawPositionData[0].Item1 - 8.0 * rawPositionData[1].Item1 + 8.0 * rawPositionData[3].Item1 - rawPositionData[4].Item1);
}
else
{
NDerivativeVal = (1.0 / 35.0) * (-3.0 * rawPositionData[0].Item1 + 12.0 * rawPositionData[1].Item1 + 17.0 * rawPositionData[2].Item1 + 12.0 * rawPositionData[3].Item1 + -3.0 * rawPositionData[4].Item1);
}
}
if (windowSize == 7)
{
if (nd == NDerivative.second)
{
NDerivativeVal = (1.0 / (42.0 * h * h)) * (5.0 * rawPositionData[0].Item1 - 3.0 * rawPositionData[2].Item1 - 4.0 * rawPositionData[3].Item1 - 3.0 * rawPositionData[4].Item1 + 5.0 * rawPositionData[6].Item1);
}
}
if (windowSize == 9)
{
if (nd == NDerivative.second)
{
NDerivativeVal = (1.0 / (462.0 * h * h)) * (28.0 * rawPositionData[0].Item1 + 7.0 * rawPositionData[1].Item1 - 8.0 * rawPositionData[2].Item1 - 17.0 * rawPositionData[3].Item1 - 20.0 * rawPositionData[4].Item1 - 17.0 * rawPositionData[5].Item1 - 8.0 * rawPositionData[6].Item1 + 7.0 * rawPositionData[7].Item1 + 20.0 * rawPositionData[8].Item1);
}
}
Tuple <double, long> NthDerivative = new Tuple <double, long>(NDerivativeVal, rawPositionData[windowSize / 2].Item2);
return(NthDerivative);
}
/// <summary>
/// The first two parameters control smoothing and the third controls how many computations of derivations with smoothing will occur
/// </summary>
/// <param name="alpha">Dependency on current/last point</param>
/// <param name="gamma">Dependency on trend</param>
/// <param name="nd">Number of derivative/smoothing computations</param>
public DoubleExponentialDeriver(double[] alpha, double[] gamma, NDerivative nd)
{
FirstDeriver = new ContiniousFilter();
SecondDeriver = new ContiniousFilter();
FirstSmoother = new DoubleExponentialFirst(alpha[0], gamma[0]);
SecondSmoother = new DoubleExponentialFirst(alpha[1], gamma[1]);
ThirdSmoother = new DoubleExponentialFirst(alpha[2], gamma[2]);
DerivativeOrder = nd;
}
/// <summary>
/// Returns smoothed data for position, velocity, and acceleration
/// </summary>
/// <param name="nd"></param>
/// <returns></returns>
public Tuple <double, long> GetNDerivative(NDerivative nd)
{
Tuple <double, long> smoothData = null;
//Derive and smooth the raw position data
switch (nd)
{
case NDerivative.smooth:
{
if (DerivativeOrder != NDerivative.smooth)
{
throw new Exception("Must initialize class to zero(smooth) derivative order.");
}
smoothData = FirstSmoother.SmoothDataPoints();
break;
}
case NDerivative.first:
{
if (DerivativeOrder != NDerivative.first)
{
throw new Exception("Must initialize class to first derivative order.");
}
smoothData = SecondSmoother.SmoothDataPoints();
break;
}
case NDerivative.second:
{
if (DerivativeOrder != NDerivative.second)
{
throw new Exception("Must initialize class to second derivative order.");
}
smoothData = ThirdSmoother.SmoothDataPoints();
//smoothData = SecondDeriver.Derive();
break;
}
}
return(smoothData);
}
/// <summary>
/// This applies the set-up filter to an array of numbers. The left and right side is special treated by
/// applying Savitzky-Golay with appropriate adjusted left and right number of points.
/// </summary>
/// <param name="array">The array of numbers to filter.</param>
/// <param name="result">The resulting array. Must not be identical to the input array!</param>
public Tuple <double, long> GetNDerivative(NDerivative nd)
{
try
{
for (int i = 0; i < windowSize; i++)
{
if (rawPositionData[i].Item1 == double.NaN)
{
return(null);
}
}
double[] middle = new double[0];
double hsum = 0, h = 0;
//average the h parameter
for (int i = 0; i < rawPositionData.Length - 1; i++)
{
hsum += rawPositionData[i + 1].Item2 - rawPositionData[i].Item2;
}
h = hsum / (rawPositionData.Length - 1);
h *= (1.0 / TimeSpan.TicksPerSecond);
switch (nd)
{
case NDerivative.smooth:
middle = _middle[0];
break;
case NDerivative.first:
middle = _middle[1];
break;
case NDerivative.second:
middle = _middle[2];
break;
}
int filterPoints = middle.Length;
int sidePoints = (filterPoints - 1) / 2;
double result = 0;
double sum = 0;
for (int i = 0; i < filterPoints; i++)
{
sum += rawPositionData[i].Item1 * middle[i];
}
result = sum;
switch (nd)
{
case NDerivative.first:
result *= 1.0 / (h);
break;
case NDerivative.second:
result *= 2.0 / (Math.Pow(h, 2.0));
break;
}
return(new Tuple <double, long>(result, rawPositionData[windowSize / 2].Item2));
}
catch (Exception e)
{
System.Diagnostics.Debug.WriteLine(e.ToString() + " In SGS");
throw e;
}
}
