Add IMU to Kalman Filter (#259)

- [x] Real life tests
- [x] Implement IMU changes in
https://github.com/zauberzeug/rosys/tree/rework_imu
- [x] Revert U4 config changes when Lizard is fixed (Tests fail because
tornado is deactivated)
- [x] Wait for zauberzeug/rosys#250 and [RoSys
0.22.0](https://github.com/zauberzeug/rosys/milestone/38)

config/u4_config_rb28/hardware.py

@@ -111,5 +111,9 @@
         'rx_pin': 32,
         'tx_pin': 33,
         'baud': 1_000_000,
-    }
+    },
+    'imu': {
+        'name': 'imu',
+        'offset_rotation': [-1.570796, 0, 0],
+    },
 }

field_friend/hardware/field_friend.py

@@ -35,6 +35,7 @@ def __init__(
             y_axis: Axis | ChainAxis | None,
             z_axis: Axis | Tornado | None,
             mower: Mower | None,
+            imu: rosys.hardware.Imu | None,
             estop: rosys.hardware.EStop,
             bumper: rosys.hardware.Bumper | None,
             bms: rosys.hardware.Bms,
@@ -48,6 +49,7 @@ def __init__(
         self.y_axis = y_axis
         self.z_axis = z_axis
         self.mower = mower
+        self.imu = imu
         self.estop = estop
         self.bumper = bumper
         self.bms = bms

field_friend/hardware/field_friend_hardware.py

@@ -2,6 +2,7 @@
 
 import numpy as np
 import rosys
+from rosys.hardware import ImuHardware
 
 import config.config_selection as config_selector
 
@@ -15,7 +16,6 @@
 from .flashlight_pwm import FlashlightPWMHardware
 from .flashlight_pwm_v2 import FlashlightPWMHardwareV2
 from .flashlight_v2 import FlashlightHardwareV2
-from .imu import IMUHardware
 from .led_eyes import LedEyesHardware
 from .safety import SafetyHardware
 from .status_control import StatusControlHardware
@@ -396,13 +396,13 @@ def __init__(self) -> None:
         else:
             bumper = None
 
-        self.imu: IMUHardware | None
+        imu: rosys.hardware.Imu | None = None
         if 'imu' in config_hardware:
-            self.imu = IMUHardware(robot_brain,
-                                   name=config_hardware['imu']['name'],
-                                   )
-        else:
-            self.imu = None
+            imu = ImuHardware(robot_brain=robot_brain,
+                              name=config_hardware['imu']['name'],
+                              offset_rotation=rosys.geometry.Rotation.from_euler(
+                                  *config_hardware['imu']['offset_rotation']),
+                              )
 
         eyes: LedEyesHardware | None
         if 'eyes' in config_hardware:
@@ -428,7 +428,7 @@ def __init__(self) -> None:
                                                 y_axis=y_axis, z_axis=z_axis, flashlight=flashlight, mower=mower)
 
         modules = [bluetooth, can, wheels, serial, expander, can_open_master, y_axis,
-                   z_axis, mower, flashlight, bms, estop, self.battery_control, bumper, self.imu, eyes, self.status_control, safety]
+                   z_axis, mower, flashlight, bms, estop, self.battery_control, bumper, imu, eyes, self.status_control, safety]
         active_modules = [module for module in modules if module is not None]
         super().__init__(implement_name=implement,
                          wheels=wheels,
@@ -440,6 +440,7 @@ def __init__(self) -> None:
                          safety=safety,
                          flashlight=flashlight,
                          mower=mower,
+                         imu=imu,
                          modules=active_modules,
                          robot_brain=robot_brain)
 

field_friend/hardware/field_friend_simulation.py

@@ -1,5 +1,6 @@
 import numpy as np
 import rosys
+from rosys.hardware import ImuSimulation
 
 # change the config to the config of simulated Robot
 import config.config_selection as config_selector
@@ -103,10 +104,12 @@ def __init__(self, robot_id) -> None:
             bumper = rosys.hardware.BumperSimulation(estop=estop)
         else:
             bumper = None
