Skip to content
Snippets Groups Projects
Commit d43772d9 authored by Maximilian Gruber's avatar Maximilian Gruber
Browse files

Added dynamic to sensors

parent 0067c82f
No related branches found
No related tags found
No related merge requests found
import numpy as np
import scipy.interpolate as sci
import scipy.signal as scs
import matplotlib as mpl
import matplotlib.animation as mpa
import matplotlib.pyplot as plt
......@@ -52,11 +53,11 @@ class Simulation:
t_path, x_path = self.path_to_arrays()
# define floor and vehicle on floor
f = Floor(shape=(10,8), tile_length=0.5)
f = Floor(shape=(5,4), tile_length=1.0)
v = Vehicle(floor=f, init_position=x_path[0])
# update sensors on floor
v.update_floor(timestamp=0)
v.update_floor(timestamp=0.0)
# init visualization
## init figure
......@@ -65,7 +66,7 @@ class Simulation:
## draw floor
fx, fy = f.get_grid()
fc = f.get_tile_values()
pc = ax.pcolormesh(fx, fy, fc, cmap="Greens")
pc = ax.pcolormesh(fx, fy, fc, cmap="Greens", vmin=0, vmax=v.mass)
## vehicle representation
vi = VehicleInteractor(v, pc)
shape = vi.get_shape()
......@@ -102,7 +103,9 @@ class Simulation:
sensors = [tile["sensor"] for tile in f.tiles.values()]
fig, ax = plt.subplots(nrows=len(sensors), ncols=1, sharex=True, sharey=True)
for i, sensor in enumerate(sensors):
ax[i].step(sensor.timestamps, sensor.values, where="post")
ax[i].step(sensor.timestamps, sensor.output_values, where="post")
ax[i].step(sensor.timestamps, sensor.input_values, where="post")
plt.show()
class Vehicle:
......@@ -141,20 +144,23 @@ class Vehicle:
else:
ts = timestamp
# append new entry to every tile
# reset every tile
for it, tile in f.tiles.items():
sensor = tile["sensor"]
sensor.timestamps = np.append(sensor.timestamps, ts)
sensor.values = np.append(sensor.values, 0.0)
sensor.uncertainties = np.append(sensor.uncertainties, 0.0)
sensor.tmp = 0
# check in which tile a wheel is at the moment
for iw, wheel in enumerate(self.get_wheel_positions()):
for it, tile in f.tiles.items():
sensor = tile["sensor"]
if self.is_inside(wheel, tile):
sensor.values[-1] += self.wheels_weight[iw] * self.mass
break
sensor.tmp += self.wheels_weight[iw] * self.mass
break # one wheel can only be in one tile
# "excite" the sensors
for it, tile in f.tiles.items():
sensor = tile["sensor"]
sensor.excite(ts, input_value=sensor.tmp, input_uncertainty=0.0)
def is_inside(self, wheel_position, tile):
b = np.append(wheel_position,1)
......@@ -299,7 +305,7 @@ class Floor():
C = np.zeros(self.shape)
for i, tile in self.tiles.items():
C[tile["index"]] = tile["sensor"].values[-1]
C[tile["index"]] = tile["sensor"].output_values[-1]
return C
......@@ -309,14 +315,48 @@ class Sensor():
def __init__(self):
# time series
self.timestamps = np.ndarray(0)
self.values = np.ndarray(0)
self.uncertainties = np.ndarray(0)
self.timestamps = np.array([])
self.input_values = np.array([])
self.input_uncertainty = np.array([])
self.output_values = np.array([])
self.output_uncertainty = np.array([])
# sensor dynamics
#wp = 0.1 + 0.01*(np.random.random()-0.5)
#ws = 0.2 + 0.01*(np.random.random()-0.5)
#b,a = scs.iirdesign(wp=wp, ws=ws, gpass=1, gstop=60)
wp = 0.4 + 0.01*(np.random.random()-0.5)
b,a = scs.iirfilter(N=5, Wn=[wp], btype="lowpass")
# meta data
self.meta = {"calibration": {}, "location": {}}
# calibration meta data
mc = self.meta["calibration"]
mc["unit"] = ""
mc["quantity"] = ""
\ No newline at end of file
mc["quantity"] = ""
mc["model"] = {"type":"IIR", "b":b, "a":a, "noise": 4e-2}
# internal states
self._tmp = 0.0
self._zi = np.zeros((max(b.size-1, a.size-1)))
def excite(self, timestamp, input_value=0.0, input_uncertainty=0.0):
# filter coeffs
b = self.meta["calibration"]["model"]["b"]
a = self.meta["calibration"]["model"]["a"]
noise = self.meta["calibration"]["model"]["noise"]
# calculate sensor output from input (measurand)
output_value, self._zi = scs.sigtools._linear_filter(b, a, [input_value], 0, self._zi)
output_value += np.random.normal(loc=0.0, scale=noise)
output_uncertainty = input_uncertainty
# write to record
self.timestamps = np.append(self.timestamps, timestamp)
self.input_values = np.append(self.input_values, input_value)
self.input_uncertainty = np.append(self.input_uncertainty, input_uncertainty)
self.output_values = np.append(self.output_values, output_value)
self.output_uncertainty = np.append(self.output_uncertainty, output_uncertainty)
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment