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/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 32a78da..341f304 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -27,8 +27,8 @@
// effectively, things started to become unstable.
class Localizer : public frc971::control_loops::drivetrain::LocalizerInterface {
public:
- typedef frc971::control_loops::TypedPose<double> Pose;
- typedef frc971::control_loops::drivetrain::HybridEkf<double> HybridEkf;
+ typedef frc971::control_loops::TypedPose<float> Pose;
+ typedef frc971::control_loops::drivetrain::HybridEkf<float> HybridEkf;
typedef typename HybridEkf::State State;
typedef typename HybridEkf::StateIdx StateIdx;
typedef typename HybridEkf::StateSquare StateSquare;
@@ -37,7 +37,10 @@
Localizer(aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
&dt_config);
- State Xhat() const override { return ekf_.X_hat(); }
+ frc971::control_loops::drivetrain::HybridEkf<double>::State Xhat()
+ const override {
+ return ekf_.X_hat().cast<double>();
+ }
frc971::control_loops::drivetrain::TrivialTargetSelector *target_selector()
override {
return &target_selector_;
@@ -57,8 +60,10 @@
bool /*reset_theta*/) override {
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
- ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
- right_encoder, 0, 0, 0, 0, 0, 0).finished(),
+ ekf_.ResetInitialState(t,
+ (HybridEkf::State() << x, y, theta, left_encoder, 0,
+ right_encoder, 0, 0, 0, 0, 0, 0)
+ .finished(),
ekf_.P());
};