private void Test(object sender, RoutedEventArgs e)
{
double[] position = new double[] { rnd.NextDouble(), rnd.NextDouble(), rnd.NextDouble() };
SensorNode testNode = new SensorNode("test Node", testID, position);
List <CalUnit> testData = new List <CalUnit>();
CalUnit test = new CalUnit(new double[] { 32.0, 10.3, 20.5 }, 35.401);
testData.Add(test);
CalUnit test2 = new CalUnit(new double[] { -100.2, 5.0, 50.5 }, 30.458);
testData.Add(test2);
CalUnit test3 = new CalUnit(new double[] { 30.0, 40.49, 0.0 }, 26.508);
testData.Add(test3);
testNode.MeasureData = testData;
testNode.GetPosition();
double[] result = testNode.Position;
string resultString = "Result X: " + result[0] + ", Y: " + result[1] + ", Z: " + result[2] + "\n";
Info.Text = resultString;
}
public CalBuffer PartCalculation(double[] cal, CalUnit magInfo)
{
CalBuffer result = new CalBuffer();
double[] dif = new double[3];
double a1, a2;
for (var i = 0; i < 3; i++)
{
dif[i] = cal[i] - magInfo.Position[i];
}
a1 = Math.Pow(dif[0], 2) / 16 + Math.Pow(dif[1], 2) / 16 + Math.Pow(dif[1], 2) / 25;
a2 = Co * Math.Pow(a1, -1.5) - magInfo.Signal;
result.Cost = Math.Pow(a2, 2);
double gradCo = -3 * Co * Math.Pow(a1, -2.5) * a2;
double[] grad = new double[3];
grad[0] = gradCo * dif[0] / 8;
grad[1] = gradCo * dif[1] / 8;
grad[2] = gradCo * dif[2] * 2 / 25;
result.Pos = grad;
return(result);
}
