gaitmap.trajectory_reconstruction.RtsKalman#
- class gaitmap.trajectory_reconstruction.RtsKalman(*, initial_orientation: ndarray | Rotation = cf(array([0., 0., 0., 1.])), zupt_variance: float = 1e-07, velocity_error_variance: float = 1000000.0, orientation_error_variance: float = 0.1, level_walking: bool = True, level_walking_variance: float = 1e-07, zupt_detector=cf(NormZuptDetector(inactive_signal_threshold=34.0, metric='maximum', sensor='gyr', window_length_s=0.05, window_overlap=0.5, window_overlap_samples=None)))[source]#
An Error-State-Kalman-Filter (ESKF) with Rauch-Tung-Striebel (RTS) smoothing for trajectory estimation.
This algorithm employs an ESKF to estimate a trajectory and velocity profile from acceleration and gyroscope data. It does three passes over the data, where in the first one the data is integrated and the current error in position, velocity and orientation is tracked with an extended kalman filter. This filter uses a linearized transition matrix in the prediction phase and Zero-Velocity-Updates (ZUPT) in its update phase. In the second pass RTS smoothing is applied to the previously estimated error states and error covariances and in the thrid pass the smoothed error states are applied as correction to the integrated nominal states from the first pass.
This method is expected to be run on long sections of data including multiple strides.
The implementation of the RTS smoothing is based on the paper [1]. The error state kalman filter part itself is based on the paper [2].
- Parameters:
- initial_orientation
The initial orientation of the sensor that is assumed. It is critical that this value is close to the actual orientation. If you pass an array, remember that the order of elements must be x, y, z, w.
- zupt_variance
The variance of the noise of the measured velocity during a ZUPT. As we are typically pretty sure, that the velocity should be zero then, this should be very small.
- velocity_error_variance
The variance of the noise present in the velocity error. Should be based on the sensor accelerometer noise.
- orientation_error_variance
The variance of the noise present in the orientation error. Should be based on the sensor gyroscope noise. The orientation error is internally not represented as quaternion, but as axis-angle representation, which also explains the unit of rad^2 for this variance.
- level_walking
Flag to control if the level walking assumptions should be used during ZUPTs. If this is True, additionally to the velocity, the z position is reset to zero during a ZUPT.
- level_walking_variance
The variance of the noise of the measured position during a level walking update. Should typically be very small.
- zupt_detector
An instance of a valid Zupt detector that will be used to find ZUPTs.
- Other Parameters:
- data
The data passed to the estimate method
- sampling_rate_hz
The sampling rate of this data
- Attributes:
orientation_Orientations as pd.DataFrame.
- orientation_object_
The orientations as a single scipy Rotation object
- position_
The calculated positions
- velocity_
The calculated velocities
- covariance_
The covariance matrices of the kalman filter after smoothing. They can be used as a measure of how good the filter worked and how accurate the results are.
- zupts_
2D array indicating the start and the end samples of the detected ZUPTs for debug porpuses.
rotated_data_Rotated data.
See also
gaitmap.trajectory_reconstructionOther implemented algorithms for orientation and position estimation
gaitmap.trajectory_reconstruction.RegionLevelTrajectoryApply the method to a gait sequence or regions of interest.
Notes
The default values chosen for the noise parameters and the ZUPT-threshold are optimized based on NilsPod recordings with healthy subjects. If you are using a different sensor system or other cohorts, it is advisable to readjust these parameter values. When adjusting the variance of the three different noises for velocity error, orientation error and zupts, the relation between those values is much more important, than the absolute values assigned. As long as the quotient between them is kept about the same and
zupt_threshold_dpsis not changed, similar results are expected.This class uses Numba as a just-in-time-compiler to achieve fast run times. In result, the first execution of the algorithm will take longer as the methods need to be compiled first.
[1]Simón Colomar, J. Nilsson and P. Händel, “Smoothing for ZUPT-aided INSs,”
[2]Solà, Joan. (2015). Quaternion kinematics for the error-state KF.
Examples
Your data must be a pd.DataFrame with columns defined by
SF_COLS.>>> import pandas as pd >>> from gaitmap.utils.consts import SF_COLS >>> from gaitmap.trajectory_reconstruction import RtsKalman >>> data = pd.DataFrame(..., columns=SF_COLS) >>> sampling_rate_hz = 100 >>> # Create an algorithm instance >>> kalman = RtsKalman( ... initial_orientation=np.array([0, 0, 0, 1.0]), ... zupt_variance=10e-8, ... velocity_error_variance=10e5, ... orientation_error_variance=10e-2, ... zupt_detector=NormZuptDetector(semsor="gyr", window_length_s=0.05), ... ) >>> # Apply the algorithm >>> kalman = kalman.estimate(data, sampling_rate_hz=sampling_rate_hz) >>> # Inspect the results >>> kalman.orientation_ <pd.Dataframe with resulting quaternions> >>> kalman.orientation_object_ <scipy.Rotation object> >>> kalman.position_ <pd.Dataframe with resulting positions> >>> kalman.velocity_ <pd.Dataframe with resulting velocities> >>> kalman.covariances_ <pd.Dataframe with resulting covariances>
Methods
clone()Create a new instance of the class with all parameters copied over.
estimate(data, *, sampling_rate_hz[, ...])Estimate the position, velocity and orientation of the sensor.
from_json(json_str)Import an gaitmap object from its json representation.
get_params([deep])Get parameters for this algorithm.
set_params(**params)Set the parameters of this Algorithm.
to_json()Export the current object parameters as json.
- __init__(*, initial_orientation: ndarray | Rotation = cf(array([0., 0., 0., 1.])), zupt_variance: float = 1e-07, velocity_error_variance: float = 1000000.0, orientation_error_variance: float = 0.1, level_walking: bool = True, level_walking_variance: float = 1e-07, zupt_detector=cf(NormZuptDetector(inactive_signal_threshold=34.0, metric='maximum', sensor='gyr', window_length_s=0.05, window_overlap=0.5, window_overlap_samples=None))) None[source]#
- clone() Self[source]#
Create a new instance of the class with all parameters copied over.
This will create a new instance of the class itself and all nested objects
- estimate(data: DataFrame, *, sampling_rate_hz: float, stride_event_list: DataFrame | None = None) Self[source]#
Estimate the position, velocity and orientation of the sensor.
- Parameters:
- data
Continuous sensor data including gyro and acc values. The gyro data is expected to be in deg/s!
- sampling_rate_hz
The sampling rate of the data in Hz
- stride_event_list
Optional stride event list that will be passed to the ZUPT detector. If this information is actually used depends on the ZUPT detector.
- Returns:
- self
The class instance with all result attributes populated
- classmethod from_json(json_str: str) Self[source]#
Import an gaitmap object from its json representation.
For details have a look at the this example.
You can use the
to_jsonmethod of a class to export it as a compatible json string.- Parameters:
- json_str
json formatted string
- get_params(deep: bool = True) dict[str, Any][source]#
Get parameters for this algorithm.
- Parameters:
- deep
Only relevant if object contains nested algorithm objects. If this is the case and deep is True, the params of these nested objects are included in the output using a prefix like
nested_object_name__(Note the two “_” at the end)
- Returns:
- params
Parameter names mapped to their values.
