Account for delayed U in using localizer
This makes the tests pass to within numerical precision errors, rather
than to merely very small errors.
Change-Id: I3e764082e3f86c2d06fc9e7a3bbc6ef749a75d85
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b6b8b5d..cab8043 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -232,11 +232,9 @@
Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
- // TODO(james): Account for delayed_U as appropriate (should be
- // last_last_*_voltage).
- localizer_->Update({last_left_voltage_, last_right_voltage_}, monotonic_now,
- position->left_encoder, position->right_encoder,
- last_gyro_rate_, last_accel_);
+ localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
+ monotonic_now, position->left_encoder,
+ position->right_encoder, last_gyro_rate_, last_accel_);
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
@@ -348,6 +346,8 @@
// Gyro heading vs left-right
// Voltage error.
+ last_last_left_voltage_ = last_left_voltage_;
+ last_last_right_voltage_ = last_right_voltage_;
Eigen::Matrix<double, 2, 1> U;
U(0, 0) = last_left_voltage_;
U(1, 0) = last_right_voltage_;