SPosition CPositioningBasicAlgorithm::CalcPositionInternal(std::map<int /*SensorID*/, double /*Distance*/> Measuremnts, SPosition &Accuracy, int &NumOfIterations)
{
NumOfIterations++;
std::vector<int> ParticipatingSensors = GetListOfSensorsInMeasurements(Measuremnts);
if ((int)Measuremnts.size() < m_SensorsLocationMap->GetMinNumberOfParticipatingSensor())
{
LogEvent(LE_WARNING, __FUNCTION__ ": Tried to calculate position with only %d Sensors. Minimum defined is %d. NOT CALCULATING!",
Measuremnts.size(), m_SensorsLocationMap->GetMinNumberOfParticipatingSensor());
return InvalidPosition;
}
CMatrix B = BuildMatrixB(ParticipatingSensors);
VERIFY_MATRIX(B);
CMatrix F = BuildMatrixF(Measuremnts);
VERIFY_MATRIX(F);
CMatrix Bt = B.GetTransposed();
CMatrix Bsquared = (Bt * B);
CMatrix Bsquared_inverted = Bsquared.GetInverted();
CMatrix ErrorMatrix = Bsquared_inverted * Bt * F;
double dX = ErrorMatrix[0][0];
double dY = ErrorMatrix[1][0];
UpdateLastLocation(dX, dY);
if (IsPositionWellEstimated(dX, dY, NumOfIterations))
{
Accuracy.x = dX;
Accuracy.y = dY;
return m_LastPosition;
}
else
return CalcPositionInternal(Measuremnts, Accuracy, NumOfIterations); /*Note: m_LastPosition has changed in this iteration !!*/
}
