Baseline Removal

src/hip_controller/__main__.py

@@ -25,6 +25,7 @@ def main(
     log_level: str = DEFAULT_LOG_LEVEL,
     stderr_level: str = DEFAULT_LOG_LEVEL,
     csv_path: Path = BasicConfig.read_data_from_path,
+    show_plot: bool = False,
 ) -> None:  # pragma: no cover
     """Run the main pipeline.
 
@@ -41,7 +42,9 @@ def main(
     app = QtWidgets.QApplication([])
 
     player = CSVPlayer(csv_path)
-    config = BasicConfig(filtered=True)
+    config = BasicConfig(
+        filtered=True, left_limb_plot=show_plot, right_limb_plot=show_plot
+    )
 
     controller_left = WalkOnController(left_limb=True, config=config)
     controller_right = WalkOnController(left_limb=False, config=config)
@@ -102,10 +105,19 @@ def sigint_handler(signal, frame) -> None:
         required=False,
         type=Path,
     )
+
+    parser.add_argument(
+        "--graph-plot",
+        "-g",
+        help="Show PyQT6 plots.",
+        action="store_true",
+    )
+
     args = parser.parse_args()
 
     main(
         log_level=args.log_level,
         stderr_level=args.stderr_level,
         csv_path=args.file_path,
+        show_plot=args.graph_plot,
     )

src/hip_controller/control/signal_processing/filtering.py

@@ -10,7 +10,12 @@
 
 from abc import ABC, abstractmethod
 
-from hip_controller.definitions import LowPassFilterConfig, SogiFllConfig
+from hip_controller.definitions import (
+    KalmanFilterConfig,
+    LowPassFilterConfig,
+    SogiFllConfig,
+)
+from hip_controller.filters.kalman_filter import KalmanFilter
 from hip_controller.filters.second_order_low_pass_filter import (
     SecondOrderLowPassFilter,
 )
@@ -62,12 +67,11 @@ def __init__(self, config: SogiFllConfig) -> None:
     def filter(self, angle_rad: float, time_difference: float) -> float:
         """Estimate velocity using SOGI phase-locked structure.
 
-        :param float angle: Drift-compensated angle [rad].
+        :param float angle_rad: Drift-compensated angle [rad].
         :param float time_difference: Time elapsed since previous sample [s].
-        :param float gyro_velocity: Unused in this implementation.
 
-        :return: (angle_surrogate, velocity_quadrature).
-        :rtype: tuple[float, float]
+        :return: angle_surrogate.
+        :rtype: float
         """
         angle_surrogate, _ = self._sogi_filter.filter(
             raw_theta_rad=angle_rad, time_difference=time_difference
@@ -87,15 +91,12 @@ class LowPassFiltering(FilteringStrategy):
 
     The filter tracks the slow drift component; subtracting its output from the
     raw angle acts as a high-pass and yields *angle_no_drift_low_pass*.
-
-    :param lpf: A configured :class:`SecondOrderLowPassFilter` instance whose
-        cut-off frequency sits well below the motion band.
     """
 
     def __init__(self, config: LowPassFilterConfig) -> None:
         """Create a low-pass drift removal strategy.
 
-        :param SecondOrderLowPassFilter lpf: Low-pass filter for drift estimation.
+        :param config: Low-pass filter configuration for drift estimation.
         :return: None
         :rtype: None
         """
@@ -104,7 +105,7 @@ def __init__(self, config: LowPassFilterConfig) -> None:
     def filter(self, angle_rad: float, time_difference: float) -> float:
         """Execute one drift-removal step.
 
-        :param float raw_angle: Raw angle reading [rad].
+        :param float angle_rad: Raw angle reading [rad].
         :param float time_difference: Difference dt between current timestamp and previous timestamp.
         :return: Drift-compensated angle [rad].
         :rtype: float
@@ -120,3 +121,32 @@ def reset(self) -> None:
         :return: None
         """
         self._low_pass_filter.reset()
+
+
+class KalmanFiltering(FilteringStrategy):
+    """Kalman filter."""
+
+    def __init__(self, config: KalmanFilterConfig):
+        """Initialize the Kalman filter.
+
+        :param config: Kalman filter configuration.
+        """
+        self._kalman_filter = KalmanFilter(config=config)
+
+    def filter(self, angle_rad: float, time_difference: float) -> float:
+        """Execute one Kalman filter step.
+
+        :param angle_rad: Raw angle in rad.
+        :param time_difference: Difference dt between current timestamp and previous timestamp.
+        :return: Drift-compensated angle in rad.
+        """
+        return self._kalman_filter.filter(
+            angle_rad=angle_rad, time_difference=time_difference
+        )
+
+    def reset(self) -> None:
+        """Reset the filter to a known initial condition.
+
+        :return: None
+        """
+        self._kalman_filter.reset()

