updated Odometry.java

src/main/java/ipos/project/Functionality/Odometry.java

 package ipos.project.Functionality;
 
 import ipos.project.DataModellntegration.iPos_Datamodel.*;
+import org.apache.logging.log4j.LogManager;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.springframework.stereotype.Component;
@@ -8,66 +9,98 @@ import org.springframework.stereotype.Component;
 import java.sql.Timestamp;
 import java.text.ParseException;
 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;
 
 @Component
 public class Odometry {
-    private final String agentId;
-    private double accuracy;
-    private double timeOfLastCalibration;
-    private double timeLastSensorEvent;
-    private double [] a = new double[]{0, 0, 0};
-    private double [] aOffset;
-    private double [] v = new double[]{0, 0, 0};
-    private double [] p = new double[]{0, 0, 0};
-    private double [] delta = new double[]{0, 0, 0};
-    private double [] vLast = new double[]{0, 0, 0};
-    private double [] pLast = new double[]{0, 0, 0};
-    private double NS2S = 1/10000.0;
-    private boolean calibrated = false;
-    SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS");
+    private static org.apache.logging.log4j.Logger LOG = LogManager.getLogger();
+    private final String agentId = "";
+    private static double accuracy;
+    private static double timeOfLastCalibration;
+    private static double timeLastSensorEvent;
+    private static double [] a = new double[]{0, 0, 0};
+    private static double [][] a_ori = new double[][]{{0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0}};
+    private static double [] aOffset;
+    private static double [] v = new double[]{0, 0, 0};
+    private static double [] p = new double[]{0, 0, 0};
+    private static double [] delta = new double[]{0, 0, 0};
+    private static double [] vLast = new double[]{0, 0, 0};
+    private static double [] pLast = new double[]{0, 0, 0};
+    private static double NS2S = 1/1000.0;
+    private static boolean calibrated = false;
+    private static int index_ori = 0;
+    static SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS");
 
-    public Odometry(String agentId) {
-        this.agentId = agentId;
+    //public Odometry(String agentId) {
+      //  this.agentId = agentId;
+    //}
+    private static Date convertToDateViaInstant(LocalDateTime dateToConvert) {
+        return Date.from(dateToConvert.atZone(ZoneId.systemDefault()).toInstant());
+    }
+
+    private static LocalDateTime convertToLocalDateTimeViaInstant(Date dateToConvert){
+        return dateToConvert.toInstant().atZone(ZoneId.systemDefault()).toLocalDateTime();
     }
+    public static double average(double[] data) {
+        double sum = 0;
+        double average;
 
-    public void calibrate (PositionEvent event) throws ParseException {
-        this.vLast[0] = 0;
-        this.vLast[1] = 0;
-        this.vLast[2] = 0;
-        aOffset =a;
-        this.accuracy = 0.03;
+        for(int i=0; i < data.length; i++){
+            sum = sum + data[i];
+        }
+        average = sum/data.length;
+        return average;
+    }
+    public static void calibrate (PositionEvent event) throws ParseException {
+        v = new double[] {0,0,0};
+        vLast = new double[] {0,0,0};
+        aOffset = new double[] {average(a_ori[0]),average(a_ori[1]),average(a_ori[2])};
+        accuracy = 0.03;
         if (event.getPlacing().getPosition().getPoint() instanceof Point3D) {
-            this.pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX();
-            this.pLast[1] = ((Point3D) event.getPlacing().getPosition().getPoint()).getY();
-            this.pLast[2] = ((Point3D) event.getPlacing().getPosition().getPoint()).getZ();
+            pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX();
+            pLast[1] = ((Point3D) event.getPlacing().getPosition().getPoint()).getY();
+            pLast[2] = ((Point3D) event.getPlacing().getPosition().getPoint()).getZ();
+            p = pLast;
         }
-        Date date = sdf2.parse(event.getTimeStamp());
+        LOG.info("Odometry: timestamp" + event.getTimeStamp());
+        LocalDateTime eventTime = LocalDateTime.parse(event.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
+        Date date = convertToDateViaInstant(eventTime);
+
         timeOfLastCalibration = date.getTime();
+        calibrated = true;
     }
 
-    public PositionEvent Update (IMU imuRawdataEvent) throws ParseException {
+    public static PositionEvent update(IMU imuRawdataEvent) throws ParseException {
         IPos_DatamodelFactory datamodelFactory = IPos_DatamodelFactory.eINSTANCE;
         Timestamp timestamp = new Timestamp(System.currentTimeMillis());
-
-
-
         Date date = sdf2.parse(imuRawdataEvent.getTimeStamp());
-        timeLastSensorEvent = date.getTime();
+
         if (calibrated) {
             double dt = (date.getTime() - timeLastSensorEvent) * NS2S;
+            a_ori[0][index_ori] = imuRawdataEvent.getAcceleration().getX();
+            a_ori[1][index_ori] = imuRawdataEvent.getAcceleration().getY();
+            a_ori[2][index_ori] = imuRawdataEvent.getAcceleration().getZ();
+            index_ori += 1;
+            if (index_ori>9) {
+                index_ori =0;
+            }
             a[0] = imuRawdataEvent.getAcceleration().getX() - aOffset[0];
             a[1] = imuRawdataEvent.getAcceleration().getY() - aOffset[1];
             a[2] = imuRawdataEvent.getAcceleration().getZ() - aOffset[2];
             for (int index = 0; index < 3; ++index) {
-                double last_velocity = vLast[index];
-                v[index] += (a[index] + vLast[index]) / 2 * dt;
-                delta[index] += (v[index] + last_velocity) / 2 * dt;
+                v[index] = a[index] * dt + vLast[index];
+                delta[index] = (v[index] + vLast[index]) / 2 * dt;
                 p[index] = pLast[index] + delta[index];
+                pLast[index] = p[index];
+                vLast[index] = v[index];
             }
-            accuracy = (date.getTime() - timeOfLastCalibration) * 0.01;
-
+            accuracy = (date.getTime() - timeOfLastCalibration + 500) * 0.0001;
+            timeLastSensorEvent = date.getTime();
+            Quaternion calOrientation = datamodelFactory.createQuaternion();
             PositionEvent calPositionEvent = datamodelFactory.createPositionEvent();
             Placing calPlacing = datamodelFactory.createPlacing();
             LocalizableObject calLObject = datamodelFactory.createLocalizableObject();
@@ -78,18 +111,33 @@ public class Odometry {
             calPoint3D.setX((float) p[0]);
             calPoint3D.setY((float) p[1]);
             calPoint3D.setZ((float) p[2]);
+            calOrientation.setX(0);
+            calOrientation.setY(0);
+            calOrientation.setZ(0);
+            calOrientation.setW(0);
             calPosition.setPoint(calPoint3D);
             calPosition.setAccuracy(calGaussian);
+            calPosition.setReferenceSystem(DataServices.getReferenceSystemByIdOrNull("ROOT"));
             calPlacing.setPosition(calPosition);
-            calLObject.setId(imuRawdataEvent.getSensorId());
-            calPositionEvent.setTimeStamp(imuRawdataEvent.getTimeStamp());
+            calPlacing.setOrientation(calOrientation);
+            //calLObject.setId(imuRawdataEvent.getSensorId());
+            LocalDateTime timestamp_LDT=convertToLocalDateTimeViaInstant(date);
+
+
+            calPositionEvent.setLObjectId(imuRawdataEvent.getSensorId());
+            calPositionEvent.setTimeStamp(timestamp_LDT.toString() + "+00:00");
             calPositionEvent.setPlacing(calPlacing);
             return calPositionEvent;
         }
         else {
-            a[0] = imuRawdataEvent.getAcceleration().getX();
-            a[1] = imuRawdataEvent.getAcceleration().getY();
-            a[2] = imuRawdataEvent.getAcceleration().getZ();
+            timeLastSensorEvent = date.getTime();
+            a_ori[0][index_ori] = imuRawdataEvent.getAcceleration().getX();
+            a_ori[1][index_ori] = imuRawdataEvent.getAcceleration().getY();
+            a_ori[2][index_ori] = imuRawdataEvent.getAcceleration().getZ();
+            index_ori += 1;
+            if (index_ori>9) {
+                index_ori =0;
+            }
 
             throw new MissingResourceException("Odometry not calibrated!", "Odometry", "calibration");
         }