Tune the localizer to work reasonably well
The biggest changes here are adding back in voltage/angular error (in
effect, reverting parts of I9744c4808edf3a43ae1c76d022460ee1d4c9ed3e)
and tweaking some of the constants so that we better trust encoders.
Change-Id: Ibd9488e0d92d86792bb7cc6437a387589252a463
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 0015b70..6a2ea76 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -242,7 +242,8 @@
// TODO(james): Use a watcher (instead of a fetcher) once we support it in
// simulation.
if (localizer_control_fetcher_.Fetch()) {
- localizer_->ResetPosition(localizer_control_fetcher_->x,
+ LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
+ localizer_->ResetPosition(monotonic_now, localizer_control_fetcher_->x,
localizer_control_fetcher_->y,
localizer_control_fetcher_->theta);
}
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 119386a..104deee 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -49,8 +49,11 @@
kLeftVelocity = 4,
kRightEncoder = 5,
kRightVelocity = 6,
+ kLeftVoltageError = 7,
+ kRightVoltageError = 8 ,
+ kAngularError = 9,
};
- static constexpr int kNStates = 7;
+ static constexpr int kNStates = 10;
static constexpr int kNInputs = 2;
// Number of previous samples to save.
static constexpr int kSaveSamples = 50;
@@ -68,11 +71,19 @@
// variable-size measurement updates.
typedef Eigen::Matrix<Scalar, kNOutputs, 1> Output;
typedef Eigen::Matrix<Scalar, kNStates, kNStates> StateSquare;
- // State is [x_position, y_position, theta, left encoder, left ground vel,
- // right encoder, right ground vel]. left/right encoder should correspond
- // directly to encoder readings left/right velocities are the velocity of the
- // left/right sides over the
+ // State is [x_position, y_position, theta, Kalman States], where
+ // Kalman States are the states from the standard drivetrain Kalman Filter,
+ // which is: [left encoder, left ground vel, right encoder, right ground vel,
+ // left voltage error, right voltage error, angular_error], where:
+ // left/right encoder should correspond directly to encoder readings
+ // left/right velocities are the velocity of the left/right sides over the
// ground (i.e., corrected for angular_error).
+ // voltage errors are the difference between commanded and effective voltage,
+ // used to estimate consistent modelling errors (e.g., friction).
+ // angular error is the difference between the angular velocity as estimated
+ // by the encoders vs. estimated by the gyro, such as might be caused by
+ // wheels on one side of the drivetrain being too small or one side's
+ // wheels slipping more than the other.
typedef Eigen::Matrix<Scalar, kNStates, 1> State;
// Constructs a HybridEkf for a particular drivetrain.
@@ -96,6 +107,7 @@
const State &state, const StateSquare &P) {
observations_.clear();
X_hat_ = state;
+ have_zeroed_encoders_ = true;
P_ = P;
observations_.PushFromBottom(
{t,
@@ -140,6 +152,27 @@
const Scalar right_encoder, const Scalar gyro_rate,
const Input &U,
::aos::monotonic_clock::time_point t) {
+ // Because the check below for have_zeroed_encoders_ will add an
+ // Observation, do a check here to ensure that initialization has been
+ // performed and so there is at least one observation.
+ CHECK(!observations_.empty());
+ if (!have_zeroed_encoders_) {
+ // This logic handles ensuring that on the first encoder reading, we
+ // update the internal state for the encoders to match the reading.
+ // Otherwise, if we restart the drivetrain without restarting
+ // wpilib_interface, then we can get some obnoxious initial corrections
+ // that mess up the localization.
+ State newstate = X_hat_;
+ newstate(kLeftEncoder, 0) = left_encoder;
+ newstate(kRightEncoder, 0) = right_encoder;
+ newstate(kLeftVoltageError, 0) = 0.0;
+ newstate(kRightVoltageError, 0) = 0.0;
+ newstate(kAngularError, 0) = 0.0;
+ ResetInitialState(t, newstate, P_);
+ // We need to set have_zeroed_encoders_ after ResetInitialPosition because
+ // the reset clears have_zeroed_encoders_...
+ have_zeroed_encoders_ = true;
+ }
Output z(left_encoder, right_encoder, gyro_rate);
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
R.setZero();
@@ -294,6 +327,8 @@
Scalar encoder_noise_, gyro_noise_;
Eigen::Matrix<Scalar, kNStates, kNInputs> B_continuous_;
+ bool have_zeroed_encoders_ = false;
+
aos::PriorityQueue<Observation, kSaveSamples, ::std::less<Observation>>
observations_;
@@ -396,7 +431,9 @@
// Encoder derivatives
A_continuous_(kLeftEncoder, kLeftVelocity) = 1.0;
+ A_continuous_(kLeftEncoder, kAngularError) = 1.0;
A_continuous_(kRightEncoder, kRightVelocity) = 1.0;
+ A_continuous_(kRightEncoder, kAngularError) = -1.0;
// Pull velocity derivatives from velocity matrices.
// Note that this looks really awkward (doesn't use
@@ -413,6 +450,7 @@
B_continuous_.setZero();
B_continuous_.row(kLeftVelocity) = vel_coefs.B_continuous.row(0);
B_continuous_.row(kRightVelocity) = vel_coefs.B_continuous.row(1);
+ A_continuous_.template block<kNStates, kNInputs>(0, 7) = B_continuous_;
Q_continuous_.setZero();
// TODO(james): Improve estimates of process noise--e.g., X/Y noise can
@@ -422,13 +460,16 @@
Q_continuous_(kX, kX) = 0.01;
Q_continuous_(kY, kY) = 0.01;
Q_continuous_(kTheta, kTheta) = 0.0002;
- Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.03, 2.0);
- Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.03, 2.0);
- Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.1, 2.0);
- Q_continuous_(kRightVelocity, kRightVelocity) = ::std::pow(0.1, 2.0);
+ Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.15, 2.0);
+ Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.15, 2.0);
+ Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.5, 2.0);
+ Q_continuous_(kRightVelocity, kRightVelocity) = ::std::pow(0.5, 2.0);
+ Q_continuous_(kLeftVoltageError, kLeftVoltageError) = ::std::pow(10.0, 2.0);
+ Q_continuous_(kRightVoltageError, kRightVoltageError) = ::std::pow(10.0, 2.0);
+ Q_continuous_(kAngularError, kAngularError) = ::std::pow(2.0, 2.0);
P_.setZero();
- P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01;
+ P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
H_encoders_and_gyro_.setZero();
// Encoders are stored directly in the state matrix, so are a minor
@@ -441,8 +482,10 @@
const Eigen::Matrix<Scalar, 4, 4> R_kf_drivetrain =
dt_config_.make_kf_drivetrain_loop().observer().coefficients().R;
- encoder_noise_ = R_kf_drivetrain(0, 0);
- gyro_noise_ = R_kf_drivetrain(2, 2);
+ // TODO(james): The multipliers here are hand-waving things that I put in when
+ // tuning things. I haven't yet tried messing with these values again.
+ encoder_noise_ = 0.05 * R_kf_drivetrain(0, 0);
+ gyro_noise_ = 0.1 * R_kf_drivetrain(2, 2);
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 1702ec4..80ce95c 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -52,16 +52,22 @@
EXPECT_EQ(Xdot_ekf(StateIdx::kX, 0), ctheta * (left_vel + right_vel) / 2.0);
EXPECT_EQ(Xdot_ekf(StateIdx::kY, 0), stheta * (left_vel + right_vel) / 2.0);
EXPECT_EQ(Xdot_ekf(StateIdx::kTheta, 0), (right_vel - left_vel) / diameter);
- EXPECT_EQ(Xdot_ekf(StateIdx::kLeftEncoder, 0), left_vel);
- EXPECT_EQ(Xdot_ekf(StateIdx::kRightEncoder, 0), right_vel);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kLeftEncoder, 0),
+ left_vel + X(StateIdx::kAngularError, 0));
+ EXPECT_EQ(Xdot_ekf(StateIdx::kRightEncoder, 0),
+ right_vel - X(StateIdx::kAngularError, 0));
Eigen::Matrix<double, 2, 1> vel_x(X(StateIdx::kLeftVelocity, 0),
X(StateIdx::kRightVelocity, 0));
Eigen::Matrix<double, 2, 1> expected_vel_X =
velocity_plant_coefs_.A_continuous * vel_x +
- velocity_plant_coefs_.B_continuous * U;
+ velocity_plant_coefs_.B_continuous *
+ (U + X.middleRows<2>(StateIdx::kLeftVoltageError));
EXPECT_EQ(Xdot_ekf(StateIdx::kLeftVelocity, 0), expected_vel_X(0, 0));
EXPECT_EQ(Xdot_ekf(StateIdx::kRightVelocity, 0), expected_vel_X(1, 0));
+
+ // Dynamics don't expect error terms to change:
+ EXPECT_EQ(0.0, Xdot_ekf.bottomRows<3>().squaredNorm());
}
State DiffEq(const State &X, const Input &U) {
return ekf_.DiffEq(X, U);
@@ -87,14 +93,18 @@
CheckDiffEq(State::Zero(), Input::Zero());
CheckDiffEq(State::Zero(), {-5.0, 5.0});
CheckDiffEq(State::Zero(), {12.0, -3.0});
- CheckDiffEq((State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6).finished(),
+ CheckDiffEq((State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
+ 0.3).finished(),
{5.0, 6.0});
- CheckDiffEq((State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6).finished(),
+ CheckDiffEq((State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
+ 0.3).finished(),
{5.0, 6.0});
- CheckDiffEq((State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6).finished(),
+ CheckDiffEq((State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
+ 0.3).finished(),
{5.0, 6.0});
// And check that a theta outisde of [-M_PI, M_PI] works.
- CheckDiffEq((State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6).finished(),
+ CheckDiffEq((State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
+ 0.3).finished(),
{5.0, 6.0});
}
@@ -102,7 +112,7 @@
// with zero change in time, the state should approach the estimation.
TEST_F(HybridEkfTest, ZeroTimeCorrect) {
HybridEkf<>::Output Z(0.5, 0.5, 1);
- Eigen::Matrix<double, 3, 7> H;
+ Eigen::Matrix<double, 3, 10> H;
H.setIdentity();
auto h = [H](const State &X, const Input &) { return H * X; };
auto dhdx = [H](const State &) { return H; };
@@ -130,7 +140,7 @@
HybridEkf<>::Output Z(0, 0, 0);
// Use true_X to track what we think the true robot state is.
State true_X = ekf_.X_hat();
- Eigen::Matrix<double, 3, 7> H;
+ Eigen::Matrix<double, 3, 10> H;
H.setZero();
auto h = [H](const State &X, const Input &) { return H * X; };
auto dhdx = [H](const State &) { return H; };
@@ -161,6 +171,9 @@
EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftVelocity) * 0.8,
ekf_.X_hat(StateIdx::kRightVelocity),
ekf_.X_hat(StateIdx::kLeftVelocity) * 0.1);
+ EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kLeftVoltageError));
+ EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kRightVoltageError));
+ EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kAngularError));
const double ending_p_norm = ekf_.P().norm();
// Due to lack of corrections, noise should've increased.
EXPECT_GT(ending_p_norm, starting_p_norm * 1.10);
@@ -180,7 +193,7 @@
TEST_P(HybridEkfOldCorrectionsTest, CreateOldCorrection) {
HybridEkf<>::Output Z;
Z.setZero();
- Eigen::Matrix<double, 3, 7> H;
+ Eigen::Matrix<double, 3, 10> H;
H.setZero();
auto h_zero = [H](const State &X, const Input &) { return H * X; };
auto dhdx_zero = [H](const State &) { return H; };
@@ -218,7 +231,7 @@
expected_X_hat(0, 0) = Z(0, 0);
expected_X_hat(1, 0) = Z(1, 0) + modeled_X_hat(0, 0);
expected_X_hat(2, 0) = Z(2, 0);
- EXPECT_LT((expected_X_hat - ekf_.X_hat()).norm(),
+ EXPECT_LT((expected_X_hat.topRows<7>() - ekf_.X_hat().topRows<7>()).norm(),
1e-3)
<< "X_hat: " << ekf_.X_hat() << " expected " << expected_X_hat;
// The covariance after the predictions but before the corrections should
@@ -236,7 +249,7 @@
TEST_F(HybridEkfTest, DiscardTooOldCorrection) {
HybridEkf<>::Output Z;
Z.setZero();
- Eigen::Matrix<double, 3, 7> H;
+ Eigen::Matrix<double, 3, 10> H;
H.setZero();
auto h_zero = [H](const State &X, const Input &) { return H * X; };
auto dhdx_zero = [H](const State &) { return H; };
@@ -291,11 +304,11 @@
}
// Tests that encoder updates cause everything to converge properly in the
-// presence of an initial velocity error.
+// presence of voltage error.
TEST_F(HybridEkfTest, PerfectEncoderUpdatesWithVoltageError) {
State true_X = ekf_.X_hat();
- true_X(StateIdx::kLeftVelocity, 0) = 0.2;
- true_X(StateIdx::kRightVelocity, 0) = 0.2;
+ true_X(StateIdx::kLeftVoltageError, 0) = 2.0;
+ true_X(StateIdx::kRightVoltageError, 0) = 2.0;
Input U(5.0, 5.0);
for (int ii = 0; ii < 1000; ++ii) {
true_X = Update(true_X, U);
@@ -306,7 +319,7 @@
dt_config_.robot_radius / 2.0,
U, t0_ + (ii + 1) * dt_config_.dt);
}
- EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 5e-3)
+ EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 5e-2)
<< "Expected non-x/y estimates to converge to correct. "
"Estimated X_hat:\n"
<< ekf_.X_hat() << "\ntrue X:\n"
@@ -315,11 +328,11 @@
// Tests encoder/gyro updates when we have some errors in our estimate.
TEST_F(HybridEkfTest, PerfectEncoderUpdateConverges) {
- // In order to simulate modelling errors, we start the encoder values slightly
- // off.
+ // In order to simulate modelling errors, we add an angular_error and start
+ // the encoder values slightly off.
State true_X = ekf_.X_hat();
+ true_X(StateIdx::kAngularError, 0) = 1.0;
true_X(StateIdx::kLeftEncoder, 0) += 2.0;
- true_X(StateIdx::kLeftVelocity, 0) = 0.1;
true_X(StateIdx::kRightEncoder, 0) -= 2.0;
// After enough time, everything should converge to near-perfect (if there
// were any errors in the original absolute state (x/y/theta) state, then we
@@ -337,7 +350,7 @@
dt_config_.robot_radius / 2.0,
U, t0_ + (ii + 1) * dt_config_.dt);
}
- EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 1e-4)
+ EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 3e-5)
<< "Expected non-x/y estimates to converge to correct. "
"Estimated X_hat:\n"
<< ekf_.X_hat() << "\ntrue X:\n"
@@ -346,11 +359,11 @@
// Tests encoder/gyro updates in a realistic-ish scenario with noise:
TEST_F(HybridEkfTest, RealisticEncoderUpdateConverges) {
- // In order to simulate modelling errors, we start the encoder values slightly
- // off.
+ // In order to simulate modelling errors, we add an angular_error and start
+ // the encoder values slightly off.
State true_X = ekf_.X_hat();
+ true_X(StateIdx::kAngularError, 0) = 1.0;
true_X(StateIdx::kLeftEncoder, 0) += 2.0;
- true_X(StateIdx::kLeftVelocity, 0) = 0.1;
true_X(StateIdx::kRightEncoder, 0) -= 2.0;
Input U(10.0, 5.0);
for (int ii = 0; ii < 100; ++ii) {
@@ -364,7 +377,7 @@
U, t0_ + (ii + 1) * dt_config_.dt);
}
EXPECT_NEAR(
- (true_X.bottomRows<6>() - ekf_.X_hat().bottomRows<6>()).squaredNorm(),
+ (true_X.bottomRows<9>() - ekf_.X_hat().bottomRows<9>()).squaredNorm(),
0.0, 2e-3)
<< "Expected non-x/y estimates to converge to correct. "
"Estimated X_hat:\n" << ekf_.X_hat() << "\ntrue X:\n" << true_X;
@@ -398,7 +411,7 @@
// Check that we die when only one of h and dhdx are provided:
EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {},
[](const State &) {
- return Eigen::Matrix<double, 3, 7>::Zero();
+ return Eigen::Matrix<double, 3, 10>::Zero();
},
{}, t0_ + ::std::chrono::seconds(1)),
"make_h");
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index d63837a..9866803 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -49,7 +49,8 @@
double left_encoder, double right_encoder,
double gyro_rate, double longitudinal_accelerometer) = 0;
// Reset the absolute position of the estimator.
- virtual void ResetPosition(double x, double y, double theta) = 0;
+ virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
+ double y, double theta) = 0;
// There are several subtly different norms floating around for state
// matrices. In order to avoid that mess, we jus tprovide direct accessors for
// the values that most people care about.
@@ -58,6 +59,8 @@
virtual double theta() const = 0;
virtual double left_velocity() const = 0;
virtual double right_velocity() const = 0;
+ virtual double left_encoder() const = 0;
+ virtual double right_encoder() const = 0;
virtual double left_voltage_error() const = 0;
virtual double right_voltage_error() const = 0;
virtual TargetSelectorInterface *target_selector() = 0;
@@ -105,27 +108,36 @@
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
}
- void ResetPosition(double x, double y, double theta) override {
+ void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
+ double theta) override {
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
- ekf_.ResetInitialState(
- ::aos::monotonic_clock::now(),
- (Ekf::State() << x, y, theta, left_encoder, 0, right_encoder, 0)
- .finished(),
- ekf_.P());
+ ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
+ right_encoder, 0, 0, 0, 0).finished(),
+ ekf_.P());
};
double x() const override { return ekf_.X_hat(StateIdx::kX); }
double y() const override { return ekf_.X_hat(StateIdx::kY); }
double theta() const override { return ekf_.X_hat(StateIdx::kTheta); }
+ double left_encoder() const override {
+ return ekf_.X_hat(StateIdx::kLeftEncoder);
+ }
+ double right_encoder() const override {
+ return ekf_.X_hat(StateIdx::kRightEncoder);
+ }
double left_velocity() const override {
return ekf_.X_hat(StateIdx::kLeftVelocity);
}
double right_velocity() const override {
return ekf_.X_hat(StateIdx::kRightVelocity);
}
- double left_voltage_error() const override { return 0.0; }
- double right_voltage_error() const override { return 0.0; }
+ double left_voltage_error() const override {
+ return ekf_.X_hat(StateIdx::kLeftVoltageError);
+ }
+ double right_voltage_error() const override {
+ return ekf_.X_hat(StateIdx::kRightVoltageError);
+ }
TrivialTargetSelector *target_selector() override {
return &target_selector_;