diff --git a/src/hip_controller/__main__.py b/src/hip_controller/__main__.py index 4b4068c..cd6181b 100644 --- a/src/hip_controller/__main__.py +++ b/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, ) diff --git a/src/hip_controller/control/signal_processing/filtering.py b/src/hip_controller/control/signal_processing/filtering.py index f08a3ef..8b5f3a1 100644 --- a/src/hip_controller/control/signal_processing/filtering.py +++ b/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() diff --git a/src/hip_controller/control/signal_processing/sensor_preprocessor.py b/src/hip_controller/control/signal_processing/sensor_preprocessor.py index 39aa0ed..5421df6 100644 --- a/src/hip_controller/control/signal_processing/sensor_preprocessor.py +++ b/src/hip_controller/control/signal_processing/sensor_preprocessor.py @@ -1,16 +1,17 @@ -"""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, @@ -18,6 +19,7 @@ ) 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,8 +62,11 @@ 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`. @@ -68,6 +74,19 @@ def filter(self, raw_signal: SensorSignal) -> 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,7 +180,9 @@ 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. @@ -159,7 +190,9 @@ def reset(self) -> None: :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() diff --git a/src/hip_controller/definitions.py b/src/hip_controller/definitions.py index c9f7057..f4c1a57 100644 --- a/src/hip_controller/definitions.py +++ b/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) diff --git a/src/hip_controller/filters/kalman_filter.py b/src/hip_controller/filters/kalman_filter.py index d3d8e2d..d120351 100644 --- a/src/hip_controller/filters/kalman_filter.py +++ b/src/hip_controller/filters/kalman_filter.py @@ -3,9 +3,8 @@ import numpy as np from numpy.typing import NDArray -from hip_controller.definitions import MEASUREMENT_NOISE, PROCESS_NOISE +from hip_controller.definitions import KalmanFilterConfig from hip_controller.utils.math_utils import symmetrize_matrix -from hip_controller.utils.state_space import StateSpaceLinear class KalmanFilter: @@ -13,32 +12,19 @@ class KalmanFilter: def __init__( self, - state_space: StateSpaceLinear, - initial_x: np.ndarray, - initial_covariance: np.ndarray, - process_noise: NDArray | None = None, - measurement_noise: NDArray | None = None, + config: KalmanFilterConfig, ) -> None: """Initialize the Kalman Filter. - :param state_space: linear state space model - :param initial_x: Initial state estimate - :param initial_covariance: Initial error covariance - :param process_noise: Process noise covariance - :param measurement_noise: Measurement noise covariance + :param config: Kalman filter configuration. :return: None """ - self.state_space = state_space - if process_noise is None: - process_noise = PROCESS_NOISE * np.eye(len(state_space.A)) - self.Q: np.ndarray = process_noise - - if measurement_noise is None: - measurement_noise = MEASUREMENT_NOISE * np.eye(len(state_space.C)) - self.R: np.ndarray = measurement_noise - - self.x: np.ndarray = initial_x - self.cov: np.ndarray = initial_covariance + self.config = config # save initial state for resets + self.state_space = config.state_space + self.Q: NDArray = config.process_noise + self.R: NDArray = config.measurement_noise + self.x: NDArray = config.initial_state + self.cov: NDArray = config.initial_covariance def predict(self, u: NDArray | None = None) -> None: """Predict the next state and error covariance. @@ -65,3 +51,24 @@ def update(self, z: NDArray) -> NDArray: self.cov = symmetrize_matrix(cov) return z - self.state_space.C @ self.x + + def filter(self, angle_rad: float, time_difference: float) -> float: + """Execute one filter step, containing a prediction step and an update 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. + """ + # modify the state transition matrix with the given time step + self.state_space.A[0, 1] = time_difference + self.predict(u=None) + self.update(z=np.array([angle_rad])) + return float(self.x[0]) + + def reset(self) -> None: + """Reset the Kalman filter to its initial condition. + + :return: None + """ + self.x = self.config.initial_state + self.cov = self.config.initial_covariance diff --git a/src/matlab-files/still_to_implement_controlling.slx b/src/matlab-files/still_to_implement_controlling.slx new file mode 100644 index 0000000..578967d Binary files /dev/null and b/src/matlab-files/still_to_implement_controlling.slx differ diff --git a/tests/controller_test/pre_process_testing/drift_removal_test.py b/tests/controller_test/pre_process_testing/drift_removal_test.py index abd06aa..2f40a0b 100644 --- a/tests/controller_test/pre_process_testing/drift_removal_test.py +++ b/tests/controller_test/pre_process_testing/drift_removal_test.py @@ -51,7 +51,7 @@ def test_low_pass_drift_removal() -> None: def test_notch_drift_removal() -> None: - """Test LowPassDriftRemoval against expected outputs.""" + """Test NotchDriftRemoval against expected outputs.""" # Load test data df = pd.read_csv(DATA_PRE_PROCESSING) diff --git a/tests/controller_test/pre_process_testing/filtering_test.py b/tests/controller_test/pre_process_testing/filtering_test.py index 99d0f8a..9af5bdf 100644 --- a/tests/controller_test/pre_process_testing/filtering_test.py +++ b/tests/controller_test/pre_process_testing/filtering_test.py @@ -3,7 +3,9 @@ from numpy import testing from pandas import read_csv -from hip_controller.control.signal_processing.filtering import SogiFllFiltering +from hip_controller.control.signal_processing.filtering import ( + SogiFllFiltering, +) from hip_controller.control.signal_processing.velocity_estimation import ( DiscreteDerivativeVelocityEstimation, ) diff --git a/tests/controller_test/pre_process_testing/kalman_test.py b/tests/controller_test/pre_process_testing/kalman_test.py index 7b21029..a836dc9 100644 --- a/tests/controller_test/pre_process_testing/kalman_test.py +++ b/tests/controller_test/pre_process_testing/kalman_test.py @@ -2,6 +2,7 @@ import numpy as np +from hip_controller.definitions import KalmanFilterConfig from hip_controller.filters.kalman_filter import KalmanFilter from hip_controller.utils.state_space import StateSpaceLinear @@ -22,12 +23,14 @@ def test_kalman_filter_initialization() -> None: C = np.eye(2) ss = StateSpaceLinear(A=A, C=C) - # Act - kf = KalmanFilter( + config = KalmanFilterConfig( state_space=ss, - initial_x=np.zeros((2, 1)), + initial_state=np.zeros((2, 1)), initial_covariance=np.eye(2), ) + + # Act + kf = KalmanFilter(config=config) for _i in range(10): kf.predict() _ = kf.update(z=np.array([[0.0]]))