/// Fits a polynomial of the given degree to the data points.
public PolynomialFit solve(int degree)
{
if (degree > x.Count)
{
// Not enough data to fit a curve.
return(null);
}
PolynomialFit result = new PolynomialFit(degree);
// Shorthands for the purpose of notation equivalence to original C++ code.
int m = x.Count;
int n = degree + 1;
// Expand the X vector to a matrix A, pre-multiplied by the weights.
_Matrix a = new _Matrix(n, m);
for (int h = 0; h < m; h += 1)
{
a[0, h] = w[h];
for (int i = 1; i < n; i += 1)
{
a[i, h] = a[i - 1, h] * x[h];
}
}
// Apply the Gram-Schmidt process to A to obtain its QR decomposition.
// Orthonormal basis, column-major ordVectorer.
_Matrix q = new _Matrix(n, m);
// Upper triangular matrix, row-major order.
_Matrix r = new _Matrix(n, n);
for (int j = 0; j < n; j += 1)
{
for (int h = 0; h < m; h += 1)
{
q[j, h] = a[j, h];
}
for (int i = 0; i < j; i += 1)
{
float dot = q.getRow(j) * q.getRow(i);
for (int h = 0; h < m; h += 1)
{
q[j, h] = q[j, h] - dot * q[i, h];
}
}
float norm = q.getRow(j).norm();
if (norm < 0.000001f)
{
// Vectors are linearly dependent or zero so no solution.
return(null);
}
float inverseNorm = 1.0f / norm;
for (int h = 0; h < m; h += 1)
{
q[j, h] = q[j, h] * inverseNorm;
}
for (int i = 0; i < n; i += 1)
{
r[j, i] = i < j ? 0.0f : q.getRow(j) * a.getRow(i);
}
}
// Solve R B = Qt W Y to find B. This is easy because R is upper triangular.
// We just work from bottom-right to top-left calculating B's coefficients.
_Vector wy = new _Vector(m);
for (int h = 0; h < m; h += 1)
{
wy[h] = y[h] * w[h];
}
for (int i = n - 1; i >= 0; i -= 1)
{
result.coefficients[i] = q.getRow(i) * wy;
for (int j = n - 1; j > i; j -= 1)
{
result.coefficients[i] -= r[i, j] * result.coefficients[j];
}
result.coefficients[i] /= r[i, i];
}
// Calculate the coefficient of determination (confidence) as:
// 1 - (sumSquaredError / sumSquaredTotal)
// ...where sumSquaredError is the residual sum of squares (variance of the
// error), and sumSquaredTotal is the total sum of squares (variance of the
// data) where each has been weighted.
float yMean = 0.0f;
for (int h = 0; h < m; h += 1)
{
yMean += y[h];
}
yMean /= m;
float sumSquaredError = 0.0f;
float sumSquaredTotal = 0.0f;
for (int h = 0; h < m; h += 1)
{
float term = 1.0f;
float err = y[h] - result.coefficients[0];
for (int i = 1; i < n; i += 1)
{
term *= x[h];
err -= term * result.coefficients[i];
}
sumSquaredError += w[h] * w[h] * err * err;
float v = y[h] - yMean;
sumSquaredTotal += w[h] * w[h] * v * v;
}
result.confidence = sumSquaredTotal <= 0.000001f ? 1.0f : 1.0f - (sumSquaredError / sumSquaredTotal);
return(result);
}
public VelocityEstimate getVelocityEstimate()
{
List <float> x = new List <float>();
List <float> y = new List <float>();
List <float> w = new List <float>();
List <float> time = new List <float>();
int sampleCount = 0;
int index = _index;
_PointAtTime newestSample = _samples[index];
if (newestSample == null)
{
return(null);
}
_PointAtTime previousSample = newestSample;
_PointAtTime oldestSample = newestSample;
do
{
_PointAtTime sample = _samples[index];
if (sample == null)
{
break;
}
float age = (float)(newestSample.time - sample.time).TotalMilliseconds;
float delta = Mathf.Abs((float)(sample.time - previousSample.time).TotalMilliseconds);
previousSample = sample;
if (age > _horizonMilliseconds ||
delta > _assumePointerMoveStoppedMilliseconds)
{
break;
}
oldestSample = sample;
Offset position = sample.point;
x.Add(position.dx);
y.Add(position.dy);
w.Add(1.0f);
time.Add(-age);
index = (index == 0 ? _historySize : index) - 1;
sampleCount += 1;
} while (sampleCount < _historySize);
if (sampleCount >= _minSampleSize)
{
LeastSquaresSolver xSolver = new LeastSquaresSolver(time, x, w);
PolynomialFit xFit = xSolver.solve(2);
if (xFit != null)
{
LeastSquaresSolver ySolver = new LeastSquaresSolver(time, y, w);
PolynomialFit yFit = ySolver.solve(2);
if (yFit != null)
{
return(new VelocityEstimate(
pixelsPerSecond: new Offset(xFit.coefficients[1] * 1000, yFit.coefficients[1] * 1000),
confidence: xFit.confidence *yFit.confidence,
duration: newestSample.time - oldestSample.time,
offset: newestSample.point - oldestSample.point
));
}
}
}
return(new VelocityEstimate(
pixelsPerSecond: Offset.zero,
confidence: 1.0f,
duration: newestSample.time - oldestSample.time,
offset: newestSample.point - oldestSample.point
));
}
