static double ComputeSmoothness(List <HistoryItem> history) { // for every point in [3, length-3], generate quadratic function for points [t - 3, t + 3]. // Then compute squared error for point. double errSum = 0.0d; for (int i = 3; i < history.Count - 3; i++) { var timeRange = new Range(i - 3, i + 4, 1); var relevantPoints = history.GetRange(i - 3, 7); var t = (from n in timeRange select n).ToArray(); var fitFuncX = Fitters.GeneratePolynomialFitFunction( timeRange, (from point in relevantPoints select point.Value.X).ToList(), 2); var fitFuncY = Fitters.GeneratePolynomialFitFunction( timeRange, (from point in relevantPoints select point.Value.Y).ToList(), 2); var errX = fitFuncX(i); var errY = fitFuncY(i); errSum += errX + errY; // Return summed error. } return(errSum / history.Count); }
static void ApplyLSF(string predVal) { datapoints.Enqueue(new Tuple <int, double>(dataN, double.Parse(predVal))); if (datapoints.Count > 20) { datapoints.Dequeue(); } if (datapoints.Count >= 2) { var func = Fitters.GeneratePolynomialFit( datapoints.Select(dp => (double)dp.Item1).ToArray(), datapoints.Select(dp => dp.Item2).ToArray(), 1 ); var pred = func(dataN++); var needGoUp = pred < -0; // Should the drone go upwards? var goingUp = double.Parse(predVal) > 0; // Is the drone going upwards? if (needGoUp && goingUp) { bci_client.PutEvent(new BufferEvent("shrdcontrol.prediction", 2, -1)); } else if (!needGoUp && !goingUp) { bci_client.PutEvent(new BufferEvent("shrdcontrol.prediction", -2, -1)); } else if (needGoUp && !goingUp) { // What to do when need to go up, but going down? bci_client.PutEvent(new BufferEvent("shrdcontrol.prediction", 0, -1)); } else if (!needGoUp && goingUp) { // What if need to go down, but going up? bci_client.PutEvent(new BufferEvent("shrdcontrol.prediction", 0, -1)); } } }
public Vector2D ComputeCommand(WorldState worldState, Vector2D baseCommand) { // Create a range for t values. var ts = new Range(0, Math.Min(HistoryLength, worldState.InputHistory.Count), 1); // Fit a function on input for x-axis. Func <double, double> funcX = Fitters.GeneratePolynomialFitFunction( ts, (from l in worldState.InputHistory.TakeLast(HistoryLength) select l.Value.X).ToList(), FitFunctionDegree ); // Fit a function on input fo y-axis. Func <double, double> funcY = Fitters.GeneratePolynomialFitFunction( ts, (from l in worldState.InputHistory.TakeLast(HistoryLength) select l.Value.Y).ToList(), FitFunctionDegree ); // Predict next value and return. double time = HistoryLength + 1; var ret = new Vector2D(funcX(time), funcY(time)); if (double.IsNaN(ret.X)) { ret.X = 0.0; } if (double.IsNaN(ret.Y)) { ret.Y = 0.0; } return(ret.GetNormalized()); }