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/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);