Skip to content
Snippets Groups Projects
Commit ef80106e authored by Hailong Zhu's avatar Hailong Zhu
Browse files

updated Triangulation.java

height of the tag is now processed on the sensor side.
parents 7f68897c 9ef86458
No related branches found
No related tags found
No related merge requests found
...@@ -3,6 +3,7 @@ package ipos.project.Functionality; ...@@ -3,6 +3,7 @@ package ipos.project.Functionality;
import com.lemmingapex.trilateration.NonLinearLeastSquaresSolver; import com.lemmingapex.trilateration.NonLinearLeastSquaresSolver;
import com.lemmingapex.trilateration.TrilaterationFunction; import com.lemmingapex.trilateration.TrilaterationFunction;
import ipos.project.DataModellntegration.iPos_Datamodel.*; 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.LeastSquaresOptimizer;
import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer; import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer;
import org.apache.commons.math3.linear.RealMatrix; import org.apache.commons.math3.linear.RealMatrix;
...@@ -18,6 +19,7 @@ import java.time.LocalDateTime; ...@@ -18,6 +19,7 @@ import java.time.LocalDateTime;
import java.time.ZoneId; import java.time.ZoneId;
import java.time.format.DateTimeFormatter; import java.time.format.DateTimeFormatter;
import java.util.Date; import java.util.Date;
import java.util.HashMap;
import java.util.MissingResourceException; import java.util.MissingResourceException;
public class Triangulation { public class Triangulation {
...@@ -26,16 +28,16 @@ public class Triangulation { ...@@ -26,16 +28,16 @@ public class Triangulation {
private static IPos_DatamodelFactory modelFactory = IPos_DatamodelFactory.eINSTANCE; private static IPos_DatamodelFactory modelFactory = IPos_DatamodelFactory.eINSTANCE;
public static PositionEvent update(UWB uwbRawDataEvent) throws ParseException { public static PositionEvent update(UWB uwbRawDataEvent) throws ParseException {
double[][] positions = new double[][]{{0, 0}, {0, 0}, {0, 0}, {0, 0}}; double[][] positions = new double[][]{{0, 0}, {0, 0}, {0, 0}, {0, 0}};
double[] distances = new double[]{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; int index = 0;
for (Object key : uwbData.keySet()) { for (Object key : uwbData.keySet()) {
Point3D beaconPoint3D = modelFactory.createPoint3D(); 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()}; positions[index] = new double[]{beaconPoint3D.getX(), beaconPoint3D.getY()};
distances[index] = (Double) uwbData.get(key); distances[index] = Math.pow((Double) uwbData.get(key);
index ++; index ++;
} }
...@@ -57,10 +59,13 @@ public class Triangulation { ...@@ -57,10 +59,13 @@ public class Triangulation {
tagPoint3D.setX((float) centroid[0]); tagPoint3D.setX((float) centroid[0]);
tagPoint3D.setY((float) centroid[1]); tagPoint3D.setY((float) centroid[1]);
tagPoint3D.setZ((float) 0.1); tagPoint3D.setZ((float) PositionMonitoring.TRIANGULATION_HEIGHT);
tagGaussian.setConfidenceInterval((float) standardDeviation.getLInfNorm()); tagGaussian.setConfidenceInterval((float) standardDeviation.getLInfNorm());
tagPosition.setPoint(tagPoint3D); tagPosition.setPoint(tagPoint3D);
tagPosition.setAccuracy(tagGaussian);
tagPosition.setReferenceSystem(DataServices.getReferenceSystemByIdOrNull("ROOT"));
tagPlacing.setPosition(tagPosition); tagPlacing.setPosition(tagPosition);
tagPlacing.setOrientation(modelFactory.createQuaternion());
return createPosEvent(uwbRawDataEvent.getSensorId(), uwbRawDataEvent.getTimeStamp(), tagPlacing); return createPosEvent(uwbRawDataEvent.getSensorId(), uwbRawDataEvent.getTimeStamp(), tagPlacing);
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment