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

-added Triangulation.java

parent 4bf44b72
Branches
No related tags found
No related merge requests found
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;
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment