Merge Adam's zeroing updates and add TODOs.
This branch merges some of the updates that Adam got around to with
the version that's on master.
- Remove all code relating to the zeroing queue.
- Use the PotAndIndexPosition structure to initialize the zeroing
logic.
- Use the PositionSensorSimulator class to test the zeroing logic
instead of its own internal version.
- Removed the "SimpleStep" test since it was written for a type of
noise that used the 'floor' math function. We assume Gaussian noise
in the potentiometer.
Change-Id: I683c0647242092602eac9b8eff36466f0f28ad21
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index f6eddb5..c2150d5 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -1,4 +1,5 @@
#include "frc971/zeroing/zeroing.h"
+
#include <math.h>
#include <vector>
@@ -7,64 +8,59 @@
ZeroingEstimator::ZeroingEstimator(
const constants::Values::ZeroingConstants& constants) {
- DoInit(constants.index_difference, constants.average_filter_size);
+ index_diff_ = constants.index_difference;
+ max_sample_count_ = constants.average_filter_size;
+
+ start_pos_samples_.reserve(max_sample_count_);
+
+ Reset();
}
-ZeroingEstimator::ZeroingEstimator(double index_difference,
- size_t max_sample_count) {
- DoInit(index_difference, max_sample_count);
-}
-
-void ZeroingEstimator::DoInit(double index_difference,
- size_t max_sample_count) {
- index_diff_ = index_difference;
+void ZeroingEstimator::Reset() {
samples_idx_ = 0;
- max_sample_count_ = max_sample_count;
- start_pos_samples_.reserve(max_sample_count);
+ start_pos_ = 0;
+ start_pos_samples_.clear();
+ zeroed_ = false;
}
void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
- ZeroingInfo zinfo;
- zinfo.pot = info.pot;
- zinfo.encoder = info.encoder;
- zinfo.index_encoder = info.latched_encoder;
- zinfo.index_count = info.index_pulses;
- UpdateEstimate(zinfo);
-}
-
-void ZeroingEstimator::UpdateEstimate(const ZeroingInfo & info) {
if (start_pos_samples_.size() < max_sample_count_) {
start_pos_samples_.push_back(info.pot - info.encoder);
} else {
start_pos_samples_[samples_idx_] = info.pot - info.encoder;
}
+
+ // Drop the oldest sample when we run this function the next time around.
samples_idx_ = (samples_idx_ + 1) % max_sample_count_;
- double start_average = 0.0;
+ double sample_sum = 0.0;
+
for (size_t i = 0; i < start_pos_samples_.size(); ++i) {
- start_average += start_pos_samples_[i];
+ sample_sum += start_pos_samples_[i];
}
// Calculates the average of the starting position.
- start_average = start_average / start_pos_samples_.size();
- /* If the index_encoder is invalid, then we use
- * the average of the starting position to
- * calculate the position.
- */
- double pos;
- if (info.index_count == 0) {
- pos = start_average + info.encoder;
- zeroed_ = false;
+ double start_average = sample_sum / start_pos_samples_.size();
+
+ // If there are no index pulses to use or we don't have enough samples yet to
+ // have a well-filtered starting position then we use the filtered value as
+ // our best guess.
+ if (info.index_pulses == 0 || offset_ratio_ready() < 1.0) {
+ start_pos_ = start_average;
} else {
// We calculate an aproximation of the value of the last index position.
- double index_pos = start_average + info.index_encoder;
+ double index_pos = start_average + info.latched_encoder;
// We round index_pos to the closest valid value of the index.
double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
- // We use accurate_index_pos to calculate the position.
- pos = accurate_index_pos + info.encoder - info.index_encoder;
+ // Now we reverse the first calculation to the accurate start position.
+ start_pos_ = accurate_index_pos - info.latched_encoder;
+
+ // Now that we have an accurate starting position we can consider ourselves
+ // zeroed.
zeroed_ = true;
}
- offset_ = pos - info.encoder;
+
+ pos_ = start_pos_ + info.encoder;
}
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing.gyp b/frc971/zeroing/zeroing.gyp
index 3d17f13..a868e19 100644
--- a/frc971/zeroing/zeroing.gyp
+++ b/frc971/zeroing/zeroing.gyp
@@ -1,29 +1,16 @@
{
'targets': [
{
- 'target_name': 'zeroing_queue',
- 'type': 'static_library',
- 'sources': [
- '<(DEPTH)/frc971/zeroing/zeroing_queue.q',
- ],
- 'variables': {
- 'header_path': 'frc971/zeroing',
- },
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
'target_name': 'zeroing',
'type': 'static_library',
'sources': [
'zeroing.cc',
],
'dependencies': [
- 'zeroing_queue',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
'<(DEPTH)/frc971/frc971.gyp:constants',
],
'export_dependent_settings': [
- 'zeroing_queue',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
],
@@ -38,9 +25,10 @@
'<(EXTERNALS):gtest',
'<(AOS)/common/common.gyp:queue_testutils',
'zeroing',
- 'zeroing_queue',
'<(AOS)/common/util/util.gyp:thread',
'<(AOS)/common/common.gyp:die',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:position_sensor_sim',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
],
},
],
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 89ee9da..d85c8b8 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -2,10 +2,27 @@
#define FRC971_ZEROING_ZEROING_H_
#include <vector>
-#include "frc971/zeroing/zeroing_queue.q.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/constants.h"
+// TODO(pschrader): Support the ZeroingConstants::measured_index_position
+// parameter.
+//
+// TODO(pschrader): Wait for an index pulse during zeroing. If we start up with
+// a non-zero number of index pulses then the logic will use that pulse to
+// compute the starting position (a/k/a the offset).
+//
+// TODO(pschrader): Create an error API to flag faults/errors etc..
+//
+// TODO(pschrader): Flag an error if encoder index pulse is not n revolutions
+// away from the last one (i.e. got extra counts from noise, etc..)
+//
+// TODO(pschrader): Flag error if the pot disagrees too much with the encoder
+// after being zeroed.
+//
+// TODO(pschrader): Watch the offset over long periods of time and flag if it
+// gets too far away from the initial value.
+
namespace frc971 {
namespace zeroing {
@@ -13,35 +30,59 @@
// the pot and the indices.
class ZeroingEstimator {
public:
- ZeroingEstimator(double index_difference, size_t max_sample_count);
ZeroingEstimator(const constants::Values::ZeroingConstants &constants);
- void UpdateEstimate(const PotAndIndexPosition &info);
- void UpdateEstimate(const ZeroingInfo &info);
- double offset() const { return offset_; }
+ // Update the internal logic with the next sensor values.
+ void UpdateEstimate(const PotAndIndexPosition &info);
+
+ // Reset the internal logic so it needs to be re-zeroed.
+ void Reset();
+
+ // Returns true if the logic considers the corresponding mechanism to be
+ // zeroed. It return false otherwise. For example, right after a call to
+ // Reset() this returns false.
bool zeroed() const { return zeroed_; }
+
+ // Return the estimated position of the corresponding mechanism. This value
+ // is in SI units. For example, the estimator for the elevator would return a
+ // value in meters for the height relative to absolute zero.
+ double position() const { return pos_; }
+
+ // Return the estimated starting position of the corresponding mechansim. In
+ // some contexts we refer to this as the "offset".
+ double offset() const { return start_pos_; }
+
+ // Returns a number between 0 and 1 that represents the percentage of the
+ // samples being used in the moving average filter. A value of 0.0 means that
+ // no samples are being used. A value of 1.0 means that the filter is using
+ // as many samples as it has room for. For example, after a Reset() this
+ // value returns 0.0. As more samples get added with UpdateEstimate(...) the
+ // return value starts increasing to 1.0.
double offset_ratio_ready() const {
return start_pos_samples_.size() / static_cast<double>(max_sample_count_);
}
private:
- void DoInit(double index_difference, size_t max_sample_count);
-
- double offset_ = 0.0;
- bool zeroed_ = false;
+ // The estimated position.
+ double pos_;
// The distance between two consecutive index positions.
double index_diff_;
- // The next position in 'start_pos_samples_' to be used to store the
- // next sample.
+ // The next position in 'start_pos_samples_' to be used to store the next
+ // sample.
int samples_idx_;
// Last 'max_sample_count_' samples for start positions.
std::vector<double> start_pos_samples_;
- // The number of the last samples of start position to consider
- // in the estimation.
+ // The number of the last samples of start position to consider in the
+ // estimation.
size_t max_sample_count_;
+ // The estimated starting position of the mechanism. We also call this the
+ // 'offset' in some contexts.
+ double start_pos_;
+ // Marker to track whether we're fully zeroed yet or not.
+ bool zeroed_;
};
-} // namespace zeroing
-} // namespace frc971
+} // namespace zeroing
+} // namespace frc971
#endif // FRC971_ZEROING_ZEROING_H_
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index ce636b6..51d1274 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -5,261 +5,171 @@
#include <random>
#include "gtest/gtest.h"
-#include "frc971/zeroing/zeroing_queue.q.h"
#include "frc971/zeroing/zeroing.h"
+#include "frc971/control_loops/control_loops.q.h"
#include "aos/common/queue_testutils.h"
#include "aos/common/util/thread.h"
#include "aos/common/die.h"
+#include "frc971/control_loops/position_sensor_sim.h"
namespace frc971 {
namespace zeroing {
-const int kSeed1 = 0;
-const int kSeed2 = 3;
+using control_loops::PositionSensorSimulator;
+using constants::Values;
-class NoiseGenerator {
- public:
- virtual double AddNoiseToSample(double sample) = 0;
-};
+static const size_t kSampleSize = 30;
+static const double kAcceptableUnzeroedError = 0.2;
-class NoNoise : public NoiseGenerator {
- public:
- double AddNoiseToSample(double sample) { return sample; }
-};
-
-class FloorNoise : public NoiseGenerator {
- public:
- FloorNoise(double accuracy) : accuracy_(accuracy) {}
-
- double AddNoiseToSample(double sample) {
- return accuracy_ * ((int)(sample / accuracy_));
- }
-
- private:
- double accuracy_;
-};
-
-class GaussianNoise : public NoiseGenerator {
- public:
- GaussianNoise(unsigned int seed, double stddev)
- : generator_(seed), distribution_(0.0, stddev) {}
-
- double AddNoiseToSample(double sample) {
- return sample + distribution_(generator_);
- }
-
- private:
- std::default_random_engine generator_;
- std::normal_distribution<double> distribution_;
-};
-
-class ZeroingEstimatorSimulator {
- public:
- ZeroingEstimatorSimulator(double start_pos, double index_diff,
- NoiseGenerator& noise, int filter_size = 30)
- : estimator_(index_diff, filter_size), noise_generator_(noise) {
- cur_index_segment_ = (int)(start_pos / index_diff);
- index_diff_ = index_diff;
- start_pos_ = start_pos;
- cur_pos_ = start_pos;
- index_count_ = 0;
- encoder_slip_ = 0;
-
- // Initialize the ZeroingEstimator instance with the first sensor readings.
- estimator_.UpdateEstimate(getInfo());
- }
-
- void MoveTo(double new_pos) {
- int new_index = (int)(new_pos - encoder_slip_) / index_diff_;
- if (new_index < cur_index_segment_) {
- cur_index_ = new_index + 1;
- index_count_++;
- }
- if (new_index > cur_index_segment_) {
- cur_index_ = new_index;
- index_count_++;
- }
- cur_index_segment_ = new_index;
- cur_pos_ = new_pos;
-
- estimator_.UpdateEstimate(getInfo());
- }
-
- // Simulate the encoder slipping by `slip'.
- void MoveWithEncoderSlip(double slip) {
- encoder_slip_ += slip;
-
- MoveTo(cur_pos_ + slip);
-
- estimator_.UpdateEstimate(getInfo());
- }
-
- ZeroingInfo getInfo() {
- estimate_.pot = noise_generator_.AddNoiseToSample(cur_pos_);
- if (index_count_ == 0) {
- estimate_.index_encoder = 0.0;
- } else {
- estimate_.index_encoder = cur_index_ * index_diff_ - start_pos_;
- }
- estimate_.index_count = index_count_;
- estimate_.encoder = cur_pos_ - start_pos_ - encoder_slip_;
- return estimate_;
- }
-
- double getEstimate(void) { return estimator_.offset() + estimate_.encoder; }
-
- private:
- int index_count_;
- int cur_index_;
- int cur_index_segment_;
- double index_diff_;
- double start_pos_;
- double cur_pos_;
- double encoder_slip_;
- ZeroingEstimator estimator_;
- NoiseGenerator& noise_generator_;
- ZeroingInfo estimate_;
-};
-
-class QueueTest : public ::testing::Test {
+class ZeroingTest : public ::testing::Test {
protected:
void SetUp() override { aos::SetDieTestMode(true); }
- aos::common::testing::GlobalCoreInstance my_core;
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- ::aos::Queue<TestMessage> my_test_queue;
+ void MoveTo(PositionSensorSimulator* simulator, ZeroingEstimator* estimator,
+ double new_position) {
+ PotAndIndexPosition sensor_values_;
+ simulator->MoveTo(new_position);
+ simulator->GetSensorValues(&sensor_values_);
+ estimator->UpdateEstimate(sensor_values_);
+ }
- QueueTest() : my_test_queue(".frc971.zeroing.test_queue") {}
+ aos::common::testing::GlobalCoreInstance my_core;
};
-TEST_F(QueueTest, FetchBlocking) {
- // Make sure that the queue works.
- my_test_queue.MakeWithBuilder().test_int(0x971).Send();
- EXPECT_TRUE(my_test_queue.FetchNext());
-}
-
-TEST_F(QueueTest, SimpleStep) {
- FloorNoise floored_pot(0.25);
- ZeroingEstimatorSimulator sim(3.6, 1.0, floored_pot, 1);
-
- // The first estimate should be 3.5 since that's the only reliable number we
- // have. (i.e. 3.6 rounded down to the nearest 0.25 multiple.
- ASSERT_NEAR(3.5, sim.getEstimate(), 0.001);
-
- // Next we'll move to 3.65 which should still give us a reading of 3.50. This
- // is because we're just using one sample to "filter" the noise. In this case
- // the filter would take 3.5 from the pot value and subract the encoder
- // reading of 0.05. In order to come up with an accurate estimate, we add the
- // encoder value back in.
- sim.MoveTo(3.65);
- ASSERT_NEAR(3.50, sim.getEstimate(), 0.001);
-
- // Now we move to 3.80 which should give us the a reading of 3.70. Similar to
- // the above scenario we've now moved 0.20 in total which is the reading of
- // the encoder. Unfortunately, we can't use the encoder value yet since we
- // don't know where it is relative to the index pulse.
- sim.MoveTo(3.80);
- ASSERT_NEAR(3.75, sim.getEstimate(), 0.001);
-
- // We move past the 4.00 mark right to 4.10. The pot value will read 4.00,
- // the encoder reads 0.5 and the index pulse sample will read 0.4. Now we
- // know that we are 0.1 past the 4.00 mark.
- sim.MoveTo(4.10);
- ASSERT_NEAR(4.10, sim.getEstimate(), 0.001);
-
- // We move back to 3.80 and now we should have an accurate reading. The pot
- // value reads 3.75, the encoder reads 0.2 and the index pulse is again set
- // at 0.4. Thus we can deduce that we're 0.2 below the 4.00 mark (i.e. at
- // 3.80)
- sim.MoveTo(3.80);
- ASSERT_NEAR(3.80, sim.getEstimate(), 0.001);
-
- // Just for kicks we'll move back to a value of 2.56 which the estimator
- // should be able to calculate.
- sim.MoveTo(2.56);
- ASSERT_NEAR(2.56, sim.getEstimate(), 0.001);
-}
-
-TEST_F(QueueTest, TestMovingAverageFilter) {
- GaussianNoise pot_noise(kSeed1, 0.5 / 3.0);
- ZeroingEstimatorSimulator sim(3.6, 1.0, pot_noise);
+TEST_F(ZeroingTest, TestMovingAverageFilter) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.6 * index_diff, index_diff / 3.0);
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
// samples are sufficient to have updated the filter.
for (int i = 0; i < 300; i++) {
- sim.MoveTo(3.3);
+ MoveTo(&sim, &estimator, 3.3 * index_diff);
}
- ASSERT_NEAR(3.3, sim.getEstimate(), 0.1);
+ ASSERT_NEAR(3.3 * index_diff, estimator.position(),
+ kAcceptableUnzeroedError * index_diff);
for (int i = 0; i < 300; i++) {
- sim.MoveTo(3.9);
+ MoveTo(&sim, &estimator, 3.9 * index_diff);
}
- ASSERT_NEAR(3.9, sim.getEstimate(), 0.1);
+ ASSERT_NEAR(3.9 * index_diff, estimator.position(),
+ kAcceptableUnzeroedError * index_diff);
}
-TEST_F(QueueTest, TestLotsOfMovement) {
- double index_diff = 1.00;
- GaussianNoise pot_noise(kSeed2, index_diff / 3.0);
- ZeroingEstimatorSimulator sim(3.6, index_diff, pot_noise);
+TEST_F(ZeroingTest, NotZeroedBeforeEnoughSamplesCollected) {
+ double index_diff = 0.5;
+ double position = 3.6 * index_diff;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(position, index_diff / 3.0);
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+
+ // Make sure that the zeroing code does not consider itself zeroed until we
+ // collect a good amount of samples. In this case we're waiting until the
+ // moving average filter is full.
+ for (unsigned int i = 0; i < kSampleSize - 1; i++) {
+ MoveTo(&sim, &estimator, position += index_diff);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, position);
+ ASSERT_TRUE(estimator.zeroed());
+}
+
+TEST_F(ZeroingTest, TestLotsOfMovement) {
+ double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.6, index_diff / 3.0);
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
// samples are sufficient to have updated the filter.
for (int i = 0; i < 300; i++) {
- sim.MoveTo(3.6);
+ MoveTo(&sim, &estimator, 3.6);
}
- ASSERT_NEAR(3.6, sim.getEstimate(), 0.1);
+ ASSERT_NEAR(3.6, estimator.position(), kAcceptableUnzeroedError * index_diff);
// With a single index pulse the zeroing estimator should be able to lock
// onto the true value of the position.
- sim.MoveTo(4.01);
- ASSERT_NEAR(4.01, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 4.01);
+ ASSERT_NEAR(4.01, estimator.position(), 0.001);
- sim.MoveTo(4.99);
- ASSERT_NEAR(4.99, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 4.99);
+ ASSERT_NEAR(4.99, estimator.position(), 0.001);
- sim.MoveTo(3.99);
- ASSERT_NEAR(3.99, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 3.99);
+ ASSERT_NEAR(3.99, estimator.position(), 0.001);
- sim.MoveTo(3.01);
- ASSERT_NEAR(3.01, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 3.01);
+ ASSERT_NEAR(3.01, estimator.position(), 0.001);
- sim.MoveTo(13.55);
- ASSERT_NEAR(13.55, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 13.55);
+ ASSERT_NEAR(13.55, estimator.position(), 0.001);
}
-TEST_F(QueueTest, TestDifferentIndexDiffs) {
+TEST_F(ZeroingTest, TestDifferentIndexDiffs) {
double index_diff = 0.89;
- GaussianNoise pot_noise(kSeed2, index_diff / 3.0);
- ZeroingEstimatorSimulator sim(3.5 * index_diff, index_diff, pot_noise);
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.5 * index_diff, index_diff / 3.0);
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
// The zeroing code is supposed to perform some filtering on the difference
// between the potentiometer value and the encoder value. We assume that 300
// samples are sufficient to have updated the filter.
for (int i = 0; i < 300; i++) {
- sim.MoveTo(3.5 * index_diff);
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
}
- ASSERT_NEAR(3.5 * index_diff, sim.getEstimate(), 0.1);
+ ASSERT_NEAR(3.5 * index_diff, estimator.position(),
+ kAcceptableUnzeroedError * index_diff);
// With a single index pulse the zeroing estimator should be able to lock
// onto the true value of the position.
- sim.MoveTo(4.01);
- ASSERT_NEAR(4.01, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 4.01);
+ ASSERT_NEAR(4.01, estimator.position(), 0.001);
- sim.MoveTo(4.99);
- ASSERT_NEAR(4.99, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 4.99);
+ ASSERT_NEAR(4.99, estimator.position(), 0.001);
- sim.MoveTo(3.99);
- ASSERT_NEAR(3.99, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 3.99);
+ ASSERT_NEAR(3.99, estimator.position(), 0.001);
- sim.MoveTo(3.01);
- ASSERT_NEAR(3.01, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 3.01);
+ ASSERT_NEAR(3.01, estimator.position(), 0.001);
- sim.MoveTo(13.55);
- ASSERT_NEAR(13.55, sim.getEstimate(), 0.001);
+ MoveTo(&sim, &estimator, 13.55);
+ ASSERT_NEAR(13.55, estimator.position(), 0.001);
+}
+
+TEST_F(ZeroingTest, TestPercentage) {
+ double index_diff = 0.89;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.5 * index_diff, index_diff / 3.0);
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+
+ for (unsigned int i = 0; i < kSampleSize / 2; i++) {
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
+ }
+ ASSERT_NEAR(0.5, estimator.offset_ratio_ready(), 0.001);
+}
+
+TEST_F(ZeroingTest, TestOffset) {
+ double index_diff = 0.89;
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(3.1 * index_diff, index_diff / 3.0);
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+
+ for (unsigned int i = 0; i < kSampleSize; i++) {
+ MoveTo(&sim, &estimator, 5.0 * index_diff);
+ }
+ ASSERT_NEAR(3.1 * index_diff, estimator.offset(), 0.001);
}
} // namespace zeroing