Optimize discretization of A/Q for EKF
Because we are recomputing the discretization of A/Q for every single
step in the EKF, it had been taking up ~70-80% of the total computation
time of the entire EKF. This knocks it down to something like 50% of the
total EKF computation time (for ~3-4x speed improvements overall).
Because the discretizations of A/Q are dependent on both dt and on the
heading of the robot, they are not trivially cacheable. Future
improvements would be:
-Only recalculate discretization when the timestep changes and assume
that the heading estimate will only change by minute amounts and
so we don't need to redescritize unless dt has changed.
-Figure out how to cache 7x7 components of each that should stay
constant for constant dt.
-Determine whether any convenient relationships hold with respect to
dt/heading that would let us make a reasonably small interpolation
table.
Change-Id: I8838d4837f380a2411ab9da99d542cdbe9999e41
diff --git a/frc971/control_loops/c2d_test.cc b/frc971/control_loops/c2d_test.cc
index dcdb048..a0c4deb 100644
--- a/frc971/control_loops/c2d_test.cc
+++ b/frc971/control_loops/c2d_test.cc
@@ -69,6 +69,21 @@
<< "\nQ_d_integrated:\n" << Q_d_integrated;
}
+// Tests that the "fast" discretization produces nearly identical results.
+TEST_F(C2DTest, DiscretizeQAFast) {
+ Eigen::Matrix<double, 2, 2> Q_d;
+ Eigen::Matrix<double, 2, 2> Q_d_fast;
+ Eigen::Matrix<double, 2, 2> A_d;
+ Eigen::Matrix<double, 2, 2> A_d_fast;
+ Eigen::Matrix<double, 2, 1> B_d;
+ const auto dt = ::std::chrono::seconds(1);
+ DiscretizeQ(Q_continuous, A_continuous, dt, &Q_d);
+ C2D(A_continuous, B_continuous, dt, &A_d, &B_d);
+ DiscretizeQAFast(Q_continuous, A_continuous, dt, &Q_d_fast, &A_d_fast);
+ EXPECT_LT((Q_d - Q_d_fast).norm(), 1e-20);
+ EXPECT_LT((A_d - A_d_fast).norm(), 1e-20);
+}
+
} // namespace testing
} // namespace controls
} // namespace frc971