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());
}
