Incorporate IMU measurements into EKF
The changes in this commit forced me to up the tolerances more than I
would've liked on some of the 2019 simulation tests, partially because
the artificial disturbances in the 2019 tests don't introduce valid
accelerometer readings.
Change-Id: I66758e36d76b127ddeedfbcfadb2d77acf1f2997
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index c436232..16cd723 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -58,17 +58,17 @@
localizer_.ResetInitialState(now, state, newP);
}
-void EventLoopLocalizer::Update(
- const ::Eigen::Matrix<double, 2, 1> &U,
- ::aos::monotonic_clock::time_point now, double left_encoder,
- double right_encoder, double gyro_rate,
- double /*longitudinal_accelerometer*/) {
+void EventLoopLocalizer::Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ ::aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate,
+ const Eigen::Vector3d &accel) {
AOS_CHECK(U.allFinite());
AOS_CHECK(::std::isfinite(left_encoder));
AOS_CHECK(::std::isfinite(right_encoder));
AOS_CHECK(::std::isfinite(gyro_rate));
localizer_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U,
- now);
+ accel, now);
while (frame_fetcher_.FetchNext()) {
HandleFrame(frame_fetcher_.get());
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 83b8e79..4c6150d 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -60,7 +60,7 @@
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
::aos::monotonic_clock::time_point now, double left_encoder,
double right_encoder, double gyro_rate,
- double /*longitudinal_accelerometer*/) override;
+ const Eigen::Vector3d &accel) override;
double x() const override {
return localizer_.X_hat(StateIdx::kX); }
@@ -86,6 +86,15 @@
double right_voltage_error() const override {
return localizer_.X_hat(StateIdx::kRightVoltageError);
}
+ double angular_error() const override {
+ return localizer_.X_hat(StateIdx::kAngularError);
+ }
+ double longitudinal_velocity_offset() const override {
+ return localizer_.X_hat(StateIdx::kLongitudinalVelocityOffset);
+ }
+ double lateral_velocity() const override {
+ return localizer_.X_hat(StateIdx::kLateralVelocity);
+ }
TargetSelector *target_selector() override {
return &target_selector_;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 9b237f4..1459248 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -111,8 +111,7 @@
const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
EXPECT_NEAR(localizer_.x(), true_state(0, 0), eps);
EXPECT_NEAR(localizer_.y(), true_state(1, 0), eps);
- // TODO(james): Uncomment this.
- //EXPECT_NEAR(localizer_.theta(), true_state(2, 0), 2.0 * eps);
+ EXPECT_NEAR(localizer_.theta(), true_state(2, 0), eps);
EXPECT_NEAR(localizer_.left_velocity(), true_state(3, 0), eps);
EXPECT_NEAR(localizer_.right_velocity(), true_state(4, 0), eps);
}
@@ -290,8 +289,7 @@
// Everything but X-value should be correct:
EXPECT_NEAR(true_state.x(), localizer_.x() + 0.05, 1e-5);
EXPECT_NEAR(true_state.y(), localizer_.y(), 1e-5);
- // TODO(james): Uncomment this.
- //EXPECT_NEAR(true_state(2, 0), localizer_.theta(), 1e-3);
+ EXPECT_NEAR(true_state(2, 0), localizer_.theta(), 1e-3);
EXPECT_NEAR(true_state(3, 0), localizer_.left_velocity(), 1e-4);
EXPECT_NEAR(true_state(4, 0), localizer_.right_velocity(), 1e-4);
}
@@ -383,10 +381,9 @@
VerifyEstimatorAccurate(0.2);
// Due to the fact that we aren't modulating the throttle, we don't try to hit
// the target exactly. Instead, just run slightly past the target:
- // TODO(james): Uncomment this.
- //EXPECT_LT(
- // ::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
- // 0.5);
+ EXPECT_LT(
+ ::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
+ 0.5);
EXPECT_GT(HPSlotLeft().abs_pos().x(), drivetrain_plant_.state().x());
EXPECT_NEAR(HPSlotLeft().abs_pos().y(), drivetrain_plant_.state().y(), 0.2);
}
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index aa7f22f..7b25ca5 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -142,9 +142,6 @@
}
void SetUp() {
- // Turn on -v 1
- FLAGS_v = std::max(FLAGS_v, 1);
-
flatbuffers::DetachedBuffer goal_buffer;
{
flatbuffers::FlatBufferBuilder fbb;
@@ -315,8 +312,12 @@
}
// The differential equation for the dynamics of the system.
TestLocalizer::State DiffEq(const TestLocalizer::State &X,
- const TestLocalizer::Input &U) {
- return localizer_.DiffEq(X, U);
+ const Eigen::Vector2d &voltage) {
+ TestLocalizer::Input U;
+ U.setZero();
+ U(0) = voltage(0);
+ U(1) = voltage(1);
+ return localizer_.DiffEq(X, U, true);
}
Field field_;
@@ -433,7 +434,7 @@
output.left_voltage = 0;
output.right_voltage = 0;
spline_drivetrain_.SetOutput(&output);
- TestLocalizer::Input U(output.left_voltage, output.right_voltage);
+ Eigen::Vector2d U(output.left_voltage, output.right_voltage);
const ::Eigen::Matrix<double, 5, 1> goal_state =
spline_drivetrain_.CurrentGoalState();
@@ -454,7 +455,7 @@
U(1, 0) = ::std::max(::std::min(U(1, 0), 12.0), -12.0);
state = ::frc971::control_loops::RungeKuttaU(
- [this](const ::Eigen::Matrix<double, 10, 1> &X,
+ [this](const ::Eigen::Matrix<double, 12, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) { return DiffEq(X, U); },
state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
@@ -462,6 +463,8 @@
// interest here are that we (a) stop adding disturbances at the very end of
// the trajectory, to allow us to actually converge to the goal, and (b)
// scale disturbances by the corruent velocity.
+ // TODO(james): Figure out how to persist good accelerometer values through
+ // the disturbances.
if (GetParam().disturb && i % 75 == 0) {
// Scale the disturbance so that when we have near-zero velocity we don't
// have much disturbance.
@@ -470,7 +473,8 @@
::std::pow(state(StateIdx::kRightVelocity, 0), 2)) /
3.0);
TestLocalizer::State disturbance;
- disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0;
+ disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0;
disturbance *= disturbance_scale;
state += disturbance;
}
@@ -482,13 +486,18 @@
const double left_enc = state(StateIdx::kLeftEncoder, 0);
const double right_enc = state(StateIdx::kRightEncoder, 0);
- const double gyro = (state(StateIdx::kRightVelocity, 0) -
- state(StateIdx::kLeftVelocity, 0)) /
+ const double gyro = (state(StateIdx::kRightVelocity) -
+ state(StateIdx::kLeftVelocity)) /
dt_config_.robot_radius / 2.0;
+ const TestLocalizer::State xdot = DiffEq(state, U);
+ const Eigen::Vector3d accel(
+ localizer_.CalcLongitudinalVelocity(xdot) -
+ gyro * state(StateIdx::kLateralVelocity),
+ gyro * localizer_.CalcLongitudinalVelocity(state), 1.0);
localizer_.UpdateEncodersAndGyro(left_enc + Normal(encoder_noise_),
right_enc + Normal(encoder_noise_),
- gyro + Normal(gyro_noise_), U, t);
+ gyro + Normal(gyro_noise_), U, accel, t);
for (size_t cam_idx = 0; cam_idx < camera_queues.size(); ++cam_idx) {
auto &camera_queue = camera_queues[cam_idx];
@@ -551,14 +560,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
- /*estimate_tolerance=*/1e-5,
+ /*estimate_tolerance=*/1e-2,
/*goal_tolerance=*/2e-2,
}),
// Checks "perfect" estimation, but start off the spline and check
@@ -567,14 +576,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
- /*estimate_tolerance=*/1e-5,
+ /*estimate_tolerance=*/1e-2,
/*goal_tolerance=*/2e-2,
}),
// Repeats perfect scenario, but add sensor noise.
@@ -582,10 +591,10 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/false,
@@ -597,14 +606,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
- /*estimate_tolerance=*/1e-4,
+ /*estimate_tolerance=*/1e-2,
/*goal_tolerance=*/2e-2,
}),
// Repeats perfect scenario, but add voltage + angular errors:
@@ -612,14 +621,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
- 0.5, 0.02)
+ 0.5, 0.02, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
- /*estimate_tolerance=*/1e-4,
+ /*estimate_tolerance=*/1e-2,
/*goal_tolerance=*/2e-2,
}),
// Add disturbances while we are driving:
@@ -627,14 +636,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/true,
- /*estimate_tolerance=*/3e-2,
+ /*estimate_tolerance=*/4e-2,
/*goal_tolerance=*/0.15,
}),
// Add noise and some initial error in addition:
@@ -642,15 +651,15 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/true,
- /*estimate_tolerance=*/0.2,
- /*goal_tolerance=*/0.5,
+ /*estimate_tolerance=*/0.5,
+ /*goal_tolerance=*/1.0,
}),
// Try another spline, just in case the one I was using is special for
// some reason; this path will also go straight up to a target, to
@@ -659,10 +668,10 @@
/*control_pts_x=*/{{0.5, 3.5, 4.0, 8.0, 11.0, 10.2}},
/*control_pts_y=*/{{1.0, 1.0, -3.0, -2.0, -3.5, -3.65}},
(TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/false,