Start working on simple estimation for position

base.py

@@ -111,7 +111,7 @@ class Simulation:
     def make_video(self, floor, vehicle_1, floor_arrays, wheel_positions_1, time_string):
         # init figure
         fig, ax = plt.subplots(nrows=1, ncols=1, sharex=True, sharey=True)
-        
+
         # draw floor
         fx, fy = floor.get_grid()
         timestamp, fc, _ = floor_arrays.popleft()
@@ -246,6 +246,29 @@ class Floor():
             sensor = tile["sensor"]
             sensor.get_response(ts, input_value=sensor._tmp, input_uncertainty=0.0)
 
+    def update_virtual(self):
+        # reset every tile
+        for it, tile in self.tiles.items():
+            sensor = tile["sensor"]
+            sensor._tmp = 0.0
+
+        # sum the mass of all vehicles on the floor 
+        for vehicle in self.vehicles:
+            # check in which tile a wheel is at the moment
+            # could be optimized by binary search tree and fixed length of sensor time-series
+            for iw, wheel in enumerate(vehicle.get_wheel_positions()):
+                for it, tile in self.tiles.items():
+                    sensor = tile["sensor"]
+                    if self.is_inside(wheel, tile):
+                        sensor._tmp += vehicle.wheels_weight[iw] * vehicle.mass
+                        break  # one wheel can only be in one tile
+
+        # return array of _tmp values
+        C = np.zeros(self.shape)
+        for i, tile in self.tiles.items():
+            C[tile["index"]] = tile["sensor"]._tmp
+        return C
+
     def is_inside(self, coordinates, tile):
         b = np.append(coordinates,1)
         c = tile["corners"]

estimate_position_offline.py

+import numpy as np 
+import pandas as pd
+import scipy.optimize as opt
+import matplotlib.pyplot as plt
+
+from base import Vehicle, Floor
+
+class Estimation:
+
+    def __init__(self, filename="2020-01-03__10:06:17_output.csv"):
+
+        self.sensor_output = np.loadtxt(filename, delimiter=",", skiprows=1)
+
+        # init virtual vehicle and floor
+        self.vehicle = Vehicle(init_position=[0,0])
+        self.floor = Floor(shape=(4,3), tile_length=1.0, broadcast=False, vehicles=[self.vehicle])
+
+    # define fitness function
+    def evaluate(self, parameters, sensor_output):
+        self.vehicle.position = parameters[0:1]
+        self.vehicle.rotation = parameters[2]
+        self.vehicle.mass = parameters[3]
+
+        virtual_output = self.floor.update_virtual()
+
+        error = np.linalg.norm(virtual_output - sensor_output)
+
+        print(parameters, error)
+        return error
+    
+    # define fitness function
+    def evaluate_gaussian(self, parameters, floor_grid, sensor_output):
+        mean, std, weight = parameters
+        fx, fy = floor_grid
+
+
+
+
+        virtual_output = self.floor.get_grid()
+
+        error = np.linalg.norm(virtual_output - sensor_output)
+
+        print(parameters, error)
+        return error
+
+if __name__ == "__main__":
+    est = Estimation()
+
+    ## # estimate position via optimization ( TURNS OUT TO BE NOT SUITABLE... )
+    ## wanted_output = est.sensor_output[400,1:].reshape(est.floor.shape)
+    
+    ## # boundary settings
+    ## lb = [0, 0, 0, 0]
+    ## ub = [est.floor.shape[0]*est.floor.tile_length, est.floor.shape[0]*est.floor.tile_length, 2*np.pi, 2*est.vehicle.mass]
+    ## bounds = opt.Bounds(lb, ub, keep_feasible=True)
+    ## bounds_shgo = [(l,u) for l, u in zip(lb, ub)]
+    ## print(bounds_shgo)
+
+    ## # further optimization settings
+    ## options = {"maxiter": 10}
+    ## options_shgo = {"maxiter": 10, "local_iter": 5, "maxfev": 100}
+
+    ## #result = opt.minimize(est.evaluate, [0,0,0,est.vehicle.mass], args=(wanted_output), method="SLSQP", bounds=bounds, options=options)
+    ## result = opt.shgo(est.evaluate, bounds_shgo, args=(wanted_output,), options=options_shgo, n=10)
+    ## print(result)
+
+
+    # estimate position via gaussian fitting
+    wanted_output = est.sensor_output[400,1:].reshape(est.floor.shape)
+    floor_grid = est.floor.get_grid()
+    
+    # boundary settings
+    lb = [0, 0, 0, 0]
+    ub = [est.floor.shape[0]*est.floor.tile_length, est.floor.shape[0]*est.floor.tile_length, 2*np.pi, 2*est.vehicle.mass]
+    bounds = opt.Bounds(lb, ub, keep_feasible=True)
+    bounds_shgo = [(l,u) for l, u in zip(lb, ub)]
+    print(bounds_shgo)
+
+    # further optimization settings
+    options = {"maxiter": 10}
+    options_shgo = {"maxiter": 10, "local_iter": 5, "maxfev": 100}
+
+    #result = opt.minimize(est.evaluate, [0,0,0,est.vehicle.mass], args=(wanted_output), method="SLSQP", bounds=bounds, options=options)
+    result = opt.shgo(est.evaluate, bounds_shgo, args=(wanted_output,), options=options_shgo, n=10)
+    print(result)
+