/* public override void Reset() { oldData = new Sample<float>(TimeSpan.MinValue, 0); return; } */ public Sample<float>[] FilterData(Sample<float>[] data) { Sample<float>[] output = new Sample<float>[data.Length]; for (int i = 0; i < output.Length; i++) { Sample<float> firstValue; if (i == 0) firstValue = oldData; else firstValue = data[i - 1]; Sample<float> secondValue = data[i]; output[i] = new Sample<float>(secondValue.Time, (secondValue.Value - firstValue.Value) / (float)(secondValue.Time - firstValue.Time).TotalSeconds); } oldData = data.Last(); return output; }
//!! Need to eliminate some high frequencies? //!! Need to make real-time! //!! Does not take into account the offset! //!! Should allow cubic spline (matlab: spline() ) //!! Should start samples at the first uniform sample? public SampleSet<float> FilterData(Sample<float>[] input) { if (AutoSampleRate) { // Find minimum time between samples samplesPerSecond = GetMinimumSamplesPerSecond(input); } TimeSpan minTime = TimeSpan.FromSeconds(1.0f / samplesPerSecond); // Get new sample times float[] newSampleTimesArray = Enumerable.Range(0, int.MaxValue). Select(x => (float)(x / samplesPerSecond)). TakeWhile(x => x <= input.Last().Time.TotalSeconds). ToArray(); //float[] newSampleTimesArray = // Enumerable.Range(0, int.MaxValue). // Select(x => (float)(input.First().Time.TotalSeconds + (x / samplesPerSecond))). // TakeWhile(x => x <= input.Last().Time.TotalSeconds). // ToArray(); // Create a list of new samples SampleSet<float> output = new SampleSet<float>(TimeSpan.Zero, samplesPerSecond, newSampleTimesArray.Length); int outputCount = 0; // Add first point output.Values[0] = input.First().Value; outputCount++; // Linearly interpolate points Sample<float> lastSample = new Sample<float>(TimeSpan.Zero, 0); foreach (Sample<float> sample in input) { double m = (sample.Value - lastSample.Value) / (sample.Time - lastSample.Time).TotalSeconds; double b = lastSample.Value; double p = lastSample.Time.TotalSeconds; while (outputCount < newSampleTimesArray.Length && newSampleTimesArray[outputCount] <= sample.Time.TotalSeconds) { float newTime = newSampleTimesArray[outputCount]; // Interpolate value output.Values[outputCount] = (float)((newTime - p) * m + b); outputCount++; } lastSample = sample; } //Return filtered data return output; }