src/hip_controller/control/signal_processing/sensor_preprocessor.py

@@ -1,23 +1,25 @@
-"""Two-stage sensor preprocessing pipeline: drift removal followed by velocity estimation.
+"""Three-stage sensor preprocessing pipeline: drift removal, filtering, and velocity estimation.
 
-There are two strategies for drift removal and four strategies for velocity estimation implemented in the control module, which can be selected and configured in the :class:`PreprocessorConfig` when initializing the :class:`WalkOnController`.
+There are two strategies for drift removal, three strategies for filtering, and three strategies for velocity estimation
+implemented in the control module, which can be selected and configured in the :class:`PreprocessorConfig` when initializing the :class:`WalkOnController`.
 
 The drift removal strategies include: ``LowPassDriftRemoval`` and ``NotchDriftRemoval``.
 
-The velocity estimation strategies include: ``SogifllVelocityEstimation``, ``LowPassVelocityEstimation``, ``DiscreteDerivativeVelocityEstimation``, and ``GyroscopeVelocityEstimation``.
+The filtering strategies include: ``LowPassFiltering``, ``SogiFllFiltering``, and ``KalmanFiltering``.
+
+The velocity estimation strategies include: ``LowPassVelocityEstimation``, ``DiscreteDerivativeVelocityEstimation``, and ``GyroscopeVelocityEstimation``.
 """
 
 from __future__ import annotations
 
-from loguru import logger
-
 from hip_controller.control.signal_processing.drift_removal import (
     DriftRemovalStrategy,
     LowPassDriftRemoval,
     NotchDriftRemoval,
 )
 from hip_controller.control.signal_processing.filtering import (
     FilteringStrategy,
+    KalmanFiltering,
     LowPassFiltering,
     SogiFllFiltering,
 )
@@ -28,6 +30,7 @@
     VelocityEstimationStrategy,
 )
 from hip_controller.definitions import (
+    BASELINE_REMOVAL_SAMPLE_NUM,
     BasicConfig,
     DriftRemovalMethod,
     FilteringMethod,
@@ -49,7 +52,7 @@ class SensorPreprocessor:
     def __init__(self, basic_config: BasicConfig) -> None:
         """Initialize the sensor pre-processor.
 