+
+        imu = ImuSimulation(wheels=wheels, roll_noise=0.0, pitch_noise=0.0, yaw_noise=0.0)
         bms = rosys.hardware.BmsSimulation()
         safety = SafetySimulation(wheels=wheels, estop=estop, y_axis=y_axis,
                                   z_axis=z_axis, flashlight=flashlight, mower=mower)
-        modules = [wheels, y_axis, z_axis, flashlight, bumper, bms, estop, safety, mower]
+        modules = [wheels, y_axis, z_axis, flashlight, bumper, imu, bms, estop, safety, mower]
         active_modules = [module for module in modules if module is not None]
         super().__init__(implement_name=tool,
                          wheels=wheels,
@@ -116,6 +119,7 @@ def __init__(self, robot_id) -> None:
                          mower=mower,
                          estop=estop,
                          bumper=bumper,
+                         imu=imu,
                          bms=bms,
                          safety=safety,
                          modules=active_modules)

field_friend/interface/components/development.py

@@ -37,6 +37,9 @@ def create_development_ui(system: System) -> None:
             system.robot_locator.developer_ui()
         with ui.card():
             system.gnss.developer_ui()
+        if isinstance(system.field_friend.imu, rosys.hardware.Imu):
+            with ui.card():
+                system.field_friend.imu.developer_ui()
         with ui.card():
             system.field_navigation.developer_ui()
         if isinstance(system.field_friend, rosys.hardware.RobotHardware):

field_friend/robot_locator.py

@@ -6,12 +6,12 @@
 import rosys.helpers
 from nicegui import ui
 from rosys.geometry import Pose3d, Rotation
-from rosys.hardware import Gnss, GnssMeasurement, Wheels
+from rosys.hardware import Gnss, GnssMeasurement, Imu, ImuMeasurement, Wheels
 
 
 class RobotLocator(rosys.persistence.PersistentModule):
 
-    def __init__(self, wheels: Wheels, gnss: Gnss) -> None:
+    def __init__(self, wheels: Wheels, gnss: Gnss, imu: Imu) -> None:
         """ Robot Locator based on an extended Kalman filter.
 
         ### State
@@ -79,31 +79,38 @@ def __init__(self, wheels: Wheels, gnss: Gnss) -> None:
         """
         super().__init__(persistence_key='field_friend.robot_locator')
         self.log = logging.getLogger('field_friend.robot_locator')
+
+        self.wheels = wheels
+        self.gnss = gnss
+        self.imu = imu
+
         self.pose_frame = Pose3d().as_frame('field_friend.robot_locator')
         self.x = np.zeros((6, 1))
         self.Sxx = np.zeros((6, 6))
         self.t = rosys.time()
 
         self.ignore_odometry = False
         self.ignore_gnss = False
-
+        self.ignore_imu = False
         self.q_odometry_v = 0.01
         self.q_odometry_omega = 0.01
-
+        self.q_imu_yaw = 0.01
         self.r_x = 0.01
         self.r_y = 0.01
         self.r_theta = 0.01
         self.r_v = 0.01
         self.r_omega = 1.0
         self.r_a = 1.0
 
