Implement 2019 EKF
The main remaining task with this is to integrate it with the actual
drivetrain code, and then to tune it on the actual robot.
For performance, on my computer I'm seeing ~0.6ms per simulation
iteration, which will vary depending on exact camera frame rates and
latencies. It might need a bit of optimization for CPU load, but we
should be reasonably close.
Change-Id: I286599e2c2f88dfef200afeb9ebbe9e7108714bf
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 245e772..01a49a9 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -9,6 +9,14 @@
#include "Eigen/Dense"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+namespace y2019 {
+namespace control_loops {
+namespace testing {
+class ParameterizedLocalizerTest;
+} // namespace testing
+} // namespace control_loops
+} // namespace y2019
+
namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -95,9 +103,10 @@
// TODO(james): We may want to actually re-initialize and reset things on
// the field. Create some sort of Reset() function.
void ResetInitialState(::aos::monotonic_clock::time_point t,
- const State &state) {
+ const State &state, const StateSquare &P) {
observations_.clear();
X_hat_ = state;
+ P_ = P;
observations_.PushFromBottom(
{t,
t,
@@ -299,6 +308,7 @@
observations_;
friend class testing::HybridEkfTest;
+ friend class ::y2019::control_loops::testing::ParameterizedLocalizerTest;
}; // class HybridEkf
template <typename Scalar>
@@ -419,8 +429,9 @@
Q_continuous_.setZero();
// TODO(james): Improve estimates of process noise--e.g., X/Y noise can
- // probably be reduced because we rarely randomly jump in space, but maybe
- // wheel speed noise should increase due to likelihood of slippage.
+ // probably be reduced when we are stopped because you rarely jump randomly.
+ // Or maybe it's more appropriate to scale wheelspeed noise with wheelspeed,
+ // since the wheels aren't likely to slip much stopped.
Q_continuous_(kX, kX) = 0.005;
Q_continuous_(kY, kY) = 0.005;
Q_continuous_(kTheta, kTheta) = 0.001;
@@ -428,7 +439,7 @@
dt_config_.make_kf_drivetrain_loop().observer().coefficients().Q;
P_.setZero();
- P_.diagonal() << 1, 1, 1, 0.5, 0.1, 0.5, 0.1, 1, 1, 1;
+ 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
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 9bdb30b..27119b1 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -28,7 +28,8 @@
velocity_plant_coefs_(dt_config_.make_hybrid_drivetrain_velocity_loop()
.plant()
.coefficients()) {
- ekf_.ResetInitialState(t0_, HybridEkf<>::State::Zero());
+ ekf_.ResetInitialState(t0_, State::Zero(),
+ HybridEkf<>::StateSquare::Identity());
}
protected:
@@ -107,25 +108,26 @@
{5.0, 6.0});
}
-// Tests that if we provide a bunch of observations of the angular position
+// Tests that if we provide a bunch of observations of the position
// with zero change in time, the state should approach the estimation.
TEST_F(HybridEkfTest, ZeroTimeCorrect) {
- HybridEkf<>::Output Z(1, 0, 0);
+ HybridEkf<>::Output Z(0.5, 0.5, 1);
Eigen::Matrix<double, 3, 10> H;
- H.setZero();
- H(0, 2) = 1;
+ H.setIdentity();
auto h = [H](const State &X, const Input &) { return H * X; };
auto dhdx = [H](const State &) { return H; };
Eigen::Matrix<double, 3, 3> R;
- R.setZero();
- R.diagonal() << 0.01, 0.01, 0.01;
+ R.setIdentity();
+ R *= 1e-3;
Input U(0, 0);
EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kTheta));
const double starting_p_norm = ekf_.P().norm();
for (int ii = 0; ii < 100; ++ii) {
ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_);
}
- EXPECT_NEAR(Z(0, 0), ekf_.X_hat(StateIdx::kTheta), 1e-3);
+ EXPECT_NEAR(Z(0, 0), ekf_.X_hat(StateIdx::kX), 1e-3);
+ EXPECT_NEAR(Z(1, 0), ekf_.X_hat(StateIdx::kY), 1e-3);
+ EXPECT_NEAR(Z(2, 0), ekf_.X_hat(StateIdx::kTheta), 1e-3);
const double ending_p_norm = ekf_.P().norm();
// Due to corrections, noise should've decreased.
EXPECT_LT(ending_p_norm, starting_p_norm * 0.95);
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index b45f218..9aa1708 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -88,15 +88,14 @@
enable_ = enable;
if (enable && current_trajectory_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
- if (!current_trajectory_->is_at_end(current_state_)) {
+ if (!IsAtEnd()) {
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
U_ff = current_trajectory_->FFVoltage(current_xva_(0));
}
::Eigen::Matrix<double, 2, 5> K =
current_trajectory_->KForState(state, dt_config_.dt, Q, R);
- ::Eigen::Matrix<double, 5, 1> goal_state =
- current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+ ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
next_U_ = U_ff + U_fb;
@@ -116,7 +115,7 @@
return;
}
if (current_spline_handle_ == current_spline_idx_) {
- if (!current_trajectory_->is_at_end(current_state_)) {
+ if (!IsAtEnd()) {
output->left_voltage = next_U_(0);
output->right_voltage = next_U_(1);
current_xva_ = next_xva_;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index a065167..2a79df6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -26,6 +26,16 @@
// TODO(alex): What status do we need?
void PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const;
+
+ // Accessor for the current goal state, pretty much only present for debugging
+ // purposes.
+ Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
+ return current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+ }
+
+ bool IsAtEnd() const {
+ return current_trajectory_->is_at_end(current_state_);
+ }
private:
void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);