diff --git a/src/main/java/ipos/project/Functionality/Triangulation.java b/src/main/java/ipos/project/Functionality/Triangulation.java new file mode 100644 index 0000000000000000000000000000000000000000..88aee3fbda1d561d98e1ee7517fe18f623c5dc99 --- /dev/null +++ b/src/main/java/ipos/project/Functionality/Triangulation.java @@ -0,0 +1,80 @@ +package ipos.project.Functionality; + +import com.lemmingapex.trilateration.NonLinearLeastSquaresSolver; +import com.lemmingapex.trilateration.TrilaterationFunction; +import ipos.project.DataModellntegration.iPos_Datamodel.*; +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; +import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer; +import org.apache.commons.math3.linear.RealMatrix; +import org.apache.commons.math3.linear.RealVector; +import org.apache.logging.log4j.LogManager; +import org.eclipse.emf.common.util.EMap; + +import java.sql.Timestamp; +import java.text.ParseException; +import java.lang.*; +import java.text.SimpleDateFormat; +import java.time.LocalDateTime; +import java.time.ZoneId; +import java.time.format.DateTimeFormatter; +import java.util.Date; +import java.util.MissingResourceException; + +public class Triangulation { + private static org.apache.logging.log4j.Logger LOG = LogManager.getLogger(); + + private static IPos_DatamodelFactory modelFactory = IPos_DatamodelFactory.eINSTANCE; + + public static PositionEvent update(UWB uwbRawDataEvent) throws ParseException { + + double[][] positions = new double[][]{{0, 0}, {0, 0}, {0, 0}, {0, 0}}; + double[] distances = new double[]{0,0,0,0}; + EMap uwbData = (EMap) uwbRawDataEvent.getDistances(); + int index = 0; + for (Object key : uwbData.keySet()) { + Point3D beaconPoint3D = modelFactory.createPoint3D(); + beaconPoint3D = (Point3D) DataServices.getPoiByIdOrNull((String) key).getPlacing().getPosition(); + positions[index] = new double[]{beaconPoint3D.getX(), beaconPoint3D.getY()}; + distances[index] = Math.sqrt(Math.pow((Double) uwbData.get(key), 2) + Math.pow((beaconPoint3D.getZ() - 0.1), 2)); + index ++; + } + + + Point3D tagPoint3D = modelFactory.createPoint3D(); + Position tagPosition = modelFactory.createPosition(); + Placing tagPlacing = modelFactory.createPlacing(); + Gaussian tagGaussian = modelFactory.createGaussian(); + + NonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(new TrilaterationFunction(positions, distances), new LevenbergMarquardtOptimizer()); + LeastSquaresOptimizer.Optimum optimum = solver.solve(); + + // the answer + double[] centroid = optimum.getPoint().toArray(); + + // error and geometry information; may throw SingularMatrixException depending the threshold argument provided + RealVector standardDeviation = optimum.getSigma(0); + RealMatrix covarianceMatrix = optimum.getCovariances(0); + + tagPoint3D.setX((float) centroid[0]); + tagPoint3D.setY((float) centroid[1]); + tagPoint3D.setZ((float) 0.1); + tagGaussian.setConfidenceInterval((float) standardDeviation.getLInfNorm()); + tagPosition.setPoint(tagPoint3D); + tagPlacing.setPosition(tagPosition); + return createPosEvent(uwbRawDataEvent.getSensorId(), uwbRawDataEvent.getTimeStamp(), tagPlacing); + + } + private static double[] getPositionFromBeaconId(String beaconId) { + Point3D beaconPos = modelFactory.createPoint3D(); + beaconPos = (Point3D) DataServices.getPoiByIdOrNull(beaconId).getPlacing().getPosition().getPoint(); + double[] position = new double[]{beaconPos.getX(), beaconPos.getY(), beaconPos.getZ()}; + return position; + } + private static PositionEvent createPosEvent(String lObjectId, String timeStamp, Placing placing) { + PositionEvent positionEvent = modelFactory.createPositionEvent(); + positionEvent.setLObjectId(lObjectId); + positionEvent.setTimeStamp(timeStamp); + positionEvent.setPlacing(placing); + return positionEvent; + } +}