Add basic imu calibrator
This provides a ceres-based solver to take in values from multiple IMUs
and estimate the relative orientations and time offsets of the IMUs.
It still would need to be hooked up to an AOS log, and doesn't have
great hooks for visualizing the results.
Change-Id: Ifcc69a89b9e4b0957b207a8f62948cd87ebf9fa0
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/imu/BUILD b/frc971/imu/BUILD
index b90f844..3bccf20 100644
--- a/frc971/imu/BUILD
+++ b/frc971/imu/BUILD
@@ -61,3 +61,46 @@
uf2_from_elf(
name = "ADIS16505",
)
+
+cc_library(
+ name = "imu_calibrator",
+ srcs = ["imu_calibrator.cc"],
+ hdrs = [
+ "imu_calibrator.h",
+ "imu_calibrator-tmpl.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/time",
+ "//frc971/math:interpolate",
+ "@com_google_absl//absl/strings:str_format",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "imu_calibrator_solver",
+ srcs = [
+ "imu_calibrator_solver.cc",
+ ],
+ hdrs = [
+ "imu_calibrator_solver.h",
+ ],
+ deps = [
+ ":imu_calibrator",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "imu_calibrator_test",
+ srcs = ["imu_calibrator_test.cc"],
+ shard_count = 3,
+ deps = [
+ ":imu_calibrator",
+ ":imu_calibrator_solver",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/frc971/imu/imu_calibrator-tmpl.h b/frc971/imu/imu_calibrator-tmpl.h
new file mode 100644
index 0000000..6743106
--- /dev/null
+++ b/frc971/imu/imu_calibrator-tmpl.h
@@ -0,0 +1,248 @@
+#include "frc971/imu/imu_calibrator.h"
+#include "frc971/math/interpolate.h"
+
+DECLARE_int32(imu_zeroing_buffer);
+
+namespace frc971::imu {
+
+inline constexpr double kGravityGs = 1.0;
+// rad / sec
+inline constexpr double kGyroMaxZeroingValue = 0.1;
+
+template <typename Scalar>
+void ImuCalibrator<Scalar>::InsertImu(size_t imu_index,
+ const RawImuReading &reading) {
+ CHECK_LT(imu_index, imu_readings_.size());
+ std::vector<ImuReading> &readings = imu_readings_[imu_index];
+ if (readings.size() > 0u) {
+ CHECK_LT(readings.back().capture_time_raw, reading.capture_time)
+ << ": Readings must be inserted in time order per IMU.";
+ }
+ // Execute the stationary logic. We identify if this reading is plausibly
+ // stationary, then if it is not stationary, we go back in time to any
+ // potentially relevant readings and mark them as not stationary. Finally, we
+ // go through and as values exit the FLAGS_imu_zeroing_buffer window we do any
+ // processing that we can given that we know it must be stationary.
+ const bool plausibly_stationary =
+ reading.gyro.squaredNorm() < kGyroMaxZeroingValue * kGyroMaxZeroingValue;
+ bool stationary = plausibly_stationary;
+ int earliest_affected_index = readings.size() - FLAGS_imu_zeroing_buffer;
+ for (size_t index = std::max(0, earliest_affected_index);
+ index < readings.size(); ++index) {
+ if (!plausibly_stationary) {
+ readings[index].stationary = false;
+ }
+ if (!readings[index].plausibly_stationary) {
+ stationary = false;
+ }
+ }
+
+ // Since we don't have data from before the start, assume that we may have
+ // been moving.
+ if (earliest_affected_index < 0) {
+ stationary = false;
+ }
+
+ if (earliest_affected_index >= 0) {
+ ImuReading &earliest_reading = readings[earliest_affected_index];
+ // The stationary flag for this reading can no longer change, so we can
+ // start to do things based on it.
+ earliest_reading.stationary_is_locked = true;
+ if (earliest_reading.stationary) {
+ earliest_reading.parameter_residuals.gravity =
+ earliest_reading.accel.norm() - kGravityGs;
+ earliest_reading.parameter_residuals.gyro_zero = earliest_reading.gyro;
+ LOG(INFO) << earliest_reading.gyro.transpose();
+ }
+ }
+
+ const ImuConfig<Scalar> &config = imu_configs_[imu_index];
+ Scalar capture_time_adjusted =
+ static_cast<Scalar>(aos::time::DurationInSeconds(
+ reading.capture_time.time_since_epoch())) -
+ (config.parameters.has_value() ? config.parameters->time_offset
+ : static_cast<Scalar>(0.0));
+
+ imu_readings_[imu_index].emplace_back(
+ reading.capture_time, capture_time_adjusted,
+ reading.gyro - config.dynamic_params.gyro_zero,
+ reading.accel / config.dynamic_params.gravity,
+ DynamicImuParameters<Scalar>{static_cast<Scalar>(0.0),
+ Eigen::Matrix<Scalar, 3, 1>::Zero()},
+ plausibly_stationary, stationary, false, std::nullopt, std::nullopt);
+}
+
+template <typename Scalar>
+void ImuCalibrator<Scalar>::EvaluateRelativeResiduals() {
+ for (const auto &readings : imu_readings_) {
+ CHECK_LT(static_cast<size_t>(FLAGS_imu_zeroing_buffer * 2), readings.size())
+ << ": Insufficient readings to perform calibration.";
+ }
+ Scalar base_clock = imu_readings_[origin_index_][0].capture_time_adjusted;
+ // Current index corresponding to the base_clock time.
+ std::vector<size_t> reading_indices(imu_configs_.size(), 0);
+ // The for loops are set up so that we:
+ // 1. Iterate over every pair of readings from the origin/base IMU.
+ // 2. For each other IMU, we identify 0 or 1 readings that fall between those
+ // two readings of the origin IMU. We then calculate the residuals for
+ // that IMU relative to the origin IMU, linearly interpolating between
+ // the pair of readings from (1) (by doing a linear interpolation, we can
+ // get sub-cycle accuracy on time offsets).
+ for (;
+ reading_indices[origin_index_] < imu_readings_[origin_index_].size() - 1;
+ ++reading_indices[origin_index_]) {
+ const ImuReading &base_reading =
+ imu_readings_[origin_index_][reading_indices[origin_index_]];
+ const ImuReading &next_base_reading =
+ imu_readings_[origin_index_][reading_indices[origin_index_] + 1];
+ base_clock = base_reading.capture_time_adjusted;
+ const Scalar next_base_clock = next_base_reading.capture_time_adjusted;
+ for (size_t imu_index = 0; imu_index < imu_configs_.size(); ++imu_index) {
+ const ImuConfig<Scalar> &imu_config = imu_configs_[imu_index];
+ // We don't care about calculating the offsets from the origin IMU to
+ // itself...
+ if (imu_config.is_origin) {
+ continue;
+ }
+ auto &readings = imu_readings_[imu_index];
+ bool done_with_imu = false;
+ // This will put the index for the current IMU just past the base_clock
+ // timepoint, allowing us to interpolate between
+ // reading_indices[origin_index_] and reading_indices[origin_index_] + 1.
+ while (readings[reading_indices[imu_index]].capture_time_adjusted <
+ base_clock) {
+ if (reading_indices[imu_index] == imu_readings_[imu_index].size() - 1) {
+ done_with_imu = true;
+ break;
+ }
+ ++reading_indices[imu_index];
+ }
+ // If we've run out of readings on this imu, stop doing anything.
+ if (done_with_imu) {
+ continue;
+ }
+ ImuReading &reading = readings[reading_indices[imu_index]];
+ const Scalar reading_time = reading.capture_time_adjusted;
+ if (reading_time >= next_base_clock) {
+ // There is a gap in readings for this imu; we can't meaningfully
+ // populate the residuals.
+ continue;
+ }
+ // Sanity check the above logic.
+ CHECK_LE(base_clock, reading_time);
+ CHECK_LT(reading_time, next_base_clock);
+ CHECK(imu_config.parameters.has_value());
+ reading.gyro_residual =
+ imu_config.parameters.value().rotation * reading.gyro -
+ frc971::math::Interpolate<Eigen::Matrix<Scalar, 3, 1>, Scalar>(
+ base_clock, next_base_clock, base_reading.gyro,
+ next_base_reading.gyro, reading_time);
+ if (!reading.stationary_is_locked || !reading.stationary) {
+ continue;
+ }
+ // In order to calculate the accelerometer residual, we are assuming that
+ // the two IMUs "should" produce identical accelerations. This is only
+ // true when not rotating. Future changes may account for coriolis
+ // effects.
+ reading.accel_residual =
+ imu_config.parameters.value().rotation * reading.accel -
+ frc971::math::Interpolate(base_clock, next_base_clock,
+ base_reading.accel, next_base_reading.accel,
+ reading_time);
+ }
+ }
+}
+
+// Helpers to accommodate serializing residuals into the ceres buffer. These
+// helpers all return a buffer that points to the next value to be populated.
+namespace internal {
+template <typename Scalar>
+std::span<Scalar> SerializeScalar(Scalar value, std::span<Scalar> out) {
+ DCHECK(!out.empty());
+ out[0] = value;
+ return out.subspan(1);
+}
+template <typename Scalar, int kSize>
+std::span<Scalar> SerializeVector(const Eigen::Matrix<Scalar, kSize, 1> &value,
+ std::span<Scalar> out) {
+ DCHECK_LE(static_cast<size_t>(value.size()), out.size());
+ for (int index = 0; index < kSize; ++index) {
+ out[index] = value(index);
+ }
+ return out.subspan(kSize);
+}
+template <typename Scalar>
+std::span<Scalar> SerializeParams(const DynamicImuParameters<Scalar> ¶ms,
+ std::span<Scalar> out) {
+ return SerializeVector(params.gyro_zero,
+ SerializeScalar(params.gravity, out));
+}
+inline constexpr int kResidualsPerReading = 10u;
+} // namespace internal
+
+template <typename Scalar>
+void ImuCalibrator<Scalar>::CalculateResiduals(std::span<Scalar> residuals) {
+ EvaluateRelativeResiduals();
+ for (size_t imu_index = 0; imu_index < imu_configs_.size(); ++imu_index) {
+ const auto &readings = imu_readings_[imu_index];
+ double valid_gyro_reading_count = 0;
+ double valid_accel_reading_count = 0;
+ for (size_t reading_index = 0; reading_index < readings.size();
+ ++reading_index) {
+ const auto &reading = readings[reading_index];
+ if (reading.gyro_residual.has_value()) {
+ ++valid_gyro_reading_count;
+ }
+ if (reading.accel_residual.has_value()) {
+ ++valid_accel_reading_count;
+ }
+ }
+ if (!imu_configs_[imu_index].is_origin) {
+ CHECK_LT(0, valid_gyro_reading_count);
+ CHECK_LT(0, valid_accel_reading_count);
+ } else {
+ valid_gyro_reading_count = readings.size();
+ valid_accel_reading_count = readings.size();
+ }
+ // Adjust the residuals of the readings to ensure that the solver doesn't
+ // cheat by just making it so that the time-offsets are completely
+ // misaligned and we can say that all the residuals are "zero".
+ const Scalar gyro_reading_scalar =
+ static_cast<Scalar>(readings.size() / valid_gyro_reading_count);
+ const Scalar accel_reading_scalar =
+ static_cast<Scalar>(readings.size() / valid_accel_reading_count);
+ for (size_t reading_index = 0; reading_index < readings.size();
+ ++reading_index) {
+ const auto &reading = readings[reading_index];
+ const Scalar *const start_residual = residuals.data();
+ // 4 residuals (gravity scalar; gyro zeroes)
+ residuals =
+ internal::SerializeParams(reading.parameter_residuals, residuals);
+ const Eigen::Matrix<Scalar, 3, 1> gyro_residual =
+ reading.gyro_residual.value_or(Eigen::Matrix<Scalar, 3, 1>::Zero()) *
+ gyro_reading_scalar;
+ // 3 residuals
+ residuals = internal::SerializeVector(gyro_residual, residuals);
+ const Eigen::Matrix<Scalar, 3, 1> accel_residual =
+ reading.accel_residual.value_or(Eigen::Matrix<Scalar, 3, 1>::Zero()) *
+ accel_reading_scalar;
+ // 3 residuals
+ residuals = internal::SerializeVector(accel_residual, residuals);
+ CHECK_EQ(internal::kResidualsPerReading,
+ residuals.data() - start_residual)
+ << ": Need to update kResidualsPerReading.";
+ }
+ }
+}
+
+template <typename Scalar>
+size_t ImuCalibrator<Scalar>::CalculateNumResiduals(
+ const std::vector<size_t> &num_readings) {
+ size_t num_residuals = 0;
+ for (const size_t count : num_readings) {
+ num_residuals += internal::kResidualsPerReading * count;
+ }
+ return num_residuals;
+}
+
+} // namespace frc971::imu
diff --git a/frc971/imu/imu_calibrator.cc b/frc971/imu/imu_calibrator.cc
new file mode 100644
index 0000000..03fa377
--- /dev/null
+++ b/frc971/imu/imu_calibrator.cc
@@ -0,0 +1,6 @@
+#include "frc971/imu/imu_calibrator.h"
+
+DEFINE_int32(
+ imu_zeroing_buffer, 100,
+ "We will only consider readings stationary for purposes if calibration if "
+ "this many readings to either side appear to be stationary.");
diff --git a/frc971/imu/imu_calibrator.h b/frc971/imu/imu_calibrator.h
new file mode 100644
index 0000000..be4c4d4
--- /dev/null
+++ b/frc971/imu/imu_calibrator.h
@@ -0,0 +1,331 @@
+#ifndef FRC971_IMU_IMU_CALIBRATOR_H_
+#define FRC971_IMU_IMU_CALIBRATOR_H_
+
+#include <optional>
+#include <span>
+#include <tuple>
+#include <vector>
+
+#include "absl/strings/str_format.h"
+#include "absl/strings/str_join.h"
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "aos/time/time.h"
+
+namespace frc971::imu {
+
+// Contains a reading that comes directly from an IMU.
+// These should not be zeroed or corrected for yet.
+struct RawImuReading {
+ aos::monotonic_clock::time_point capture_time;
+ // gyro readings are in radians / sec; accelerometer readings are in g's.
+ const Eigen::Vector3d gyro;
+ const Eigen::Vector3d accel;
+};
+
+// Note on how we manage populating ceres parameters:
+// * We use dynamically-sized parameter lists for convenience.
+// * For every struct that we have that corresponds to problem parameters,
+// there is a PopulateParameters() method that takes a:
+// * ceres::Problem* that is used for setting parameter constraints.
+// * ceres::DynamicCostFunction* that is used to mark that we have added
+// a parameter block.
+// * A std::vector<double*> that will later be passed to AddResidualBlock.
+// We add any parameter blocks which we add to the problem.
+// * post_populate_methods is a list of std::function's that will get called
+// after everything is populated and added (this is necessary because
+// we cannot add constraints to the Problem until after everything
+// has been added).
+// * Additionally, there is a PopulateFromParameters() method which is used
+// in the ceres cost functor and:
+// * Takes a parameters double-pointer where parameters[X] is the
+// parameter block X.
+// * Returns a pair where the first value is a populated instance of the
+// struct and an updated parameters pointer which points to the next
+// set of pointers.
+// * All the Populate* methods must be called in the same order every
+// time so that they result in the raw pointers getting populated
+// consistently.
+
+// These are the parameters corresponding to things which will vary at every
+// power-on of the IMU.
+template <typename Scalar>
+struct DynamicImuParameters {
+ // A scalar to represent the current magnitude of gravity, in g's.
+ // This currently compensates both for any variations in local gravity as well
+ // as for some amount of variations in the IMU itself. In the future we may
+ // expand this to have a local gravity number that is global to all IMUs while
+ // separately calibrating per-axis information.
+ Scalar gravity;
+ // Current gyro zero, in radians / sec.
+ // These are the per-axis values that the gyro will read when it is sitting
+ // still. Technically these zeroes will drift over time; however, the
+ // time-windows that we expect to use for calibration are short enough that
+ // this should be a non-issue.
+ Eigen::Matrix<Scalar, 3, 1> gyro_zero;
+ void PopulateParameters(
+ ceres::Problem *problem, ceres::DynamicCostFunction *cost_function,
+ std::vector<double *> *parameters,
+ std::vector<std::function<void()>> *post_populate_methods) {
+ cost_function->AddParameterBlock(1);
+ parameters->push_back(&gravity);
+ cost_function->AddParameterBlock(3);
+ parameters->push_back(gyro_zero.data());
+ post_populate_methods->emplace_back([this, problem]() {
+ // Gravity shouldn't vary by much (these bounds are significantly larger
+ // than any real variations which we will experience).
+ problem->SetParameterLowerBound(&gravity, 0, 0.95);
+ problem->SetParameterUpperBound(&gravity, 0, 1.05);
+ for (int i = 0; i < 3; ++i) {
+ problem->SetParameterLowerBound(gyro_zero.data(), i, -0.05);
+ problem->SetParameterUpperBound(gyro_zero.data(), i, 0.05);
+ }
+ });
+ }
+ static std::tuple<DynamicImuParameters<Scalar>, const Scalar *const *>
+ PopulateFromParameters(const Scalar *const *parameters) {
+ const Scalar *const gravity = parameters[0];
+ ++parameters;
+ const Scalar *const gyro = parameters[0];
+ ++parameters;
+ return std::make_tuple(
+ DynamicImuParameters<Scalar>{
+ *gravity, Eigen::Matrix<Scalar, 3, 1>(gyro[0], gyro[1], gyro[2])},
+ parameters);
+ }
+ std::string ToString() const {
+ std::stringstream out;
+ out << "gravity: " << gravity << " gyro_zero: " << gyro_zero.transpose();
+ return out.str();
+ }
+};
+
+// These parameters correspond to IMU parameters which will not vary between
+// boots. Namely, the relative positions and time offsets of the IMU(s).
+template <typename Scalar>
+struct StaticImuParameters {
+ // If you have a point p_imu in this IMU's frame then (rotation *
+ // p_imu + position) will give you that point's position in the board frame
+ // (the "board" frame refers to the frame associated with the entire PCB,
+ // where the PCB itself contains multiple IMUs. The board frame will be
+ // attached to whichever IMU is being treated as the origin).
+ Eigen::Quaternion<Scalar> rotation;
+ // position is currently unused because it is only observeable when there
+ // are coriolis effects, and we currently only make use of accelerometer
+ // readings from when the IMU is sat still.
+ // As such, we currently assume that all position offsets are zero.
+ // Eigen::Matrix<Scalar, 3, 1> position;
+ // The "true" time at which the event occurred is equal to the capture time -
+ // time_offset. I.e., a more positive time offset implies that the there is a
+ // large delay between the imu readings being taken and us observing them.
+ Scalar time_offset;
+
+ void PopulateParameters(
+ ceres::EigenQuaternionParameterization *quaternion_local_parameterization,
+ ceres::Problem *problem, ceres::DynamicCostFunction *cost_function,
+ std::vector<double *> *parameters,
+ std::vector<std::function<void()>> *post_populate_methods) {
+ cost_function->AddParameterBlock(4);
+ parameters->push_back(rotation.coeffs().data());
+ cost_function->AddParameterBlock(1);
+ parameters->push_back(&time_offset);
+ post_populate_methods->emplace_back(
+ [this, problem, quaternion_local_parameterization]() {
+ problem->SetParameterization(rotation.coeffs().data(),
+ quaternion_local_parameterization);
+ problem->SetParameterLowerBound(&time_offset, 0, -0.03);
+ problem->SetParameterUpperBound(&time_offset, 0, 0.03);
+ });
+ }
+ static std::tuple<StaticImuParameters, const Scalar *const *>
+ PopulateFromParameters(const Scalar *const *parameters) {
+ const Scalar *const quat = parameters[0];
+ ++parameters;
+ const Scalar *const time = parameters[0];
+ ++parameters;
+ return std::make_tuple(
+ StaticImuParameters{
+ Eigen::Quaternion<Scalar>(quat[3], quat[0], quat[1], quat[2]),
+ *time},
+ parameters);
+ }
+ std::string ToString() const {
+ std::stringstream out;
+ out << "quat: " << rotation.coeffs().transpose()
+ << " time_offset: " << time_offset;
+ return out.str();
+ }
+};
+
+// Represents the calibration for a single IMU.
+template <typename Scalar>
+struct ImuConfig {
+ // Set to true if this IMU is to be considered the origin of the coordinate
+ // system. This will also mean that this IMU is treated as the source-of-truth
+ // for clock offsets.
+ bool is_origin;
+ // Will be nullopt if is_origin is true (the corresponding rotations and
+ // offsets will all be the identity/zero as appropriate).
+ std::optional<StaticImuParameters<Scalar>> parameters;
+
+ DynamicImuParameters<Scalar> dynamic_params{
+ .gravity = static_cast<Scalar>(1.0),
+ .gyro_zero = Eigen::Matrix<Scalar, 3, 1>::Zero()};
+ std::string ToString() const {
+ return absl::StrFormat(
+ "is_origin: %d params: %s dynamic: %s", is_origin,
+ parameters.has_value() ? parameters->ToString() : std::string("<null>"),
+ dynamic_params.ToString());
+ }
+};
+
+// Represents all of the configuration parameters for the entire system.
+template <typename Scalar>
+struct AllParameters {
+ // Each entry in the imus vector will be a single imu.
+ std::vector<ImuConfig<Scalar>> imus;
+ std::tuple<std::vector<double *>, std::vector<std::function<void()>>>
+ PopulateParameters(
+ ceres::EigenQuaternionParameterization *quaternion_local_parameterization,
+ ceres::Problem *problem, ceres::DynamicCostFunction *cost_function) {
+ std::vector<std::function<void()>> post_populate_methods;
+ std::vector<double *> parameters;
+ for (auto &imu : imus) {
+ if (imu.parameters.has_value()) {
+ imu.parameters.value().PopulateParameters(
+ quaternion_local_parameterization, problem, cost_function,
+ ¶meters, &post_populate_methods);
+ }
+ imu.dynamic_params.PopulateParameters(problem, cost_function, ¶meters,
+ &post_populate_methods);
+ }
+ return std::make_tuple(parameters, post_populate_methods);
+ }
+ static AllParameters PopulateFromParameters(
+ const std::vector<ImuConfig<double>> &nominal_configs,
+ const Scalar *const *parameters) {
+ std::vector<ImuConfig<Scalar>> result;
+ for (size_t imu_index = 0; imu_index < nominal_configs.size();
+ ++imu_index) {
+ ImuConfig<Scalar> config;
+ config.is_origin = nominal_configs[imu_index].is_origin;
+ if (!config.is_origin) {
+ std::tie(config.parameters, parameters) =
+ StaticImuParameters<Scalar>::PopulateFromParameters(parameters);
+ }
+ std::tie(config.dynamic_params, parameters) =
+ DynamicImuParameters<Scalar>::PopulateFromParameters(parameters);
+ result.emplace_back(std::move(config));
+ }
+ return {.imus = std::move(result)};
+ }
+ std::string ToString() const {
+ std::vector<std::string> imu_strings;
+ for (const auto &imu : imus) {
+ std::vector<std::string> dynamic_params;
+ imu_strings.push_back(absl::StrFormat("config: %s", imu.ToString()));
+ }
+ return absl::StrJoin(imu_strings, "\n");
+ }
+};
+
+// This class does the hard work to support calibrating multiple IMU's
+// orientations relative to one another. It is set up to readily be used with a
+// ceres solver (see imu_calibrator.*).
+//
+// The current theory of operation is to have some number of imus, one of which
+// we will consider to be fixed in position. We have a fixed set of data that we
+// feed into this class, which can be parameterized based on:
+// * The current zeroes for each IMU.
+// * The orientation of each non-fixed IMU.
+// * The time-offset of each non-fixed IMU.
+//
+// When run under ceres, ceres can then vary these parameters to solve for the
+// current calibrations of the IMUs.
+//
+// When solving, the main complexity is that some internal state has to be
+// tracked to try to determine when we should be calculating zeroes and when we
+// can calibrate out the magnitude of gravity. This is done by tracking when the
+// IMUs are "stationary". For a reading to be stationary, all values with
+// FLAGS_imu_zeroing_buffer readings of the reading must be "plausibly
+// stationary". Readings are plausibly stationary if they have sufficiently low
+// gyro values.
+//
+// TODO: Provide some utilities to plot the results of a calibration.
+template <typename Scalar>
+class ImuCalibrator {
+ public:
+ ImuCalibrator(const std::vector<ImuConfig<Scalar>> &imu_configs)
+ : imu_configs_(imu_configs), imu_readings_(imu_configs.size()) {
+ origin_index_ = -1;
+ for (size_t imu_index = 0; imu_index < imu_configs_.size(); ++imu_index) {
+ if (imu_configs_[imu_index].is_origin) {
+ CHECK_EQ(origin_index_, -1)
+ << ": Can't have more than one IMU specified as the origin.";
+ origin_index_ = imu_index;
+ }
+ }
+ CHECK_NE(origin_index_, -1)
+ << ": Must have at least one IMU specified as the origin.";
+ }
+
+ // These gyro readings will be "raw"---i.e., they still need to get
+ // transformed by nominal_transform.
+ // gyro readings are in radians / sec; accelerometer readings are in g's.
+ // Within a given imu, must be called in time order.
+ void InsertImu(size_t imu_index, const RawImuReading &reading);
+
+ // Populates all the residuals that we use for the cost in ceres.
+ void CalculateResiduals(std::span<Scalar> residuals);
+
+ // Returns the total number of residuals that this problem will have, given
+ // the number of readings for each IMU.
+ static size_t CalculateNumResiduals(const std::vector<size_t> &num_readings);
+
+ private:
+ // Represents an imu reading. The values in this are generally already
+ // adjusted for the provided parameters.
+ struct ImuReading {
+ // The "actual" provided capture time. Used for debugging.
+ aos::monotonic_clock::time_point capture_time_raw;
+ // The capture time, adjusted for this IMU's time offset.
+ Scalar capture_time_adjusted;
+ // gyro reading, adjusted for gyro zero but NOT rotation.
+ // In radians / sec.
+ Eigen::Matrix<Scalar, 3, 1> gyro;
+ // accelerometer reading, adjusted for gravity value but NOT rotation.
+ // In g's.
+ Eigen::Matrix<Scalar, 3, 1> accel;
+ // Residuals associated with the DynamicImuParameters for this imu.
+ DynamicImuParameters<Scalar> parameter_residuals;
+ // Set if this measurement *could* be part of a segment of time where the
+ // IMU is stationary.
+ bool plausibly_stationary;
+ // Set to true if all values with FLAGS_imu_zeroing_buffer of this reading
+ // are plausibly_stationary.
+ bool stationary;
+ // We set stationary_is_locked once we have enough readings to either side
+ // of this value to guarantee that it is stationary.
+ bool stationary_is_locked;
+ // Residuals that are used for calibrating the rotation values. These are
+ // nullopt if we can't calibrate for some reason (e.g., this is the origin
+ // IMU, or for the accelerometer residual, we don't populate it if we are
+ // not stationary).
+ std::optional<Eigen::Matrix<Scalar, 3, 1>> gyro_residual;
+ std::optional<Eigen::Matrix<Scalar, 3, 1>> accel_residual;
+ };
+ void EvaluateRelativeResiduals();
+
+ const std::vector<ImuConfig<Scalar>> imu_configs_;
+ // Index of the IMU which is the origin IMU.
+ int origin_index_;
+ std::vector<std::vector<ImuReading>> imu_readings_;
+};
+
+} // namespace frc971::imu
+
+#include "frc971/imu/imu_calibrator-tmpl.h"
+#endif // FRC971_IMU_IMU_CALIBRATOR_H_
diff --git a/frc971/imu/imu_calibrator_solver.cc b/frc971/imu/imu_calibrator_solver.cc
new file mode 100644
index 0000000..19b82e5
--- /dev/null
+++ b/frc971/imu/imu_calibrator_solver.cc
@@ -0,0 +1,89 @@
+#include "frc971/imu/imu_calibrator_solver.h"
+namespace frc971::imu {
+
+struct ImuCalibratorCostFunctor {
+ ImuCalibratorCostFunctor(
+ const std::vector<std::vector<RawImuReading>> &readings,
+ const std::vector<ImuConfig<double>> &nominal_config,
+ const size_t num_residuals)
+ : readings_(readings),
+ nominal_config_(nominal_config),
+ num_residuals_(num_residuals) {}
+ template <typename S>
+ bool operator()(const S *const *const parameters_ptr, S *residual) const {
+ AllParameters<S> params = AllParameters<S>::PopulateFromParameters(
+ nominal_config_, parameters_ptr);
+ std::vector<ImuConfig<S>> imu_configs;
+ for (const auto ¶m : params.imus) {
+ imu_configs.push_back(param);
+ }
+ ImuCalibrator<S> calibrator(imu_configs);
+ for (size_t imu_index = 0; imu_index < readings_.size(); ++imu_index) {
+ const auto imu_readings = readings_[imu_index];
+ for (size_t reading_index = 0; reading_index < imu_readings.size();
+ ++reading_index) {
+ calibrator.InsertImu(imu_index, imu_readings[reading_index]);
+ }
+ }
+ calibrator.CalculateResiduals({residual, num_residuals_});
+ return true;
+ }
+ const std::vector<std::vector<RawImuReading>> readings_;
+ const std::vector<ImuConfig<double>> nominal_config_;
+ const size_t num_residuals_;
+};
+
+AllParameters<double> Solve(
+ const std::vector<std::vector<RawImuReading>> &readings,
+ const std::vector<ImuConfig<double>> &nominal_config) {
+ ceres::Problem problem;
+
+ ceres::EigenQuaternionParameterization *quaternion_local_parameterization =
+ new ceres::EigenQuaternionParameterization();
+ AllParameters<double> parameters;
+ std::vector<size_t> num_readings;
+ CHECK_EQ(nominal_config.size(), readings.size());
+ for (size_t imu_index = 0; imu_index < nominal_config.size(); ++imu_index) {
+ const size_t num_params = readings[imu_index].size();
+ parameters.imus.emplace_back(nominal_config[imu_index]);
+ num_readings.push_back(num_params);
+ }
+ // Set up the only cost function (also known as residual). This uses
+ // auto-differentiation to obtain the derivative (jacobian).
+
+ {
+ const size_t num_residuals =
+ ImuCalibrator<double>::CalculateNumResiduals(num_readings);
+ ceres::DynamicCostFunction *cost_function =
+ new ceres::DynamicAutoDiffCostFunction<ImuCalibratorCostFunctor>(
+ new ImuCalibratorCostFunctor(readings, nominal_config,
+ num_residuals));
+
+ auto [vector_parameters, post_populate_methods] =
+ parameters.PopulateParameters(quaternion_local_parameterization,
+ &problem, cost_function);
+
+ cost_function->SetNumResiduals(num_residuals);
+
+ problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0),
+ vector_parameters);
+ for (auto &method : post_populate_methods) {
+ method();
+ }
+ }
+
+ // Run the solver!
+ ceres::Solver::Options options;
+ options.minimizer_progress_to_stdout = true;
+ options.gradient_tolerance = 1e-12;
+ options.function_tolerance = 1e-6;
+ options.parameter_tolerance = 1e-6;
+ ceres::Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ LOG(INFO) << summary.FullReport();
+ LOG(INFO) << "Solution is " << (summary.IsSolutionUsable() ? "" : "NOT ")
+ << "usable";
+ LOG(INFO) << "Solution:\n" << parameters.ToString();
+ return parameters;
+}
+} // namespace frc971::imu
diff --git a/frc971/imu/imu_calibrator_solver.h b/frc971/imu/imu_calibrator_solver.h
new file mode 100644
index 0000000..148fcc4
--- /dev/null
+++ b/frc971/imu/imu_calibrator_solver.h
@@ -0,0 +1,14 @@
+#ifndef FRC971_IMU_IMU_CALIBRATOR_SOLVER_H_
+#define FRC971_IMU_IMU_CALIBRATOR_SOLVER_H_
+
+#include "frc971/imu/imu_calibrator.h"
+
+namespace frc971::imu {
+
+// Stores all the IMU data from a log so that we can feed it into the
+// ImuCalibrator readily.
+AllParameters<double> Solve(
+ const std::vector<std::vector<RawImuReading>> &readings,
+ const std::vector<ImuConfig<double>> &nominal_config);
+} // namespace frc971::imu
+#endif // FRC971_IMU_IMU_CALIBRATOR_SOLVER_H_
diff --git a/frc971/imu/imu_calibrator_test.cc b/frc971/imu/imu_calibrator_test.cc
new file mode 100644
index 0000000..a341b2a
--- /dev/null
+++ b/frc971/imu/imu_calibrator_test.cc
@@ -0,0 +1,215 @@
+#include "frc971/imu/imu_calibrator.h"
+
+#include <random>
+
+#include "gtest/gtest.h"
+
+#include "frc971/imu/imu_calibrator_solver.h"
+
+namespace frc971::imu::testing {
+class ImuSimulator {
+ public:
+ ImuSimulator(const std::vector<ImuConfig<double>> &imu_configs)
+ : imu_configs_(imu_configs), readings_(imu_configs.size()) {}
+ void SimulateForTime(aos::monotonic_clock::duration duration,
+ aos::monotonic_clock::duration dt,
+ const Eigen::Vector3d &gravity_vector,
+ const Eigen::Vector3d &accel,
+ const Eigen::Vector3d &gyro) {
+ for (const aos::monotonic_clock::time_point end_time = now + duration;
+ now < end_time; now += dt) {
+ for (size_t imu_index = 0; imu_index < imu_configs_.size(); ++imu_index) {
+ const ImuConfig<double> &config = imu_configs_[imu_index];
+ const Eigen::Quaterniond rotation =
+ config.is_origin ? Eigen::Quaterniond::Identity()
+ : config.parameters->rotation.inverse();
+ const std::chrono::nanoseconds time_offset{
+ config.is_origin
+ ? 0
+ : static_cast<uint64_t>(config.parameters->time_offset * 1e9)};
+ readings_[imu_index].emplace_back(
+ now + time_offset,
+ rotation * gyro + config.dynamic_params.gyro_zero + GyroNoise(),
+ rotation *
+ (accel + gravity_vector * config.dynamic_params.gravity) +
+ AccelNoise());
+ }
+ }
+ }
+ Eigen::Vector3d GyroNoise() {
+ return (enable_noise_ ? 1.0 : 0.0) *
+ Eigen::Vector3d(distribution_(generator_), distribution_(generator_),
+ distribution_(generator_));
+ }
+ Eigen::Vector3d AccelNoise() { return GyroNoise() * 2.0; }
+ void set_enable_noise(bool enable) { enable_noise_ = enable; }
+ std::vector<std::vector<RawImuReading>> readings() const {
+ return readings_;
+ };
+
+ private:
+ const std::vector<ImuConfig<double>> imu_configs_;
+ std::vector<std::vector<RawImuReading>> readings_;
+ aos::monotonic_clock::time_point now = aos::monotonic_clock::epoch();
+
+ std::mt19937 generator_;
+ std::normal_distribution<> distribution_{0.0, 0.00025};
+ bool enable_noise_ = false;
+};
+
+namespace {
+void VerifyParameters(const std::vector<ImuConfig<double>> &imus,
+ const AllParameters<double> ¶ms, double eps = 1e-8) {
+ ASSERT_EQ(imus.size(), params.imus.size());
+ for (size_t imu_index = 0; imu_index < imus.size(); ++imu_index) {
+ SCOPED_TRACE(imu_index);
+ const ImuConfig<double> &expected = imus[imu_index];
+ const ImuConfig<double> &calculated = params.imus[imu_index];
+ EXPECT_EQ(expected.parameters.has_value(),
+ calculated.parameters.has_value());
+ EXPECT_NEAR(expected.dynamic_params.gravity,
+ calculated.dynamic_params.gravity, eps);
+ EXPECT_LT((expected.dynamic_params.gyro_zero -
+ calculated.dynamic_params.gyro_zero)
+ .norm(),
+ eps)
+ << expected.dynamic_params.gyro_zero.transpose() << " vs. "
+ << calculated.dynamic_params.gyro_zero.transpose();
+ if (expected.parameters.has_value()) {
+ EXPECT_NEAR(expected.parameters->time_offset,
+ calculated.parameters->time_offset, eps);
+ EXPECT_LT(((expected.parameters->rotation *
+ calculated.parameters->rotation.inverse())
+ .coeffs() -
+ Eigen::Quaterniond::Identity().coeffs())
+ .norm(),
+ eps)
+ << expected.parameters->rotation.coeffs().transpose() << " vs. "
+ << calculated.parameters->rotation.coeffs().transpose();
+ }
+ }
+}
+} // namespace
+
+// Confirms that we can calibrate in a relatively simple scenario where we have
+// some gyro/accelerometer offsets and a small rotation that is not accounted
+// for in the nominal parameters.
+TEST(ImuCalibratorTest, BasicCalibrationTest) {
+ std::vector<ImuConfig<double>> nominal_imus = {
+ ImuConfig<double>{true, std::nullopt},
+ ImuConfig<double>{false, std::make_optional<StaticImuParameters<double>>(
+ Eigen::Quaterniond::Identity(), 0.0)}};
+
+ std::vector<ImuConfig<double>> real_imus = nominal_imus;
+ real_imus[0].dynamic_params.gravity = 1.005;
+ real_imus[0].dynamic_params.gyro_zero << 0.001, 0.002, 0.003;
+ real_imus[1].dynamic_params.gyro_zero << -0.009, -0.007, -0.001;
+ real_imus[1].parameters->rotation =
+ Eigen::AngleAxisd(0.01, Eigen::Vector3d::UnitZ());
+ ImuSimulator simulator(real_imus);
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 1, 0), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(
+ std::chrono::seconds(1), std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0.1, 0.2, 0.3),
+ Eigen::Vector3d(1.0, 0.0, 0.0));
+ simulator.SimulateForTime(
+ std::chrono::seconds(1), std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0.1, 0.2, 0.3),
+ Eigen::Vector3d(0.0, 1.0, 0.0));
+ auto params = Solve(simulator.readings(), nominal_imus);
+ LOG(INFO) << params.ToString();
+ LOG(INFO) << real_imus[0].ToString();
+ LOG(INFO) << real_imus[1].ToString();
+ VerifyParameters(real_imus, params);
+}
+
+// Separately test the estimation of the time offset between IMUs.
+// This is done separately because the optimization problem is poorly condition
+// for estimating time offsets when there is just a handful of step changes in
+// the IMU inputs; feeding in a sine wave works much better for allowing the
+// solver to estimate the offset.
+TEST(ImuCalibratorTest, TimeOffsetTest) {
+ gflags::FlagSaver flag_saver;
+
+ std::vector<ImuConfig<double>> nominal_imus = {
+ ImuConfig<double>{true, std::nullopt},
+ ImuConfig<double>{false, std::make_optional<StaticImuParameters<double>>(
+ Eigen::Quaterniond::Identity(), 0.0)}};
+
+ std::vector<ImuConfig<double>> real_imus = nominal_imus;
+ real_imus[1].parameters->time_offset = 0.0255;
+ ImuSimulator simulator(real_imus);
+ // Note on convergence: It is easy to end up in situations where the problem
+ // is not outstandingly well conditioned and we can end up with local minima
+ // where changes to physical calibration attributes can explain the time
+ // offset.
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ for (size_t ii = 0; ii < 10000; ++ii) {
+ simulator.SimulateForTime(
+ std::chrono::milliseconds(1), std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0.0, 0.0, 0.0),
+ Eigen::Vector3d(std::sin(ii / 1000.0), 0.0, 0.0));
+ }
+ auto params = Solve(simulator.readings(), nominal_imus);
+ LOG(INFO) << params.ToString();
+ LOG(INFO) << real_imus[0].ToString();
+ LOG(INFO) << real_imus[1].ToString();
+ VerifyParameters(real_imus, params, 1e-6);
+}
+
+// Test that if we add in some random noise that the solver still behaves
+// itself.
+TEST(ImuCalibratorTest, RandomNoise) {
+ std::vector<ImuConfig<double>> nominal_imus = {
+ ImuConfig<double>{true, std::nullopt},
+ ImuConfig<double>{false, std::make_optional<StaticImuParameters<double>>(
+ Eigen::Quaterniond::Identity(), 0.0)}};
+
+ std::vector<ImuConfig<double>> real_imus = nominal_imus;
+ real_imus[0].dynamic_params.gravity = 0.999;
+ real_imus[0].dynamic_params.gyro_zero << 0.001, 0.002, 0.003;
+ real_imus[1].dynamic_params.gyro_zero << -0.009, -0.007, -0.001;
+ real_imus[1].parameters->rotation =
+ Eigen::AngleAxisd(0.01, Eigen::Vector3d::UnitZ());
+ ImuSimulator simulator(real_imus);
+ simulator.set_enable_noise(true);
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 1, 0), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ for (size_t ii = 0; ii < 6000; ++ii) {
+ simulator.SimulateForTime(std::chrono::milliseconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1),
+ Eigen::Vector3d(std::sin(ii / 1000.0), 0.0, 0.0),
+ Eigen::Vector3d(1.0, 0.0, 0.0));
+ }
+ auto params = Solve(simulator.readings(), nominal_imus);
+ LOG(INFO) << params.ToString();
+ LOG(INFO) << real_imus[0].ToString();
+ LOG(INFO) << real_imus[1].ToString();
+ VerifyParameters(real_imus, params, 1e-4);
+}
+} // namespace frc971::imu::testing