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 if self.component_instance.level == "raw": self.data['dS'] *= self._factor else: self._drift_yaw += self._gyro_drift real_dx = self.component_instance._dx real_dy = self.component_instance._dy dx = self._factor * ( real_dx * cos(self._drift_yaw) + real_dy * sin(self._drift_yaw)) dy = self._factor * ( real_dy * cos(self._drift_yaw) - real_dx * sin(self._drift_yaw)) self._drift_x += dx - real_dx self._drift_y += dy - real_dy if self.component_instance.level == "integrated": 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'] = real_dx / freq self.data['vy'] = real_dy / freq self.data['wz'] = self.component_instance._dyaw / freq else: self.data['dx'] = dx self.data['dy'] = dy self.data['dyaw'] += self._gyro_drift