Add visualize IMU example and biases to EKF

docs/examples/apriltag.md

@@ -1,7 +1,7 @@
 ---
 title: AprilTag Detection
 parent: Examples
-nav_order: 2
+nav_order: 5
 ---
 
 # AprilTag Detection

docs/examples/ekf/index.md

@@ -1,7 +1,7 @@
 ---
 title: Extended Kalman Filter
 parent: Examples
-nav_order: 4
+nav_order: 3
 ---
 
 # Extended Kalman Filter (EKF) Examples

docs/examples/ekf/se23_one_robot.md

@@ -76,6 +76,15 @@ gt_se23 = liegroups.get_se23_poses(
 )
 ```
 
+Additionally, we extract the IMU biases at these timestamps to evaluate the EKF's bias estimates. These ground truth biases are provided as part of the dataset, and are computed by comparing the smoothed IMU data with the ground truth pose data.
+
+```py
+gt_bias = imu_at_query_timestamps["imu_px4"][[
+    "gyro_bias.x", "gyro_bias.y", "gyro_bias.z", 
+    "accel_bias.x", "accel_bias.y", "accel_bias.z"
+]].to_numpy()
+```
+
 ## Extended Kalman Filter
 
 We now implement the EKF for the one-robot IMU example. In here, we will go through some of the implementation details of the EKF, but as before, the EKF implementation is in the *models* module and the main script only calls these EKF methods to avoid cluttering the main script. 
@@ -89,7 +98,7 @@ ekf_history = {
 }
 ```
 
-We then initialize the EKF with the first ground truth pose, the anchor postions, and UWB tag moment arms. Inside the constructor of the EKF, we add noise to have some initial uncertainty in the state, and set the initial bias estimate to zero.
+We then initialize the EKF with the first ground truth pose, the anchor positions, and UWB tag moment arms. Inside the constructor of the EKF, we add noise to have some initial uncertainty in the state, and set the initial bias estimate to zero.
 
 ```py
 ekf = model.EKF(gt_se23[0], miluv.anchors, miluv.tag_moment_arms)
@@ -275,21 +284,17 @@ for i in range(0, len(query_timestamps)):
 We can now evaluate the EKF using the ground truth data and plot the results. We first evaluate the EKF using the ground truth data and the EKF history, using the example-specific evaluation functions in the *models* module. 
 
 ```py
-analysis = model.EvaluateEKF(gt_se23, ekf_history, exp_name)
+analysis = model.EvaluateEKF(gt_se23, gt_bias, ekf_history, exp_name)
 ```
 
 Lastly, we call the following functions to plot the results and save the results to disk, and we are done!
 
 ```py
 analysis.plot_error()
 analysis.plot_poses()
-analysis.plot_biases()
+analysis.plot_bias_error()
 analysis.save_results()
 ```
 
-<p align="center">
-<img src="https://decargroup.github.io/miluv/assets/imu/13_error.png" alt="drawing" width="400" class="center"/>
-</p>
-
-![IMU EKF Pose Plot for Experiment #13](https://decargroup.github.io/miluv/assets/imu/13_poses.png) | ![IMU EKF Bias Plot for Experiment #13](https://decargroup.github.io/miluv/assets/imu/13_biases.png)
+![IMU EKF Pose Plot for Experiment #13](https://decargroup.github.io/miluv/assets/ekf_imu/13_poses.png) | ![IMU EKF Error Plot for Experiment #13](https://decargroup.github.io/miluv/assets/ekf_imu/13_error.png)
 

docs/examples/ekf/se23_three_robot.md

@@ -63,7 +63,7 @@ accel: pd.DataFrame = {
 }
 ```
 
-Lastly, we extract the ground truth poses for all three robots at these timestamps, again taking advantage of the MILUV-provided spline interpolation to get the poses at the query timestamps and get the first derivative of the position to get the velocity.
+Lastly, we extract the ground truth poses and biases for all three robots at these timestamps, again taking advantage of the MILUV-provided spline interpolation to get the poses at the query timestamps and get the first derivative of the position to get the velocity.
 
 ```py
 gt_se23 = {
@@ -72,6 +72,13 @@ gt_se23 = {
     )
     for robot in data.keys()
 }
+gt_bias = {
+    robot: imu_at_query_timestamps[robot]["imu_px4"][[
+        "gyro_bias.x", "gyro_bias.y", "gyro_bias.z", 
+        "accel_bias.x", "accel_bias.y", "accel_bias.z"
+    ]].to_numpy()
+    for robot in data.keys()
+}
 ```
 
 ## Extended Kalman Filter
