diff --git a/src/main/java/ipos/project/Functionality/Triangulation.java b/src/main/java/ipos/project/Functionality/Triangulation.java index a523e3425c3f1a2f098f588291becb1aab1309f5..36c0197cb49a36e5c9884570f72359babc1eff07 100644 --- a/src/main/java/ipos/project/Functionality/Triangulation.java +++ b/src/main/java/ipos/project/Functionality/Triangulation.java @@ -3,6 +3,7 @@ package ipos.project.Functionality; import com.lemmingapex.trilateration.NonLinearLeastSquaresSolver; import com.lemmingapex.trilateration.TrilaterationFunction; import ipos.project.DataModellntegration.iPos_Datamodel.*; +import ipos.project.UseCaseController.PositionMonitoring; import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer; import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer; import org.apache.commons.math3.linear.RealMatrix; @@ -18,6 +19,7 @@ import java.time.LocalDateTime; import java.time.ZoneId; import java.time.format.DateTimeFormatter; import java.util.Date; +import java.util.HashMap; import java.util.MissingResourceException; public class Triangulation { @@ -26,16 +28,16 @@ public class Triangulation { 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(); + HashMap uwbData = (HashMap) uwbRawDataEvent.getDistances(); int index = 0; for (Object key : uwbData.keySet()) { Point3D beaconPoint3D = modelFactory.createPoint3D(); - beaconPoint3D = (Point3D) DataServices.getPoiByIdOrNull((String) key).getPlacing().getPosition(); + beaconPoint3D = (Point3D) DataServices.getPoiByIdOrNull((String) key).getPlacing().getPosition().getPoint(); positions[index] = new double[]{beaconPoint3D.getX(), beaconPoint3D.getY()}; - distances[index] = (Double) uwbData.get(key); + distances[index] = Math.pow((Double) uwbData.get(key); index ++; } @@ -57,10 +59,13 @@ public class Triangulation { tagPoint3D.setX((float) centroid[0]); tagPoint3D.setY((float) centroid[1]); - tagPoint3D.setZ((float) 0.1); + tagPoint3D.setZ((float) PositionMonitoring.TRIANGULATION_HEIGHT); tagGaussian.setConfidenceInterval((float) standardDeviation.getLInfNorm()); tagPosition.setPoint(tagPoint3D); + tagPosition.setAccuracy(tagGaussian); + tagPosition.setReferenceSystem(DataServices.getReferenceSystemByIdOrNull("ROOT")); tagPlacing.setPosition(tagPosition); + tagPlacing.setOrientation(modelFactory.createQuaternion()); return createPosEvent(uwbRawDataEvent.getSensorId(), uwbRawDataEvent.getTimeStamp(), tagPlacing); }