Attitude-Bias-Calibration Equivariant FilterĀ¶
This notebook demonstrates the Equivariant Filter (EqF) for attitude estimation with both gyroscope bias and sensor extrinsic calibration, based on the paper:
āOvercoming Bias: Equivariant Filter Design for Biased Attitude Estimation with Online Calibrationā by Fornasier et al.
The filter estimates:
Attitude (Rotation): The orientation of the body frame relative to a reference frame
Bias: Gyroscope bias correction vector (3D)
Calibration: Sensor calibration rotation matrices
This demo uses the AbcEquivariantFilter class which provides a clean interface for predict/update operations.
Note: We have hidden verbose loading/plotting functions under cell dropdowns. Feel free to expand to take a closer look.
Try it out in Colab!
Setup and ImportsĀ¶
First, we import all the necessary libraries. We use GTSAM for the filter implementation, NumPy for numerical operations, and Plotly for creating interactive visualizations. The dataclass decorator helps us create clean data structures for organizing measurements and results.
Notebook Cell
from __future__ import annotations
from dataclasses import dataclass
import csv
import math
from typing import List
import numpy as np
import plotly.graph_objects as go
from plotly.subplots import make_subplots
import gtsam
from gtsam import Rot3, Unit3
from gtsam.utils import findExampleDataFileData StructuresĀ¶
We define three key data structures:
MeasurementRecord: Stores a single direction measurement with its reference direction, covariance, and calibration index. Each measurement represents an observation of a known direction vector in the body frame.DataRecord: Contains all information for a single time step, including ground truth state (attitude, bias, calibration), gyroscope measurements, input covariance, and all direction measurements at that timestep.FilterResults: Aggregates the entire time series of ground truth, estimates, and errors for easy plotting. This allows us to track how the filter performs over the entire trajectory.
@dataclass
class MeasurementRecord:
y: np.ndarray
d: np.ndarray
R: np.ndarray
cal_idx: int
@dataclass
class DataRecord:
R: Rot3
b: np.ndarray
cal_rot: Rot3
omega: np.ndarray
input_covariance: np.ndarray
measurements: List[MeasurementRecord]
t: float
dt: float
@dataclass
class FilterResults:
"""Store filter results for plotting"""
times: List[float]
# Ground truth
truth_att_rpy: List[np.ndarray]
truth_bias: List[np.ndarray]
truth_cal_rpy: List[np.ndarray]
# Estimates
est_att_rpy: List[np.ndarray]
est_bias: List[np.ndarray]
est_cal_rpy: List[np.ndarray]
# Errors
att_errors: List[float]
bias_errors: List[float]
cal_errors: List[float]Data Loading FunctionsĀ¶
The CSV file contains simulated IMU and direction measurement data with ground truth. Each row includes quaternion representations of attitude and calibration, gyroscope measurements with noise parameters, and direction observations. The data loader parses this CSV format and converts it into our DataRecord structures, handling normalization of direction vectors and construction of covariance matrices from standard deviations.
Notebook Cell
def _normalize(v: np.ndarray) -> np.ndarray:
"""Normalize a vector to unit length."""
n = np.linalg.norm(v)
if n == 0.0:
return v
return v / n
def load_data_from_csv(
filename: str, start_row: int = 0, max_rows: int = -1, downsample: int = 1
) -> List[DataRecord]:
"""
Load data from CSV file into a list of DataRecord objects.
The CSV contains:
- Time stamps and ground truth state (attitude quaternion, bias, calibration quaternion)
- Gyroscope measurements (omega) with noise standard deviations
- Direction measurements from two sensors with their reference directions
- Measurement noise covariances
"""
data_list: List[DataRecord] = []
with open(filename, newline="") as csvfile:
reader = csv.reader(csvfile)
header = next(reader, None)
if header is None:
return data_list
line_number = 1
row_count = 0
prev_time = 0.0
for row in reader:
line_number += 1
if line_number < start_row:
continue
if ((line_number - start_row - 1) % downsample) != 0:
continue
if max_rows != -1 and row_count >= max_rows:
break
if len(row) < 39:
continue
values = [float(x) if x else 0.0 for x in row]
t = values[0]
dt = 0.0 if row_count == 0 else t - prev_time
prev_time = t
# Extract ground truth state
R = Rot3.Quaternion(values[1], values[2], values[3], values[4])
b = np.array([values[5], values[6], values[7]])
cal_rot = Rot3.Quaternion(values[8], values[9], values[10], values[11])
# Extract gyroscope input
omega = np.array([values[12], values[13], values[14]])
# Build input covariance (6x6: angular velocity + bias process noise)
input_covariance = np.zeros((6, 6))
input_covariance[0, 0] = values[15] ** 2
input_covariance[1, 1] = values[16] ** 2
input_covariance[2, 2] = values[17] ** 2
input_covariance[3, 3] = values[18] ** 2
input_covariance[4, 4] = values[19] ** 2
input_covariance[5, 5] = values[20] ** 2
measurements: List[MeasurementRecord] = []
# First measurement (calibrated sensor, cal_idx = 0)
y0 = _normalize(np.array([values[21], values[22], values[23]]))
d0 = _normalize(np.array([values[33], values[34], values[35]]))
cov_y0 = np.diag([values[27] ** 2, values[28] ** 2, values[29] ** 2])
measurements.append(MeasurementRecord(y0, d0, cov_y0, 0))
# Second measurement (uncalibrated sensor, cal_idx = -1)
y1 = _normalize(np.array([values[24], values[25], values[26]]))
d1 = _normalize(np.array([values[36], values[37], values[38]]))
cov_y1 = np.diag([values[30] ** 2, values[31] ** 2, values[32] ** 2])
measurements.append(MeasurementRecord(y1, d1, cov_y1, -1))
data_list.append(
DataRecord(
R=R,
b=b,
cal_rot=cal_rot,
omega=omega,
input_covariance=input_covariance,
measurements=measurements,
t=t,
dt=dt,
)
)
row_count += 1
return data_listFilter Processing with Results TrackingĀ¶
This function runs the ABC Equivariant Filter through the entire data sequence. For each time step, it performs a prediction step using gyroscope measurements, then updates with direction observations. The function tracks both the filter estimates and ground truth at each step, computing errors in attitude, bias, and calibration. All results are stored in a FilterResults object for later visualization.
def process_data_with_eqf(
filter_eqf: gtsam.abc.AbcEquivariantFilter1,
data_list: List[DataRecord],
) -> FilterResults:
"""
Process data through the filter and return results for plotting.
At each timestep:
1. Predict: Use gyroscope measurements to propagate the state forward
2. Update: Incorporate direction measurements to correct the estimate
3. Track: Store estimates, ground truth, and errors for visualization
"""
if not data_list:
print("No data to process")
return None
print(f"Processing {len(data_list)} data points with EqF...")
rad_to_deg = 180.0 / math.pi
# Storage for results
results = FilterResults(
times=[],
truth_att_rpy=[],
truth_bias=[],
truth_cal_rpy=[],
est_att_rpy=[],
est_bias=[],
est_cal_rpy=[],
att_errors=[],
bias_errors=[],
cal_errors=[],
)
total_measurements = 0
valid_measurements = 0
progress_step = max(len(data_list) // 10, 1)
print("Progress: ", end="", flush=True)
for i, data in enumerate(data_list):
# Predict step: propagate state using gyroscope measurements
filter_eqf.predict(data.omega, data.input_covariance, data.dt)
# Update step: correct state using direction measurements
for measurement in data.measurements:
total_measurements += 1
if np.any(np.isnan(measurement.y)) or np.any(np.isnan(measurement.d)):
continue
try:
y_unit = Unit3(measurement.y)
d_unit = Unit3(measurement.d)
filter_eqf.update(y_unit, d_unit, measurement.R, measurement.cal_idx)
valid_measurements += 1
except Exception:
continue
# Get current estimates using the wrapper's accessor methods
estimate_R = filter_eqf.attitude()
estimate_b = np.array(filter_eqf.bias()).reshape(3)
estimate_cal = filter_eqf.calibration(0)
# Calculate errors (in Lie algebra for rotations)
att_error = Rot3.Logmap(data.R.between(estimate_R))
bias_error = estimate_b - data.b
cal_error = Rot3.Logmap(data.cal_rot.between(estimate_cal))
# Store results for plotting
results.times.append(data.t)
results.truth_att_rpy.append(data.R.rpy() * rad_to_deg)
results.truth_bias.append(data.b)
results.truth_cal_rpy.append(data.cal_rot.rpy() * rad_to_deg)
results.est_att_rpy.append(estimate_R.rpy() * rad_to_deg)
results.est_bias.append(estimate_b)
results.est_cal_rpy.append(estimate_cal.rpy() * rad_to_deg)
results.att_errors.append(np.linalg.norm(att_error) * rad_to_deg)
results.bias_errors.append(np.linalg.norm(bias_error))
results.cal_errors.append(np.linalg.norm(cal_error) * rad_to_deg)
if i % progress_step == 0:
print(".", end="", flush=True)
print(" Done!")
# Print summary statistics
avg_att_error = float(np.mean(results.att_errors))
avg_bias_error = float(np.mean(results.bias_errors))
avg_cal_error = float(np.mean(results.cal_errors))
print("\n=== Filter Performance Summary ===")
print(f"Processed measurements: {total_measurements} (valid: {valid_measurements})")
print("\n-- Average Errors --")
print(f"Attitude: {avg_att_error}Ā°")
print(f"Bias: {avg_bias_error}")
print(f"Calibration: {avg_cal_error}Ā°")
print("\n-- Final Errors --")
print(f"Attitude: {results.att_errors[-1]}Ā°")
print(f"Bias: {results.bias_errors[-1]}")
print(f"Calibration: {results.cal_errors[-1]}Ā°")
print("\n-- Final State vs Ground Truth --")
print(f"Attitude (RPY) - Estimate: {results.est_att_rpy[-1]} Ā° | Truth: {results.truth_att_rpy[-1]} Ā°")
print(f"Bias - Estimate: {results.est_bias[-1]} | Truth: {results.truth_bias[-1]}")
print(f"Calibration (RPY) - Estimate: {results.est_cal_rpy[-1]} Ā° | Truth: {results.truth_cal_rpy[-1]} Ā°")
return resultsVisualization FunctionsĀ¶
These functions create interactive Plotly visualizations of the filter results. Each function generates time-series plots comparing ground truth (solid lines) with filter estimates (dashed lines).
Notebook Cell
def plot_attitude_time_series(results: FilterResults):
"""
Plot attitude (roll, pitch, yaw) over time.
Creates a 3-subplot figure showing each Euler angle component.
Solid lines show ground truth, dashed lines show filter estimates.
This helps visualize how well the filter tracks the true orientation.
"""
fig = make_subplots(
rows=3, cols=1,
subplot_titles=("Roll", "Pitch", "Yaw"),
vertical_spacing=0.08,
x_title="Time (s)",
)
times = results.times
truth_rpy = np.array(results.truth_att_rpy)
est_rpy = np.array(results.est_att_rpy)
labels = ["Roll", "Pitch", "Yaw"]
colors_truth = ["rgb(200, 60, 60)", "rgb(60, 200, 60)", "rgb(60, 60, 200)"]
colors_est = ["rgb(255, 120, 120)", "rgb(120, 255, 120)", "rgb(120, 120, 255)"]
for i in range(3):
# Ground truth
fig.add_trace(
go.Scatter(
x=times, y=truth_rpy[:, i],
name=f"{labels[i]} (Truth)",
line=dict(color=colors_truth[i], width=2),
showlegend=(i == 0),
),
row=i+1, col=1
)
# Estimate
fig.add_trace(
go.Scatter(
x=times, y=est_rpy[:, i],
name=f"{labels[i]} (Estimate)",
line=dict(color=colors_est[i], width=2, dash='dash'),
showlegend=(i == 0),
),
row=i+1, col=1
)
fig.update_yaxes(title_text=f"{labels[i]} (Ā°)", row=i+1, col=1)
fig.update_layout(
height=800,
title_text="Attitude Estimation: Roll-Pitch-Yaw over Time",
showlegend=True,
legend=dict(orientation="h", yanchor="bottom", y=1.02, xanchor="right", x=1),
)
return fig
def plot_bias_time_series(results: FilterResults):
"""
Plot gyroscope bias over time.
Shows all three bias components (X, Y, Z) on a single plot.
The bias represents systematic errors in the gyroscope measurements that the filter
must estimate and correct. Watch how the estimates converge to the true bias values.
"""
fig = go.Figure()
times = results.times
truth_bias = np.array(results.truth_bias)
est_bias = np.array(results.est_bias)
labels = ["Bias X", "Bias Y", "Bias Z"]
colors_truth = ["rgb(200, 60, 60)", "rgb(60, 200, 60)", "rgb(60, 60, 200)"]
colors_est = ["rgb(255, 120, 120)", "rgb(120, 255, 120)", "rgb(120, 120, 255)"]
for i in range(3):
# Ground truth
fig.add_trace(
go.Scatter(
x=times, y=truth_bias[:, i],
name=f"{labels[i]} (Truth)",
line=dict(color=colors_truth[i], width=2),
)
)
# Estimate
fig.add_trace(
go.Scatter(
x=times, y=est_bias[:, i],
name=f"{labels[i]} (Estimate)",
line=dict(color=colors_est[i], width=2, dash='dash'),
)
)
fig.update_layout(
height=500,
title_text="Gyroscope Bias Estimation over Time",
xaxis_title="Time (s)",
yaxis_title="Bias (rad/s)",
showlegend=True,
legend=dict(orientation="h", yanchor="bottom", y=1.02, xanchor="right", x=1),
)
return fig
def plot_calibration_time_series(results: FilterResults):
"""
Plot sensor calibration over time.
Creates a 3-subplot figure showing the calibration rotation in Roll-Pitch-Yaw.
Calibration represents the rotation between the sensor frame and body frame,
which must be estimated online. Notice how the filter converges to the true
calibration parameters even while the body is rotating.
"""
fig = make_subplots(
rows=3, cols=1,
subplot_titles=("Cal Roll", "Cal Pitch", "Cal Yaw"),
vertical_spacing=0.08,
x_title="Time (s)",
)
times = results.times
truth_cal = np.array(results.truth_cal_rpy)
est_cal = np.array(results.est_cal_rpy)
labels = ["Roll", "Pitch", "Yaw"]
colors_truth = ["rgb(200, 60, 60)", "rgb(60, 200, 60)", "rgb(60, 60, 200)"]
colors_est = ["rgb(255, 120, 120)", "rgb(120, 255, 120)", "rgb(120, 120, 255)"]
for i in range(3):
# Ground truth
fig.add_trace(
go.Scatter(
x=times, y=truth_cal[:, i],
name=f"{labels[i]} (Truth)",
line=dict(color=colors_truth[i], width=2),
showlegend=(i == 0),
),
row=i+1, col=1
)
# Estimate
fig.add_trace(
go.Scatter(
x=times, y=est_cal[:, i],
name=f"{labels[i]} (Estimate)",
line=dict(color=colors_est[i], width=2, dash='dash'),
showlegend=(i == 0),
),
row=i+1, col=1
)
fig.update_yaxes(title_text=f"{labels[i]} (Ā°)", row=i+1, col=1)
fig.update_layout(
height=800,
title_text="Sensor Calibration Estimation over Time",
showlegend=True,
legend=dict(orientation="h", yanchor="bottom", y=1.02, xanchor="right", x=1),
)
return fig
def plot_errors_time_series(results: FilterResults):
"""
Plot estimation errors over time.
Shows the norm of attitude and calibration errors in degrees.
Lower values indicate better tracking performance. Spikes in error may occur
when measurements are sparse or during rapid maneuvers.
"""
fig = go.Figure()
times = results.times
fig.add_trace(
go.Scatter(
x=times, y=results.att_errors,
name="Attitude Error",
line=dict(color="rgb(200, 60, 60)", width=2),
)
)
fig.add_trace(
go.Scatter(
x=times, y=results.cal_errors,
name="Calibration Error",
line=dict(color="rgb(60, 60, 200)", width=2),
)
)
fig.update_layout(
height=500,
title_text="Estimation Errors over Time",
xaxis_title="Time (s)",
yaxis_title="Error (Ā°)",
showlegend=True,
)
return fig
def plot_bias_error_time_series(results: FilterResults):
"""
Plot bias estimation error over time.
Shows the norm of the bias error in rad/s with a filled area for emphasis.
The bias typically converges more slowly than attitude because it relies on
integrating information over time. The shaded area helps visualize the overall
convergence trend.
"""
fig = go.Figure()
times = results.times
fig.add_trace(
go.Scatter(
x=times, y=results.bias_errors,
name="Bias Error",
line=dict(color="rgb(60, 200, 60)", width=2),
fill='tozeroy',
fillcolor='rgba(60, 200, 60, 0.2)',
)
)
fig.update_layout(
height=400,
title_text="Bias Estimation Error over Time",
xaxis_title="Time (s)",
yaxis_title="Error (rad/s)",
showlegend=False,
)
return figLoad DataĀ¶
Here we locate and load the example dataset (EqFdata.csv). This CSV file contains simulated IMU trajectory data with ground truth, generated from a realistic motion profile. The data includes approximately 12,000 timesteps with gyroscope measurements and direction observations from two sensors (one calibrated, one uncalibrated). If you want to test with your own data, simply provide the path to a CSV file in the same format.
Notebook Cell
print("ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo")
print("==============================================================")
# Load data using GTSAM's dataset utility
try:
csv_file_path = findExampleDataFile("EqFdata.csv")
print(f"Loading data from: {csv_file_path}")
except Exception as e:
print(f"Error: Could not find EqFdata.csv: {e}")
csv_file_path = None
if csv_file_path:
data = load_data_from_csv(csv_file_path)
print(f"Loaded {len(data)} data points")
else:
print("No data available to process.")
data = []ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo
==============================================================
Loading data from: /Users/apollo/dev/research/gtsam/build/python/gtsam/Data/EqFdata.csv
Loaded 12001 data points
Initialize and Run FilterĀ¶
Now we create and configure the ABC Equivariant Filter. The initial covariance matrix initial_sigma encodes our uncertainty in the initial state: weāre moderately uncertain about the attitude (0.1 radĀ²) and calibration (0.1 radĀ²), but quite confident about the initial bias (0.01 radĀ²/sĀ²). The filter starts at the identity state (zero attitude, zero bias, identity calibration) and will converge to the true state as it processes measurements. This cell runs the filter through the entire trajectory, which takes a few seconds for 12,000 timesteps.
if data:
# Define initial covariance (uncertainty in the initial state)
n_cal = 1 # Number of calibrated sensors
initial_sigma = np.eye(6 + 3 * n_cal)
initial_sigma[0:3, 0:3] = 0.1 * np.eye(3) # Attitude uncertainty (radĀ²)
initial_sigma[3:6, 3:6] = 0.01 * np.eye(3) # Bias uncertainty (radĀ²/sĀ²)
initial_sigma[6:9, 6:9] = 0.1 * np.eye(3) # Calibration uncertainty (radĀ²)
# Create filter instance (starts at identity state)
filter_eqf = gtsam.abc.AbcEquivariantFilter1(initial_sigma)
# Process all data through the filter
results = process_data_with_eqf(filter_eqf, data)
print("\nFilter processing completed successfully!")
else:
results = None
print("Skipping filter processing - no data loaded.")Processing 12001 data points with EqF...
Progress: ........... Done!
=== Filter Performance Summary ===
Processed measurements: 24002 (valid: 4923)
-- Average Errors --
Attitude: 1.1308383647121234Ā°
Bias: 0.0075498876180193865
Calibration: 0.957466838125976Ā°
-- Final Errors --
Attitude: 3.662018753470247Ā°
Bias: 0.0020652584435729942
Calibration: 0.3643824462229628Ā°
-- Final State vs Ground Truth --
Attitude (RPY) - Estimate: [-13.80445039 -0.64271376 86.74437182] Ā° | Truth: [-17.40672752 -1.04149482 86.27016675] Ā°
Bias - Estimate: [0.00468984 0.00994432 0.00194401] | Truth: [0.00399696 0.00799933 0.00189684]
Calibration (RPY) - Estimate: [24.81667419 5.05822205 29.67403453] Ā° | Truth: [25. 5. 30.] Ā°
Filter processing completed successfully!
Visualize ResultsĀ¶
Attitude Estimation (Roll-Pitch-Yaw)Ā¶
This plot shows how the filter estimates the bodyās orientation over time. The three subplots display Roll (rotation about X-axis), Pitch (rotation about Y-axis), and Yaw (rotation about Z-axis). Solid lines represent ground truth, while dashed lines show the filterās estimates. Notice how closely the estimates track the true values even during dynamic maneuvers. You can zoom into specific time regions and hover over the curves to see exact values at each timestep.
if results:
fig = plot_attitude_time_series(results)
fig.show()Gyroscope Bias EstimationĀ¶
This plot displays the three components of gyroscope bias (systematic errors in angular velocity measurements). The filter must simultaneously estimate the attitude while learning these constant biases. Notice how the bias estimates converge from their initial values (zero) toward the true bias values. The convergence rate depends on the richness of the motion - more diverse rotations provide better observability of the bias. The darker solid lines are ground truth, while lighter dashed lines are the filterās estimates.
if results:
fig = plot_bias_time_series(results)
fig.show()Sensor Calibration EstimationĀ¶
These three subplots show the online estimation of sensor calibration parameters, represented as Roll-Pitch-Yaw angles. Calibration represents the fixed rotation between the sensor frame and the body frame, which is unknown at startup. The equivariant filter can estimate this calibration simultaneously with attitude and bias. Watch how the calibration estimates (dashed lines) converge to the true calibration (solid lines) within the first portion of the trajectory, demonstrating the filterās ability to handle online calibration.
if results:
fig = plot_calibration_time_series(results)
fig.show()Estimation ErrorsĀ¶
These error plots quantify the filterās performance by showing the magnitude of estimation errors over time. The red curve shows attitude error (in degrees), while the blue curve shows calibration error. Both errors start high when the filter has little information, then converge to low values as the filter processes more measurements. Temporary spikes may occur during aggressive maneuvers or measurement dropouts. The average errors across the entire trajectory are printed in the summary above.
if results:
fig = plot_errors_time_series(results)
fig.show()Bias Estimation ErrorĀ¶
This final plot focuses specifically on the gyroscope bias estimation error. The shaded area under the curve emphasizes the overall error magnitude. Bias estimation typically exhibits a characteristic convergence pattern: starting from the initial uncertainty, the error decreases as the filter observes more motion. The rate of convergence depends on the trajectoryās observability properties. Richer motion (diverse rotations) leads to faster convergence. The final bias error shown in the summary above indicates how accurately the filter has identified the true gyroscope bias.
if results:
fig = plot_bias_error_time_series(results)
fig.show()