2222def preprocessing (data : dict [str , Array ]) -> dict [str , Array ]:
2323 """Applies preprocessing to collected data.
2424
25- The preprocessing includes
26- outlier detection and interpolation,
27- normalizing orientation (assuming hover at start),
28- calculating rpy from quaternions,
29- and calculating rotational error
25+ The preprocessing includes outlier detection and interpolation, normalizing orientation
26+ (assuming hover at start), calculating rpy from quaternions, and calculating rotational error.
3027
3128 Args:
32- data: The raw data dictionary containing
33- time [s], pos [m], quat, cmd_rpy [rad], cmd_f [N].
29+ data: The raw data dictionary containing time [s], pos [m], quat, cmd_rpy [rad], cmd_f [N].
3430
3531 Returns:
3632 The same dict with the following keys added or modified:
@@ -40,8 +36,7 @@ def preprocessing(data: dict[str, Array]) -> dict[str, Array]:
4036 * ``"quat"``: Quaternions corrected so the initial attitude is identity.
4137 * ``"rpy"``: Roll/pitch/yaw in radians, shape ``(N, 3)``.
4238 * ``"z_axis"``: Body z-axis in world frame, shape ``(N, 3)``.
43- * ``"eR"``: Rotation error vector (vee of skew-symmetric error matrix),
44- shape ``(N, 3)``.
39+ * ``"eR"``: Rotation error vector (vee of skew-symmetric error matrix), shape ``(N, 3)``.
4540 * ``"eR_vec"``: Rotation error as rotation vector, shape ``(N, 3)``.
4641 """
4742 data ["dt" ] = np .diff (data ["time" ])
@@ -94,10 +89,9 @@ def preprocessing(data: dict[str, Array]) -> dict[str, Array]:
9489def derivatives_svf (data : dict [str , Array ]) -> dict [str , Array ]:
9590 """Apply a State Variable Filter (SVF) to compute smoothed signals and their time derivatives.
9691
97- Filters position, attitude (RPY), and command signals with separate
98- corner frequencies (6 Hz for translation, 8 Hz for rotation) and computes
99- up to third-order time derivatives. All output keys are prefixed with
100- ``"SVF_"``.
92+ Filters position, attitude (RPY), and command signals with separate corner frequencies (6 Hz for
93+ translation, 8 Hz for rotation) and computes up to third-order time derivatives. All output
94+ keys are prefixed with ``"SVF_"``.
10195
10296 Args:
10397 data: Dict produced by [preprocessing][crazyflow.dynamics.utils.data_utils.preprocessing].
@@ -106,18 +100,17 @@ def derivatives_svf(data: dict[str, Array]) -> dict[str, Array]:
106100 Returns:
107101 The same dict with the following ``"SVF_"`` keys added:
108102
109- * ``"SVF_pos"``, ``"SVF_vel"``, ``"SVF_acc"``, ``"SVF_jerk"``:
110- Filtered position and its first three derivatives.
111- * ``"SVF_rpy"``, ``"SVF_drpy"``, ``"SVF_ddrpy"``, ``"SVF_dddrpy"``:
112- Filtered roll/pitch/yaw and its first three derivatives.
103+ * ``"SVF_pos"``, ``"SVF_vel"``, ``"SVF_acc"``, ``"SVF_jerk"``: Filtered position and its
104+ first three derivatives.
105+ * ``"SVF_rpy"``, ``"SVF_drpy"``, ``"SVF_ddrpy"``, ``"SVF_dddrpy"``: Filtered roll/pitch/yaw
106+ and its first three derivatives.
113107 * ``"SVF_quat"``: Quaternion computed from ``SVF_rpy``.
114108 * ``"SVF_z_axis"``: Body z-axis in world frame computed from ``SVF_rpy``.
115- * ``"SVF_ang_vel"``, ``"SVF_ang_acc"``, ``"SVF_ang_jerk"``:
116- Angular velocity/acceleration/jerk in body frame.
109+ * ``"SVF_ang_vel"``, ``"SVF_ang_acc"``, ``"SVF_ang_jerk"``: Angular
110+ velocity/acceleration/jerk in body frame.
117111 * ``"SVF_cmd_f"``: Filtered collective thrust command.
118112 * ``"SVF_cmd_rpy"``: Filtered roll/pitch/yaw command.
119- * ``"SVF_eR"``, ``"SVF_eR_vec"``: Rotation error between actual and
120- commanded attitude.
113+ * ``"SVF_eR"``, ``"SVF_eR_vec"``: Rotation error between actual and commanded attitude.
121114 """
122115 # Important: Don't mix with unfiltered signals (also for input!)
123116 if data is None :
0 commit comments