public Tuple <double, double> DoDeltaCalibration(int numFactors, IList <PointError> zBedProbePoints, bool normalise)
{
if (numFactors != 3 && numFactors != 4 && numFactors != 6 && numFactors != 7)
{
throw new Exception("Error: " + numFactors + " factors requested but only 3, 4, 6 and 7 supported");
}
if (numFactors > zBedProbePoints.Count)
{
throw new Exception("Error: need at least as many points as factors you want to calibrate");
}
// Transform the probing points to motor endpoints and store them in a matrix, so that we can do multiple iterations using the same data
var probeMotorPositions = new PointError[zBedProbePoints.Count];
var corrections = new double[zBedProbePoints.Count];
var initialSumOfSquares = 0.0;
for (var i = 0; i < zBedProbePoints.Count; ++i)
{
corrections[i] = 0.0;
var machinePos = new double[3]
{
zBedProbePoints[i].X,
zBedProbePoints[i].Y,
0
};
probeMotorPositions[i] = new PointError(Transform(machinePos, 0), Transform(machinePos, 1), Transform(machinePos, 2));
initialSumOfSquares += FSquare(zBedProbePoints[i].X) + FSquare(zBedProbePoints[i].Y) + FSquare(zBedProbePoints[i].ZError);
}
// remove any erroneous data points.. maybe not the best idea??
var zip = zBedProbePoints
.Zip(probeMotorPositions, (point, pos) => new { point, pos })
.Where(_ => !_.pos.X.Equals(double.NaN) && !_.pos.Y.Equals(double.NaN) && !_.pos.ZError.Equals(double.NaN))
.ToList();
zBedProbePoints = (from z in zip select z.point).ToList();
probeMotorPositions = (from z in zip select z.pos).ToArray();
// Do 1 or more Newton-Raphson iterations
var iteration = 0;
double expectedRmsError;
for (;;)
{
// Build a Nx7 matrix of derivatives with respect to xa, xb, yc, za, zb, zc, diagonal.
var derivativeMatrix = new double[zBedProbePoints.Count, numFactors];
for (var i = 0; i < zBedProbePoints.Count; ++i)
{
for (var j = 0; j < numFactors; ++j)
{
derivativeMatrix[i, j] =
ComputeDerivative(j, probeMotorPositions[i].X, probeMotorPositions[i].Y, probeMotorPositions[i].ZError);
}
}
// Now build the normal equations for least squares fitting
var normalMatrix = new double[numFactors, numFactors + 1];
double temp;
for (var i = 0; i < numFactors; ++i)
{
for (var j = 0; j < numFactors; ++j)
{
temp = derivativeMatrix[0, i] * derivativeMatrix[0, j];
for (var k = 1; k < zBedProbePoints.Count; ++k)
{
temp += derivativeMatrix[k, i] * derivativeMatrix[k, j];
}
normalMatrix[i, j] = temp;
}
temp = derivativeMatrix[0, i] * -(zBedProbePoints[0].ZError + corrections[0]);
for (var k = 1; k < zBedProbePoints.Count; ++k)
{
temp += derivativeMatrix[k, i] * -(zBedProbePoints[k].ZError + corrections[k]);
}
normalMatrix[i, numFactors] = temp;
}
double[] solution = GaussJordan(ref normalMatrix, numFactors);
if (solution.Any(_ => _.Equals(double.NaN)))
{
throw new Exception("Unable to calculate corrections. Please make sure the bed probe points are all distinct.");
}
//if (debug)
//{
// DebugPrint(PrintVector("Solution", solution));
// // Calculate and display the residuals
// var residuals = [];
// for (var i = 0; i < numPoints; ++i)
// {
// var r = zBedProbePoints[i];
// for (var j = 0; j < numFactors; ++j)
// {
// r += solution[j] * derivativeMatrix.data[i][j];
// }
// residuals.push(r);
// }
// DebugPrint(PrintVector("Residuals", residuals));
//}
Adjust(numFactors, solution, normalise);
// Calculate the expected probe heights using the new parameters
{
var expectedResiduals = new double[zBedProbePoints.Count];
var sumOfSquares = 0.0;
for (var i = 0; i < zBedProbePoints.Count; ++i)
{
probeMotorPositions[i] = new PointError(probeMotorPositions[i].X + solution[0], probeMotorPositions[i].Y + solution[1], probeMotorPositions[i].ZError + solution[2]);
var newZ = InverseTransform(probeMotorPositions[i].X, probeMotorPositions[i].Y, probeMotorPositions[i].ZError);
corrections[i] = newZ;
expectedResiduals[i] = zBedProbePoints[i].ZError + newZ;
sumOfSquares += FSquare(expectedResiduals[i]);
}
expectedRmsError = Math.Sqrt(sumOfSquares / zBedProbePoints.Count);
}
// Decide whether to do another iteration Two is slightly better than one, but three doesn't improve things.
// Alternatively, we could stop when the expected RMS error is only slightly worse than the RMS of the residuals.
++iteration;
if (iteration == 2)
{
break;
}
}
return(Tuple.Create(Math.Sqrt(initialSumOfSquares / zBedProbePoints.Count), expectedRmsError));
}
public Tuple<double, double> DoDeltaCalibration(int numFactors, IList<PointError> zBedProbePoints, bool normalise)
{
if (numFactors != 3 && numFactors != 4 && numFactors != 6 && numFactors != 7)
{
throw new Exception("Error: " + numFactors + " factors requested but only 3, 4, 6 and 7 supported");
}
if (numFactors > zBedProbePoints.Count)
{
throw new Exception("Error: need at least as many points as factors you want to calibrate");
}
// Transform the probing points to motor endpoints and store them in a matrix, so that we can do multiple iterations using the same data
var probeMotorPositions = new PointError[zBedProbePoints.Count];
var corrections = new double[zBedProbePoints.Count];
var initialSumOfSquares = 0.0;
for (var i = 0; i < zBedProbePoints.Count; ++i)
{
corrections[i] = 0.0;
var machinePos = new double[3]
{
zBedProbePoints[i].X,
zBedProbePoints[i].Y,
0
};
probeMotorPositions[i] = new PointError(Transform(machinePos, 0), Transform(machinePos, 1), Transform(machinePos, 2));
initialSumOfSquares += FSquare(zBedProbePoints[i].X) + FSquare(zBedProbePoints[i].Y) + FSquare(zBedProbePoints[i].ZError);
}
// remove any erroneous data points.. maybe not the best idea??
var zip = zBedProbePoints
.Zip(probeMotorPositions, (point, pos) => new { point, pos })
.Where(_ => !_.pos.X.Equals(double.NaN) && !_.pos.Y.Equals(double.NaN) && !_.pos.ZError.Equals(double.NaN))
.ToList();
zBedProbePoints = (from z in zip select z.point).ToList();
probeMotorPositions = (from z in zip select z.pos).ToArray();
// Do 1 or more Newton-Raphson iterations
var iteration = 0;
double expectedRmsError;
for (;;)
{
// Build a Nx7 matrix of derivatives with respect to xa, xb, yc, za, zb, zc, diagonal.
var derivativeMatrix = new double[zBedProbePoints.Count, numFactors];
for (var i = 0; i < zBedProbePoints.Count; ++i)
{
for (var j = 0; j < numFactors; ++j)
{
derivativeMatrix[i, j] =
ComputeDerivative(j, probeMotorPositions[i].X, probeMotorPositions[i].Y, probeMotorPositions[i].ZError);
}
}
// Now build the normal equations for least squares fitting
var normalMatrix = new double[numFactors, numFactors + 1];
double temp;
for (var i = 0; i < numFactors; ++i)
{
for (var j = 0; j < numFactors; ++j)
{
temp = derivativeMatrix[0,i] * derivativeMatrix[0,j];
for (var k = 1; k < zBedProbePoints.Count; ++k)
{
temp += derivativeMatrix[k,i] * derivativeMatrix[k,j];
}
normalMatrix[i,j] = temp;
}
temp = derivativeMatrix[0,i] * -(zBedProbePoints[0].ZError + corrections[0]);
for (var k = 1; k < zBedProbePoints.Count; ++k)
{
temp += derivativeMatrix[k,i] * -(zBedProbePoints[k].ZError + corrections[k]);
}
normalMatrix[i, numFactors] = temp;
}
double[] solution = GaussJordan(ref normalMatrix, numFactors);
if (solution.Any(_ => _.Equals(double.NaN)))
throw new Exception("Unable to calculate corrections. Please make sure the bed probe points are all distinct.");
//if (debug)
//{
// DebugPrint(PrintVector("Solution", solution));
// // Calculate and display the residuals
// var residuals = [];
// for (var i = 0; i < numPoints; ++i)
// {
// var r = zBedProbePoints[i];
// for (var j = 0; j < numFactors; ++j)
// {
// r += solution[j] * derivativeMatrix.data[i][j];
// }
// residuals.push(r);
// }
// DebugPrint(PrintVector("Residuals", residuals));
//}
Adjust(numFactors, solution, normalise);
// Calculate the expected probe heights using the new parameters
{
var expectedResiduals = new double[zBedProbePoints.Count];
var sumOfSquares = 0.0;
for (var i = 0; i < zBedProbePoints.Count; ++i)
{
probeMotorPositions[i] = new PointError(probeMotorPositions[i].X + solution[0], probeMotorPositions[i].Y + solution[1], probeMotorPositions[i].ZError + solution[2]);
var newZ = InverseTransform(probeMotorPositions[i].X, probeMotorPositions[i].Y, probeMotorPositions[i].ZError);
corrections[i] = newZ;
expectedResiduals[i] = zBedProbePoints[i].ZError + newZ;
sumOfSquares += FSquare(expectedResiduals[i]);
}
expectedRmsError = Math.Sqrt(sumOfSquares / zBedProbePoints.Count);
}
// Decide whether to do another iteration Two is slightly better than one, but three doesn't improve things.
// Alternatively, we could stop when the expected RMS error is only slightly worse than the RMS of the residuals.
++iteration;
if (iteration == 2) { break; }
}
return Tuple.Create(Math.Sqrt(initialSumOfSquares / zBedProbePoints.Count), expectedRmsError);
}
