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 !!*/ }