Improve gyro zeroing
Instead of using a single large data chunk for zeroing, collect multiple
shorter samples that we then gradually accumulate.
Change-Id: I5496a7c47a703764fcd0f5b1498ef080e9b30fe9
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index a2b908b..75d610a 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -35,6 +35,8 @@
],
deps = [
":averager",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/wpilib:imu_fbs",
"@com_github_google_glog//:glog",
"@org_tuxfamily_eigen//:eigen",
diff --git a/frc971/zeroing/averager.h b/frc971/zeroing/averager.h
index b6ff09d..23a1b06 100644
--- a/frc971/zeroing/averager.h
+++ b/frc971/zeroing/averager.h
@@ -19,6 +19,11 @@
class Averager {
public:
typedef Eigen::Matrix<Scalar, rows_per_sample, 1> Vector;
+ Averager() {
+ for (Vector &datum : data_) {
+ datum.setZero();
+ }
+ }
// Adds one data point to the set of data points to be averaged.
// If more than "num_samples" samples are added, they will start overwriting
// the oldest ones.
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index 25bf6f6..b13b43d 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -4,22 +4,30 @@
ImuZeroer::ImuZeroer() {
gyro_average_.setZero();
+ accel_average_.setZero();
last_gyro_sample_.setZero();
last_accel_sample_.setZero();
}
-bool ImuZeroer::Zeroed() const { return zeroed_ || Faulted(); }
+bool ImuZeroer::Zeroed() const {
+ return num_zeroes_ > kRequiredZeroPoints || Faulted();
+}
bool ImuZeroer::Faulted() const { return faulted_; }
Eigen::Vector3d ImuZeroer::ZeroedGyro() const {
return last_gyro_sample_ - gyro_average_;
}
-Eigen::Vector3d ImuZeroer::ZeroedAccel() const { return last_accel_sample_; }
+Eigen::Vector3d ImuZeroer::ZeroedAccel() const {
+ return last_accel_sample_ - accel_average_;
+}
Eigen::Vector3d ImuZeroer::GyroOffset() const { return gyro_average_; }
bool ImuZeroer::GyroZeroReady() const {
- return gyro_averager_.full() && gyro_averager_.GetRange() < kGyroMaxVariation;
+ return gyro_averager_.full() &&
+ gyro_averager_.GetRange() < kGyroMaxVariation &&
+ (last_gyro_sample_.lpNorm<Eigen::Infinity>() <
+ kGyroMaxZeroingMagnitude);
}
bool ImuZeroer::AccelZeroReady() const {
@@ -34,19 +42,45 @@
values.accelerometer_z();
accel_averager_.AddData(last_accel_sample_);
if (GyroZeroReady() && AccelZeroReady()) {
- if (!zeroed_) {
- zeroed_ = true;
- gyro_average_ = gyro_averager_.GetAverage();
- } else {
- // If we got a new zero and it is substantially different from the
- // original zero, fault.
- if ((gyro_averager_.GetAverage() - gyro_average_).norm() >
- kGyroFaultVariation) {
- faulted_ = true;
+ ++good_iters_;
+ if (good_iters_ > kSamplesToAverage / 4) {
+ const Eigen::Vector3d current_gyro_average = gyro_averager_.GetAverage();
+ constexpr double kAverageUpdateWeight = 0.05;
+ if (num_zeroes_ > 0) {
+ gyro_average_ +=
+ (current_gyro_average - gyro_average_) * kAverageUpdateWeight;
+ } else {
+ gyro_average_ = current_gyro_average;
}
+ if (num_zeroes_ > 0) {
+ // If we got a new zero and it is substantially different from the
+ // original zero, fault.
+ if ((current_gyro_average - gyro_average_).norm() >
+ kGyroFaultVariation) {
+ faulted_ = true;
+ }
+ }
+ ++num_zeroes_;
+ gyro_averager_.Reset();
}
- gyro_averager_.Reset();
+ } else {
+ good_iters_ = 0;
}
}
+flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState>
+ImuZeroer::PopulateStatus(flatbuffers::FlatBufferBuilder *fbb) const {
+ control_loops::drivetrain::ImuZeroerState::Builder builder(*fbb);
+
+ builder.add_zeroed(Zeroed());
+ builder.add_faulted(Faulted());
+ builder.add_number_of_zeroes(num_zeroes_);
+
+ builder.add_gyro_x_average(GyroOffset().x());
+ builder.add_gyro_y_average(GyroOffset().y());
+ builder.add_gyro_z_average(GyroOffset().z());
+
+ return builder.Finish();
+}
+
} // namespace frc971::zeroing
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index 2662934..b55c6dd 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -1,6 +1,7 @@
#ifndef FRC971_ZEROING_IMU_ZEROER_H_
#define FRC971_ZEROING_IMU_ZEROER_H_
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/wpilib/imu_generated.h"
#include "frc971/zeroing/averager.h"
@@ -15,7 +16,8 @@
// constant number of samples...
// TODO(james): Run average and GetRange calculations over every sample on
// every timestep, to provide consistent timing.
- static constexpr size_t kSamplesToAverage = 1000.0;
+ static constexpr size_t kSamplesToAverage = 1000;
+ static constexpr size_t kRequiredZeroPoints = 10;
ImuZeroer();
bool Zeroed() const;
@@ -24,14 +26,22 @@
Eigen::Vector3d GyroOffset() const;
Eigen::Vector3d ZeroedGyro() const;
Eigen::Vector3d ZeroedAccel() const;
+
+ flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
private:
// Max variation (difference between the maximum and minimum value) in a
// kSamplesToAverage range before we allow using the samples for zeroing.
// These values are currently based on looking at results from the ADIS16448.
- static constexpr double kGyroMaxVariation = 0.02; // rad / sec
+ static constexpr double kGyroMaxVariation = 0.02; // rad / sec
+ // Maximum magnitude we allow the gyro zero to have--this is used to prevent
+ // us from zeroing the gyro if we just happen to be spinning at a very
+ // consistent non-zero rate. Currently this is only plausible in simulation.
+ static constexpr double kGyroMaxZeroingMagnitude = 0.1; // rad / sec
// Max variation in the range before we consider the accelerometer readings to
// be steady.
- static constexpr double kAccelMaxVariation = 0.02; // g's
+ static constexpr double kAccelMaxVariation = 0.05; // g's
// If we ever are able to rezero and get a zero that is more than
// kGyroFaultVariation away from the original zeroing, fault.
static constexpr double kGyroFaultVariation = 0.005; // rad / sec
@@ -46,12 +56,13 @@
Averager<double, kSamplesToAverage, 3> accel_averager_;
// The average zero position of the gyro.
Eigen::Vector3d gyro_average_;
+ Eigen::Vector3d accel_average_;
Eigen::Vector3d last_gyro_sample_;
Eigen::Vector3d last_accel_sample_;
- // Whether we have currently zeroed yet.
- bool zeroed_ = false;
// Whether the zeroing has faulted at any point thus far.
bool faulted_ = false;
+ size_t good_iters_ = 0;
+ size_t num_zeroes_ = 0;
};
} // namespace frc971::zeroing
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index 9ec52da..7a74413 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -4,6 +4,9 @@
namespace frc971::zeroing {
+static constexpr int kMinSamplesToZero =
+ 2 * ImuZeroer::kSamplesToAverage * ImuZeroer::kRequiredZeroPoints;
+
aos::FlatbufferDetachedBuffer<IMUValues> MakeMeasurement(
const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel) {
flatbuffers::FlatBufferBuilder fbb;
@@ -29,13 +32,14 @@
ASSERT_EQ(0.0, zeroer.ZeroedAccel().norm());
// A measurement before we are zeroed should just result in the measurement
// being passed through without modification.
- zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
ASSERT_FALSE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
- ASSERT_EQ(1.0, zeroer.ZeroedGyro().x());
- ASSERT_EQ(2.0, zeroer.ZeroedGyro().y());
- ASSERT_EQ(3.0, zeroer.ZeroedGyro().z());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
+ ASSERT_FLOAT_EQ(0.02, zeroer.ZeroedGyro().y());
+ ASSERT_FLOAT_EQ(0.03, zeroer.ZeroedGyro().z());
ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
@@ -45,16 +49,16 @@
TEST(ImuZeroerTest, ZeroOnConstantData) {
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
- ASSERT_FALSE(zeroer.Zeroed());
- zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
// Gyro should be zeroed to {1, 2, 3}.
- ASSERT_EQ(1.0, zeroer.GyroOffset().x());
- ASSERT_EQ(2.0, zeroer.GyroOffset().y());
- ASSERT_EQ(3.0, zeroer.GyroOffset().z());
+ ASSERT_FLOAT_EQ(0.01, zeroer.GyroOffset().x());
+ ASSERT_FLOAT_EQ(0.02, zeroer.GyroOffset().y());
+ ASSERT_FLOAT_EQ(0.03, zeroer.GyroOffset().z());
ASSERT_EQ(0.0, zeroer.ZeroedGyro().x());
ASSERT_EQ(0.0, zeroer.ZeroedGyro().y());
ASSERT_EQ(0.0, zeroer.ZeroedGyro().z());
@@ -64,11 +68,25 @@
ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
// If we get another measurement offset by {1, 1, 1} we should read the result
// as {1, 1, 1}.
- zeroer.ProcessMeasurement(MakeMeasurement({2, 3, 4}, {0, 0, 0}).message());
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_EQ(1.0, zeroer.ZeroedGyro().x());
- ASSERT_EQ(1.0, zeroer.ZeroedGyro().y());
- ASSERT_EQ(1.0, zeroer.ZeroedGyro().z());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().y());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().z());
+}
+
+// Tests that we do not zero if the gyro is producing particularly high
+// magnitude results.
+TEST(ImuZeroerTest, NoZeroOnHighMagnitudeGyro) {
+ ImuZeroer zeroer;
+ ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.1, 0.2, 0.3}, {4, 5, 6}).message());
+ ASSERT_FALSE(zeroer.Zeroed());
+ }
+ ASSERT_FALSE(zeroer.Faulted());
}
// Tests that we tolerate small amounts of noise in the incoming data and can
@@ -76,29 +94,27 @@
TEST(ImuZeroerTest, ZeroOnLowNoiseData) {
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
- ASSERT_FALSE(zeroer.Zeroed());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
const double offset =
- (static_cast<double>(ii) / (ImuZeroer::kSamplesToAverage - 1) - 0.5) *
- 0.01;
+ (static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 0.001;
zeroer.ProcessMeasurement(
- MakeMeasurement({1 + offset, 2 + offset, 3 + offset},
+ MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
{4 + offset, 5 + offset, 6 + offset})
.message());
}
ASSERT_TRUE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
- // Gyro should be zeroed to {1, 2, 3}.
- ASSERT_NEAR(1.0, zeroer.GyroOffset().x(), 1e-8);
- ASSERT_NEAR(2.0, zeroer.GyroOffset().y(), 1e-8);
- ASSERT_NEAR(3.0, zeroer.GyroOffset().z(), 1e-8);
- // If we get another measurement offset by {1, 1, 1} we should read the result
- // as {1, 1, 1}.
- zeroer.ProcessMeasurement(MakeMeasurement({2, 3, 4}, {0, 0, 0}).message());
+ ASSERT_NEAR(0.01, zeroer.GyroOffset().x(), 1e-3);
+ ASSERT_NEAR(0.02, zeroer.GyroOffset().y(), 1e-3);
+ ASSERT_NEAR(0.03, zeroer.GyroOffset().z(), 1e-3);
+ // If we get another measurement offset by {0.01, 0.01, 0.01} we should read
+ // the result as {0.01, 0.01, 0.01}.
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().x(), 1e-8);
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().y(), 1e-8);
- ASSERT_NEAR(1.0, zeroer.ZeroedGyro().z(), 1e-8);
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().x(), 1e-3);
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().y(), 1e-3);
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().z(), 1e-3);
ASSERT_EQ(0.0, zeroer.ZeroedAccel().x());
ASSERT_EQ(0.0, zeroer.ZeroedAccel().y());
ASSERT_EQ(0.0, zeroer.ZeroedAccel().z());
@@ -108,13 +124,12 @@
TEST(ImuZeroerTest, NoZeroOnHighNoiseData) {
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
ASSERT_FALSE(zeroer.Zeroed());
const double offset =
- (static_cast<double>(ii) / (ImuZeroer::kSamplesToAverage - 1) - 0.5) *
- 1.0;
+ (static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 1.0;
zeroer.ProcessMeasurement(
- MakeMeasurement({1 + offset, 2 + offset, 3 + offset},
+ MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
{4 + offset, 5 + offset, 6 + offset})
.message());
}
@@ -127,15 +142,16 @@
TEST(ImuZeroerTest, FaultOnNewZero) {
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
- ASSERT_FALSE(zeroer.Zeroed());
- zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
- ASSERT_FALSE(zeroer.Faulted())
- << "We should not fault until we complete a second cycle of zeroing.";
- zeroer.ProcessMeasurement(MakeMeasurement({1, 5, 3}, {4, 5, 6}).message());
+ ASSERT_FALSE(zeroer.Faulted())
+ << "We should not fault until we complete a second cycle of zeroing.";
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.05, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Faulted());
}
@@ -144,14 +160,14 @@
TEST(ImuZeroerTest, NoFaultOnSimilarZero) {
ImuZeroer zeroer;
ASSERT_FALSE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
- ASSERT_FALSE(zeroer.Zeroed());
- zeroer.ProcessMeasurement(MakeMeasurement({1, 2, 3}, {4, 5, 6}).message());
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
+ zeroer.ProcessMeasurement(
+ MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
}
ASSERT_TRUE(zeroer.Zeroed());
- for (size_t ii = 0; ii < ImuZeroer::kSamplesToAverage; ++ii) {
+ for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
zeroer.ProcessMeasurement(
- MakeMeasurement({1, 2.0001, 3}, {4, 5, 6}).message());
+ MakeMeasurement({0.01, 0.020001, 0.03}, {4, 5, 6}).message());
}
ASSERT_FALSE(zeroer.Faulted());
}