Source code for morse.modifiers.odometry_noise

import logging; logger = logging.getLogger("morse." + __name__)

from morse.modifiers.abstract_modifier import AbstractModifier
from math import cos, sin

[docs]class OdometryNoiseModifier(AbstractModifier): """ This modifier allows to simulate two common issues when calculating odometry : - an error in the scale factor used to compute the distance from the value returned by the odometer (parameter **factor**) - the gyroscope natural drift (parameter **gyro_drift** (rad by tick)) Modified data ------------- The modifier only accumulate errors for a 2D odometry sensor. It modifies so the following variables : - **dS** by the scale factor - **dx** considering the scale factor and gyroscope drift - **dy** considering the scale factor and gyroscope drift - **dyaw** considering the gyroscope drift - **x** considering the scale factor and gyroscope drift - **y** considering the scale factor and gyroscope drift - **yaw** considering the gyroscope drift - **vx** considering the new **dx** - **vy** considering the new **dy** - **wz** considering the new **dyaw** Available methods ----------------- - ``noisify``: Simulate drift of gyroscope and possible error in the scale factor """
[docs] def initialize(self): self._factor = float(self.parameter("factor", default=1.05)) self._gyro_drift = float(self.parameter("gyro_drift", default=0)) self._drift_x = 0.0 self._drift_y = 0.0 self._drift_yaw = 0.0
[docs] def modify(self): # Basic 2D odometry implementation dx = dS * sin(yaw) and # dy = dS * cos(yaw) # If we have some error on dS and yaw, we have # dx = factor * dS * sin(yaw + drift_yaw) # = factor * dS * sin(yaw) * cos(drift_yaw) + # factor * dS * cos(yaw) * sin(drift_yaw) # = factor * ( dx * cos(drift_yaw) + dy * sin(drift_yaw)) # Same thing to compute dy try: self._drift_yaw += self._gyro_drift dx = self._factor * ( self.data['dx'] * cos(self._drift_yaw) + self.data['dy'] * sin(self._drift_yaw)) dy = self._factor * ( self.data['dy'] * cos(self._drift_yaw) - self.data['dx'] * sin(self._drift_yaw)) self._drift_x += dx - self.data['dx'] self._drift_y += dy - self.data['dy'] self.data['dS'] *= self._factor self.data['dx'] = dx self.data['dy'] = dy self.data['dyaw'] += self._gyro_drift self.data['x'] += self._drift_x self.data['y'] += self._drift_y self.data['yaw'] += self._drift_yaw freq = self.component_instance.frequency self.data['vx'] = self.data['dx'] / freq self.data['vy'] = self.data['dy'] / freq self.data['wz'] = self.data['dyaw'] / freq except KeyError as detail: self.key_error(detail)