Add pi localizer binary
Also, run the logger on the imu node.
Change-Id: Iea96906429364777068c95285891ea46644874dc
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
index dd2e67d..3c68e59 100644
--- a/y2022/control_loops/localizer/localizer.cc
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -23,16 +23,6 @@
}
return vector;
}
-
-#if 0
-Eigen::Matrix<double, 3, 3> AxisToMatrix(const Eigen::Vector3d &vec) {
- const double rotation_norm = vec.norm();
- return rotation_norm < 1e-5
- ? Eigen::Matrix<double, 3, 3>::Identity()
- : Eigen::AngleAxis<double>(rotation_norm, vec / rotation_norm)
- .toRotationMatrix();
-}
-#endif
} // namespace
ModelBasedLocalizer::ModelBasedLocalizer(
@@ -43,6 +33,8 @@
.plant()
.coefficients()),
down_estimator_(dt_config) {
+ CHECK_EQ(branches_.capacity(), static_cast<size_t>(std::chrono::seconds(1) /
+ kNominalDt / kBranchPeriod));
if (dt_config_.is_simulated) {
down_estimator_.assume_perfect_gravity();
}
@@ -394,7 +386,11 @@
}
new_branch.accumulated_divergence = 0.0;
- branches_.Push(new_branch);
+ ++branch_counter_;
+ if (branch_counter_ % kBranchPeriod == 0) {
+ branches_.Push(new_branch);
+ branch_counter_ = 0;
+ }
last_residual_ = model_divergence;