@@ -187,32 +194,20 @@ for i in range(0, len(query_timestamps)):
 We can now evaluate the EKF using the ground truth data and plot the results. We first evaluate the EKF using the ground truth data and the EKF history, using the example-specific evaluation functions in the *models* module. 
 
 ```py
-analysis = model.EvaluateEKF(gt_se23, ekf_history, exp_name)
+analysis = model.EvaluateEKF(gt_se23, gt_bias, ekf_history, exp_name)
 ```
 
 Lastly, we call the following functions to plot the results and save the results to disk, and we are done!
 
 ```py
 analysis.plot_error()
 analysis.plot_poses()
-analysis.plot_biases()
+analysis.plot_bias_error()
 analysis.save_results()
 ```
 
-<p align="center">
-<img src="https://decargroup.github.io/miluv/assets/imu/1c_error_ifo001.png" alt="drawing" width="400" class="center"/>
-</p>
-
-![IMU EKF Pose Plot for Experiment #1c ifo001](https://decargroup.github.io/miluv/assets/imu/1c_poses_ifo001.png) | ![IMU EKF Bias Plot for Experiment #1c ifo001](https://decargroup.github.io/miluv/assets/imu/1c_biases_ifo001.png)
-
-<p align="center">
-<img src="https://decargroup.github.io/miluv/assets/imu/1c_error_ifo002.png" alt="drawing" width="400" class="center"/>
-</p>
-
-![IMU EKF Pose Plot for Experiment #1c ifo002](https://decargroup.github.io/miluv/assets/imu/1c_poses_ifo002.png) | ![IMU EKF Bias Plot for Experiment #1c ifo002](https://decargroup.github.io/miluv/assets/imu/1c_biases_ifo002.png)
+![IMU EKF Pose Plot for Experiment #1c ifo001](https://decargroup.github.io/miluv/assets/ekf_imu/1c_poses_ifo001.png) | ![IMU EKF Error Plot for Experiment #1c ifo001](https://decargroup.github.io/miluv/assets/ekf_imu/1c_error_ifo001.png)
 
-<p align="center">
-<img src="https://decargroup.github.io/miluv/assets/imu/1c_error_ifo003.png" alt="drawing" width="400" class="center"/>
-</p>
+![IMU EKF Pose Plot for Experiment #1c ifo002](https://decargroup.github.io/miluv/assets/ekf_imu/1c_poses_ifo002.png) | ![IMU EKF Error Plot for Experiment #1c ifo002](https://decargroup.github.io/miluv/assets/ekf_imu/1c_error_ifo002.png)
 
-![IMU EKF Pose Plot for Experiment #1c ifo003](https://decargroup.github.io/miluv/assets/imu/1c_poses_ifo003.png) | ![IMU EKF Bias Plot for Experiment #1c ifo003](https://decargroup.github.io/miluv/assets/imu/1c_biases_ifo003.png)
+![IMU EKF Pose Plot for Experiment #1c ifo003](https://decargroup.github.io/miluv/assets/ekf_imu/1c_poses_ifo003.png) | ![IMU EKF Error Plot for Experiment #1c ifo003](https://decargroup.github.io/miluv/assets/ekf_imu/1c_error_ifo003.png)

docs/examples/ekf/se3_one_robot.md

@@ -13,7 +13,7 @@ nav_order: 1
 
 This example shows he we can use MILUV to test out an Extended Kalman Filter (EKF) for a single robot using Visual-Inertial Navigation System (VINS) data. In this example, we will use the following data:
 
-- Gyroscope data from the robot's PX4 IMU, where we ignore the gyro bias for this example. 
+- Gyroscope data from the robot's PX4 IMU, where we remove the gyro bias for this example. 
 - VINS data, which uses the robot's camera and IMU to estimate the robot's velocity.
 - UWB range data between the 2 tags on the robot and the 6 anchors in the environment.
 - Height data from the robot's downward-facing laser rangefinder.
@@ -64,10 +64,10 @@ We start by defining the experiment we want to run the EKF on. In this case, we
 exp_name = "13"
 ```
 
-We then, in one line, load all the sensor data we want for our EKF. For this example, we only care about ifo001's data.
+We then, in one line, load all the sensor data we want for our EKF. For this example, we only care about ifo001's data, and we remove the IMU bias to simplify the EKF implementation.
 
 ```py
-miluv = DataLoader(exp_name, imu = "px4", cam = None, mag = False)
+miluv = DataLoader(exp_name, imu = "px4", cam = None, mag = False, remove_imu_bias = True)
 data = miluv.data["ifo001"]
 ```
 
@@ -296,4 +296,4 @@ analysis.plot_poses()
 analysis.save_results()
 ```
 
-![VINS EKF Pose Plot for Experiment #13](https://decargroup.github.io/miluv/assets/vins/13_poses.png) | ![VINS EKF Error Plot for Experiment #13](https://decargroup.github.io/miluv/assets/vins/13_error.png)
+![VINS EKF Pose Plot for Experiment #13](https://decargroup.github.io/miluv/assets/ekf_vins/13_poses.png) | ![VINS EKF Error Plot for Experiment #13](https://decargroup.github.io/miluv/assets/ekf_vins/13_error.png)

docs/examples/ekf/se3_three_robot.md

@@ -45,7 +45,7 @@ exp_name = "1c"
 We then, in one line, load all the sensor data we want for our EKF. We keep only the data for this example and get rid of the other functions in the `DataLoader` class.
 
 ```py
-miluv = DataLoader(exp_name, imu = "px4", cam = None, mag = False)
+miluv = DataLoader(exp_name, imu = "px4", cam = None, mag = False, remove_imu_bias = True)
 data = miluv.data
 ```
 
@@ -250,10 +250,10 @@ analysis.plot_poses()
 analysis.save_results()
 ```
 
-![VINS EKF Pose Plot for Experiment #1c ifo001](https://decargroup.github.io/miluv/assets/vins/1c_poses_ifo001.png) | ![VINS EKF Error Plot for Experiment #1c ifo001](https://decargroup.github.io/miluv/assets/vins/1c_error_ifo001.png)
+![VINS EKF Pose Plot for Experiment #1c ifo001](https://decargroup.github.io/miluv/assets/ekf_vins/1c_poses_ifo001.png) | ![VINS EKF Error Plot for Experiment #1c ifo001](https://decargroup.github.io/miluv/assets/ekf_vins/1c_error_ifo001.png)
 
-![VINS EKF Pose Plot for Experiment #1c ifo002](https://decargroup.github.io/miluv/assets/vins/1c_poses_ifo002.png) | ![VINS EKF Error Plot for Experiment #1c ifo002](https://decargroup.github.io/miluv/assets/vins/1c_error_ifo002.png)
+![VINS EKF Pose Plot for Experiment #1c ifo002](https://decargroup.github.io/miluv/assets/ekf_vins/1c_poses_ifo002.png) | ![VINS EKF Error Plot for Experiment #1c ifo002](https://decargroup.github.io/miluv/assets/ekf_vins/1c_error_ifo002.png)
 
-![VINS EKF Pose Plot for Experiment #1c ifo003](https://decargroup.github.io/miluv/assets/vins/1c_poses_ifo003.png) | ![VINS EKF Error Plot for Experiment #1c ifo003](https://decargroup.github.io/miluv/assets/vins/1c_error_ifo003.png)
+![VINS EKF Pose Plot for Experiment #1c ifo003](https://decargroup.github.io/miluv/assets/ekf_vins/1c_poses_ifo003.png) | ![VINS EKF Error Plot for Experiment #1c ifo003](https://decargroup.github.io/miluv/assets/ekf_vins/1c_error_ifo003.png)
 
 

docs/examples/losclassification.md

@@ -1,7 +1,7 @@
 ---
 title: LOS/NLOS Classification
 parent: Examples
-nav_order: 3
+nav_order: 4
 ---
 
 # LOS/NLOS Classification

docs/examples/visualizeimu.md

@@ -0,0 +1,130 @@
+---
+title: Visualize IMU
+parent: Examples
+nav_order: 2
+---
+
+# Visualize IMU data
+
+This example loads and visualizes IMU measurements and bias using the `miluv` development kit. We start by importing MILUV's `DataLoader` module, which is the core module for loading the MILUV dataset. We also import the `utils` module, which contains utility functions for processing the data.
+
+```py
+from miluv.data import DataLoader
+import miluv.utils as utils
+```
+
+Additionally, to visualize the data, we import the `matplotlib` library and set the grid to be visible in the plots by default.
+
+```py
+import matplotlib.pyplot as plt
+
+plt.rcParams['axes.grid'] = True
+```
+
+We set the experiment to be `1c` and the robot we want to visualize to be `ifo001`.
+
+```py
+exp_name = "1c"
+robot = "ifo001"
+```
+
+Then, as always, loading the data is as simple as instantiating the `DataLoader` class with the experiment name.
+
+```py
+miluv = DataLoader(exp_name, cam = None, mag = False)
+```
+
+We then keep the data only for the robot we are interested in.
+
+```py
+data = miluv.data[robot]
+```
+
+We extract the IMU data from the robot's data by simply accessing the `imu_px4` key.
+
+```py
+imu_px4 = data["imu_px4"]
+```
+
+We extract the ground truth position and orientation from the robot's data by accessing the `mocap_pos` and `mocap_quat` keys, which return splines that can be queried at the IMU timestamps.
+
+```py
+time = imu_px4["timestamp"]
+pos = data["mocap_pos"](time)
+quat = data["mocap_quat"](time)
+```
+
+The MILUV utilities module provides functions to extract the angular velocity and accelerometer measurements from the ground truth position and orientation splines. The `get_angular_velocity_splines()` function extracts the angular velocity in a straightforward manneer by fitting a spline to the first derivative of the orientation splines. Meanwhile, the `get_accelerometer_splines()` function extracts the accelerometer measurements by fitting a spline to the second derivative of the position splines, adding the contribution of gravity, and then resolving the specific force in the body frame using the orientation splines. These functions are also used under the hood to compute the ``ground truth'' bias for the IMU measurements.
+
+```py
+gt_gyro = utils.get_angular_velocity_splines(time, data["mocap_quat"])(time)
+gt_accelerometer = utils.get_accelerometer_splines(time, data["mocap_pos"], data["mocap_quat"])(time)
+```
+
+Plotting the IMU measurements is then straightforward. We first plot the angular velocity measurements from the IMU and the ground truth.
+
+```py
+axs[0].plot(time, imu_px4["angular_velocity.x"], label="IMU PX4 Measurement")
+axs[1].plot(time, imu_px4["angular_velocity.y"], label="IMU PX4 Measurement")
+axs[2].plot(time, imu_px4["angular_velocity.z"], label="IMU PX4 Measurement")
+
+axs[0].plot(time, gt_gyro[0, :], label="Ground Truth")
+axs[1].plot(time, gt_gyro[1, :], label="Ground Truth")
+axs[2].plot(time, gt_gyro[2, :], label="Ground Truth")
+```
+
+<p align="center">
+<img src="https://decargroup.github.io/miluv/assets/imu/gyro.png" alt="gyro" width="400" class="center"/>
+</p>
+
+We then plot the measurement error and the compute ground truth bias for the gyroscope measurements.
+
+```py
+axs[0].plot(time, gt_gyro[0, :] - imu_px4["angular_velocity.x"], label="Measurement Error")
+axs[1].plot(time, gt_gyro[1, :] - imu_px4["angular_velocity.y"], label="Measurement Error")
+axs[2].plot(time, gt_gyro[2, :] - imu_px4["angular_velocity.z"], label="Measurement Error")
+```
+
+```py
+axs[0].plot(time, imu_px4["gyro_bias.x"], label="IMU Bias")
+axs[1].plot(time, imu_px4["gyro_bias.y"], label="IMU Bias")
+axs[2].plot(time, imu_px4["gyro_bias.z"], label="IMU Bias")
+```
+
+<p align="center">
+<img src="https://decargroup.github.io/miluv/assets/imu/gyro_bias.png" alt="gyro_bias" width="400" class="center"/>
+</p>
+
+Similarly, we plot the accelerometer measurements, the ground truth, the measurement error, and the computed ground truth bias as follows.
+
+```py
+axs[0].plot(time, imu_px4["linear_acceleration.x"], label="IMU PX4 Measurement")
+axs[1].plot(time, imu_px4["linear_acceleration.y"], label="IMU PX4 Measurement")
+axs[2].plot(time, imu_px4["linear_acceleration.z"], label="IMU PX4 Measurement")
+```
+
+```py
+axs[0].plot(time, gt_accelerometer[0, :], label="Ground Truth")
+axs[1].plot(time, gt_accelerometer[1, :], label="Ground Truth")
+axs[2].plot(time, gt_accelerometer[2, :], label="Ground Truth")
+```
+
+<p align="center">
+<img src="https://decargroup.github.io/miluv/assets/imu/accel.png" alt="accel" width="400" class="center"/>
+</p>
+
+```py
+axs[0].plot(time, gt_accelerometer[0, :] - imu_px4["linear_acceleration.x"], label="Measurement Error")
+axs[1].plot(time, gt_accelerometer[1, :] - imu_px4["linear_acceleration.y"], label="Measurement Error")
+axs[2].plot(time, gt_accelerometer[2, :] - imu_px4["linear_acceleration.z"], label="Measurement Error")
+```
+
+```py
+axs[0].plot(time, imu_px4["accel_bias.x"], label="IMU Bias")
+axs[1].plot(time, imu_px4["accel_bias.y"], label="IMU Bias")
+axs[2].plot(time, imu_px4["accel_bias.z"], label="IMU Bias")
+```
+
+<p align="center">
+<img src="https://decargroup.github.io/miluv/assets/imu/accel_bias.png" alt="accel_bias" width="400" class="center"/>
+</p>