Move y2020 localizer to use floats
This change seems to save ~20-30% of the current drivetrain CPU usage.
I experimented with changing the down estimator to use floats, but the
effects were negligible.
Change-Id: I19edb0431ba03414a890342122db781dc6a7ed51
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index a33ff22..3f553bb 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -101,8 +101,8 @@
// frame.
// I.e., imu_transform * imu_readings will give the imu readings in the
// robot frame.
- Eigen::Matrix<double, 3, 3> imu_transform =
- Eigen::Matrix<double, 3, 3>::Identity();
+ Eigen::Matrix<Scalar, 3, 3> imu_transform =
+ Eigen::Matrix<Scalar, 3, 3>::Identity();
// True if we are running a simulated drivetrain.
bool is_simulated = false;
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index cb50165..e03d581 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -163,7 +163,7 @@
// Currently, we use the drivetrain config for modelling constants
// (continuous time A and B matrices) and for the noise matrices for the
// encoders/gyro.
- HybridEkf(const DrivetrainConfig<Scalar> &dt_config)
+ HybridEkf(const DrivetrainConfig<double> &dt_config)
: dt_config_(dt_config),
velocity_drivetrain_coefficients_(
dt_config.make_hybrid_drivetrain_velocity_loop()
@@ -458,6 +458,19 @@
StateSquare A_c = AForState(*state);
StateSquare A_d;
StateSquare Q_d;
+ // TODO(james): By far the biggest CPU sink in the localization appears to
+ // be this discretization--it's possible the spline code spikes higher,
+ // but it doesn't create anywhere near the same sustained load. There
+ // are a few potential options for optimizing this code, but none of
+ // them are entirely trivial, e.g. we could:
+ // -Reduce the number of states (this function grows at O(kNStates^3))
+ // -Adjust the discretization function itself (there're a few things we
+ // can tune there).
+ // -Try to come up with some sort of lookup table or other way of
+ // pre-calculating A_d and Q_d.
+ // I also have to figure out how much we care about the precision of
+ // some of these values--I don't think we care much, but we probably
+ // do want to maintain some of the structure of the matrices.
controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &Q_d, &A_d);
*state = RungeKuttaU(
@@ -496,9 +509,9 @@
state, P);
}
- DrivetrainConfig<Scalar> dt_config_;
+ DrivetrainConfig<double> dt_config_;
State X_hat_;
- StateFeedbackHybridPlantCoefficients<2, 2, 2, Scalar>
+ StateFeedbackHybridPlantCoefficients<2, 2, 2, double>
velocity_drivetrain_coefficients_;
StateSquare A_continuous_;
StateSquare Q_continuous_;
@@ -637,9 +650,9 @@
// too much when we have accelerometer readings available.
B_continuous_.setZero();
B_continuous_.template block<1, 2>(kLeftVelocity, kLeftVoltage) =
- vel_coefs.B_continuous.row(0);
+ vel_coefs.B_continuous.row(0).template cast<Scalar>();
B_continuous_.template block<1, 2>(kRightVelocity, kLeftVoltage) =
- vel_coefs.B_continuous.row(1);
+ vel_coefs.B_continuous.row(1).template cast<Scalar>();
A_continuous_.template block<kNStates, 2>(0, kLeftVoltageError) =
B_continuous_.template block<kNStates, 2>(0, kLeftVoltage);
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index ab8afd0..7945e5e 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -74,8 +74,8 @@
// pitch/roll components of the rotation. Ignores the bottom row.
TypedPose(const Eigen::Matrix<Scalar, 4, 4> &H) {
pos_ = H.template block<3, 1>(0, 3);
- const Eigen::Vector3d rotated_x =
- H.template block<3, 3>(0, 0) * Eigen::Vector3d::UnitX();
+ const Eigen::Matrix<Scalar, 3, 1> rotated_x =
+ H.template block<3, 3>(0, 0) * Eigen::Matrix<Scalar, 3, 1>::UnitX();
theta_ = std::atan2(rotated_x.y(), rotated_x.x());
}