-        :param PreprocessorConfig config: Preprocessor configuration.
+        :param basic_config: controller configuration.
         :return: None
         """
         self._basic_config: BasicConfig = basic_config
@@ -59,15 +62,31 @@ def __init__(self, basic_config: BasicConfig) -> None:
         self._velocity_estimation: VelocityEstimationStrategy
 
         self._prev_timestamp: float | None = None
+        self._baseline: float = 0.0
+        self._baseline_count: int = 0
+        self._baseline_sum: float = 0.0
 
-        self.__init_strategies__()
+        self._init_strategies()
 
     def filter(self, raw_signal: SensorSignal) -> SensorSignal:
         """Run one preprocessing step and return a :class:`SensorSignal`.
 
         :return: Preprocessed :class:`SensorSignal` with timestamp of the current sample [s], raw angle from the sensor [rad] and gyroscope angular rate [rad/s] read from sensor.
         :rtype: SensorSignal
         """
+        # Baseline capture by taking avg of first N samples
+        if self._baseline_count < BASELINE_REMOVAL_SAMPLE_NUM:
+            self._baseline_count += 1
+            self._baseline_sum += raw_signal.angle_rad
+
+            if self._baseline_count == BASELINE_REMOVAL_SAMPLE_NUM:
+                self._baseline = self._baseline_sum / BASELINE_REMOVAL_SAMPLE_NUM
+
+            raw_signal.angle_rad = 0.0
+        else:
+            # normal operation: baseline removal
+            raw_signal.angle_rad -= self._baseline
+
         if self._prev_timestamp is None or raw_signal.timestamp is None:
             self._prev_timestamp = raw_signal.timestamp
             return raw_signal
@@ -80,6 +99,7 @@ def filter(self, raw_signal: SensorSignal) -> SensorSignal:
         # check dt too big
         if time_difference > 1.0:
             self.reset()
+            return raw_signal
 
         self._prev_timestamp = raw_signal.timestamp
 
@@ -103,7 +123,7 @@ def filter(self, raw_signal: SensorSignal) -> SensorSignal:
             velocity_rad_per_sec=velocity_out_rad_per_sec,
         )
 
-    def __init_strategies__(self):
+    def _init_strategies(self):
         """Get instance of different options of drift removal, filtering, and velocity estimation."""
         if self._basic_config.drift_removal_method == DriftRemovalMethod.LOW_PASS:
             self._drift_removal = LowPassDriftRemoval(
@@ -115,7 +135,9 @@ def __init_strategies__(self):
                 PreprocessorConfig.drift_removal_notch_config
             )
         else:
-            logger.warning("Selected method does not exist.")
+            raise ValueError(
+                f"Unrecognized drift-removal method: {self._basic_config.drift_removal_method}"
+            )
 
         if self._basic_config.filtering_method == FilteringMethod.SOGI:
             self._filtering = SogiFllFiltering(
@@ -127,8 +149,15 @@ def __init_strategies__(self):
                 PreprocessorConfig.filtering_lowpass_config
             )
 
+        elif self._basic_config.filtering_method == FilteringMethod.KALMAN:
+            self._filtering = KalmanFiltering(
+                PreprocessorConfig.filtering_kalman_config
+            )
+
         else:
-            logger.warning("Selected method does not exist.")
+            raise ValueError(
+                f"Unrecognized filtering method: {self._basic_config.filtering_method}"
+            )
 
         if (
             self._basic_config.velocity_estimation_method
@@ -151,15 +180,19 @@ def __init_strategies__(self):
             self._velocity_estimation = GyroscopeVelocityEstimation()
 
         else:
-            logger.warning("Selected method does not exist.")
+            raise ValueError(
+                f"Unrecognized velocity-estimation method: {self._basic_config.velocity_estimation_method}"
+            )
 
     def reset(self) -> None:
         """Reset the Signal Preprocessor if exosuit is disconnected or timeout occured.
 
         :return: None
         """
         self._prev_timestamp = None
-
+        self._baseline: float = 0.0
+        self._baseline_count: int = 0
+        self._baseline_sum: float = 0.0
         self._drift_removal.reset()
         self._filtering.reset()
         self._velocity_estimation.reset()

src/hip_controller/definitions.py

@@ -1,7 +1,9 @@
 """Common definitions for this module."""
 
 import sys
-from dataclasses import asdict, dataclass
+from dataclasses import asdict, dataclass, field
+
+from numpy.typing import NDArray
 
 if sys.version_info >= (3, 11):
     from enum import StrEnum, auto
@@ -17,6 +19,8 @@ class StrEnum(str, Enum):
 
 import numpy as np
 
+from hip_controller.utils.state_space import StateSpaceLinear
+
 np.set_printoptions(precision=3, floatmode="fixed", suppress=True)
 
 
@@ -125,6 +129,21 @@ class LowPassFilterConfig:
     )  # SolverType enum of numerical integration strategy
 
 
+@dataclass(frozen=True)
+class KalmanFilterConfig:
+    """Settings for the Kalman filter."""
+
+    process_noise: NDArray = field(default_factory=lambda: 2e-2 * np.eye(2))
+    measurement_noise: NDArray = field(default_factory=lambda: 0.75 * np.eye(1))
+    state_space: StateSpaceLinear = field(
+        default_factory=lambda: StateSpaceLinear(
+            A=np.array([[1.0, 0.01], [0.0, 1.0]]), C=np.array([[1.0, 0.0]])
+        )
+    )
+    initial_state: NDArray = field(default_factory=lambda: np.array([0.0, 0.0]))
+    initial_covariance: NDArray = field(default_factory=lambda: 10 * np.eye(2))
+
+
 # Pre processing
 
 
@@ -208,6 +227,7 @@ class PreprocessorConfig:
 
     filtering_sogifll_config: SogiFllConfig = SogiFllConfig()
     filtering_lowpass_config: LowPassFilterConfig = LowPassFilterConfig()
+    filtering_kalman_config: KalmanFilterConfig = KalmanFilterConfig()
 
     filtering_second_order_lpf_config: LowPassFilterConfig = LowPassFilterConfig(
         cut_off_frequency_rad_per_sec=80.0, damping_ratio=1.0, initial_condition=0.0
@@ -218,6 +238,9 @@ class PreprocessorConfig:
     )
 
 
+# baseline removal using first N samples
+BASELINE_REMOVAL_SAMPLE_NUM = 10
+
 # centering & normalization
 VALUE_NEAR_ZERO = 1e-6
 
@@ -233,10 +256,6 @@ class PreprocessorConfig:
 SIGMOID_POWER = 50
 AMPLITUDE_GAIN = -6.5  # Motor position desidered amplitude (rad)
 
-# Kalman filter definitions
-PROCESS_NOISE = 2e-2
-MEASUREMENT_NOISE = 0.75
-
 
 # Cubic Spline Interpolation
 @dataclass(frozen=True)