-        wheels.VELOCITY_MEASURED.register(self.handle_velocity_measurement)
-        gnss.NEW_MEASUREMENT.register(self.handle_gnss_measurement)
+        self.wheels.VELOCITY_MEASURED.register(self.handle_velocity_measurement)
+        self.gnss.NEW_MEASUREMENT.register(self.handle_gnss_measurement)
+        self.imu.NEW_MEASUREMENT.register(self.handle_imu_measurement)
 
     def backup(self) -> dict[str, Any]:
         return {
             'q_odometry_v': self.q_odometry_v,
             'q_odometry_omega': self.q_odometry_omega,
+            'q_imu_yaw': self.q_imu_yaw,
             'r_x': self.r_x,
             'r_y': self.r_y,
             'r_theta': self.r_theta,
@@ -115,6 +122,7 @@ def backup(self) -> dict[str, Any]:
     def restore(self, data: dict[str, Any]) -> None:
         self.q_odometry_v = data.get('q_odometry_v', 0.01)
         self.q_odometry_omega = data.get('q_odometry_omega', 0.01)
+        self.q_imu_yaw = data.get('q_imu_yaw', 0.01)
         self.r_x = data.get('r_x', 0.01)
         self.r_y = data.get('r_y', 0.01)
         self.r_theta = data.get('r_theta', 0.01)
@@ -179,6 +187,18 @@ def handle_gnss_measurement(self, gnss_measurement: GnssMeasurement) -> None:
         Q = np.diag([r_xy, r_xy, r_theta])**2
         self.update(z=np.array(z), h=np.array(h), H=np.array(H), Q=Q)
 
+    def handle_imu_measurement(self, imu_measurement: ImuMeasurement) -> None:
+        if self.ignore_imu:
+            return
+        self.predict(imu_measurement.time)
+        z = [[imu_measurement.angular_velocity.yaw]]
+        h = [[self.x[4, 0]]]
+        H = [
+            [0, 0, 0, 0, 1, 0]
+        ]
+        Q = np.diag([self.q_imu_yaw])**2
+        self.update(z=np.array(z), h=np.array(h), H=np.array(H), Q=Q)
+
     def predict(self, time: float | None = None) -> None:
         if time is None:
             time = rosys.time()
@@ -242,9 +262,12 @@ def developer_ui(self) -> None:
                     .bind_value(self, 'ignore_odometry')
                 ui.checkbox('Ignore GNSS').props('dense color=red').classes('col-span-2') \
                     .bind_value(self, 'ignore_gnss')
+                ui.checkbox('Ignore IMU').props('dense color=red').classes('col-span-2') \
+                    .bind_value(self, 'ignore_imu')
                 for key, label in {
                     'q_odometry_v': 'Q Odometry v',
                     'q_odometry_omega': 'Q Odometry ω',
+                    'q_imu_yaw': 'Q IMU yaw',
                     'r_x': 'R x',
                     'r_y': 'R y',
                     'r_theta': 'R θ',

field_friend/system.py

@@ -73,7 +73,9 @@ def __init__(self) -> None:
                 self.log.exception(f'failed to initialize FieldFriendHardware {self.version}')
             assert isinstance(self.field_friend, FieldFriendHardware)
             self.gnss = self.setup_gnss()
-            self.robot_locator = RobotLocator(self.field_friend.wheels, self.gnss)
+            # TODO: is IMU optional?
+            assert isinstance(self.field_friend.imu, rosys.hardware.ImuHardware)
+            self.robot_locator = RobotLocator(self.field_friend.wheels, self.gnss, self.field_friend.imu)
             self.mjpeg_camera_provider = rosys.vision.MjpegCameraProvider(username='root', password='zauberzg!')
             self.detector = rosys.vision.DetectorHardware(port=8004)
             self.monitoring_detector = rosys.vision.DetectorHardware(port=8005)
@@ -82,7 +84,9 @@ def __init__(self) -> None:
             self.field_friend = FieldFriendSimulation(robot_id=self.version)
             assert isinstance(self.field_friend.wheels, rosys.hardware.WheelsSimulation)
             self.gnss = self.setup_gnss(self.field_friend.wheels)
-            self.robot_locator = RobotLocator(self.field_friend.wheels, self.gnss)
+            # TODO: is IMU optional?
+            assert isinstance(self.field_friend.imu, rosys.hardware.ImuSimulation)
+            self.robot_locator = RobotLocator(self.field_friend.wheels, self.gnss, self.field_friend.imu)
             # NOTE we run this in rosys.startup to enforce setup AFTER the persistence is loaded
             rosys.on_startup(self.setup_simulated_usb_camera)
             self.detector = rosys.vision.DetectorSimulation(self.camera_provider)