Add zeroing estimator class.
Change-Id: I355db861a289a4f0f24eb2ca3e84a4e21270bfce
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
new file mode 100644
index 0000000..342bc0b
--- /dev/null
+++ b/frc971/zeroing/zeroing_test.cc
@@ -0,0 +1,266 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include <random>
+
+#include "gtest/gtest.h"
+#include "frc971/zeroing/zeroing_queue.q.h"
+#include "frc971/zeroing/zeroing.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/util/thread.h"
+#include "aos/common/die.h"
+
+namespace frc971 {
+namespace zeroing {
+
+const int kSeed1 = 0;
+const int kSeed2 = 3;
+
+class NoiseGenerator {
+ public:
+ virtual double AddNoiseToSample(double sample) = 0;
+};
+
+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() {
+ ZeroingInfo estimate;
+ 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_.getPosition(); }
+
+ 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_;
+};
+
+class QueueTest : 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;
+
+ QueueTest() : my_test_queue(".frc971.zeroing.test_queue") {}
+};
+
+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);
+
+ // 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);
+ }
+ ASSERT_NEAR(3.3, sim.getEstimate(), 0.1);
+
+ for (int i = 0; i < 300; i++) {
+ sim.MoveTo(3.9);
+ }
+ ASSERT_NEAR(3.9, sim.getEstimate(), 0.1);
+}
+
+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);
+
+ // 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);
+ }
+ ASSERT_NEAR(3.6, sim.getEstimate(), 0.1);
+
+ // 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);
+
+ sim.MoveTo(4.99);
+ ASSERT_NEAR(4.99, sim.getEstimate(), 0.001);
+
+ sim.MoveTo(3.99);
+ ASSERT_NEAR(3.99, sim.getEstimate(), 0.001);
+
+ sim.MoveTo(3.01);
+ ASSERT_NEAR(3.01, sim.getEstimate(), 0.001);
+
+ sim.MoveTo(13.55);
+ ASSERT_NEAR(13.55, sim.getEstimate(), 0.001);
+}
+
+TEST_F(QueueTest, TestDifferentIndexDiffs) {
+ double index_diff = 0.89;
+ GaussianNoise pot_noise(kSeed2, index_diff / 3.0);
+ ZeroingEstimatorSimulator sim(3.5 * index_diff, index_diff, pot_noise);
+
+ // 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);
+ }
+ ASSERT_NEAR(3.5 * index_diff, sim.getEstimate(), 0.1);
+
+ // 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);
+
+ sim.MoveTo(4.99);
+ ASSERT_NEAR(4.99, sim.getEstimate(), 0.001);
+
+ sim.MoveTo(3.99);
+ ASSERT_NEAR(3.99, sim.getEstimate(), 0.001);
+
+ sim.MoveTo(3.01);
+ ASSERT_NEAR(3.01, sim.getEstimate(), 0.001);
+
+ sim.MoveTo(13.55);
+ ASSERT_NEAR(13.55, sim.getEstimate(), 0.001);
+}
+
+} // namespace zeroing
+} // namespace frc971