updated Triangulation.java

height of the tag is now processed on the sensor side.

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 {
@@ -29,13 +31,13 @@ public class Triangulation {
         
         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);
 
     }