Check for instability in trajectory generation
Under some circumstances the solver ends up diverging and this then
causes problems which are non-intuitive to debug.
Ideally we'd actually *fix* the divergence, but for now just detect it.
Change-Id: Id4ae2ce1efe3e45c4187bd1dac72360ed82f477d
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 163311a..2fd6381 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -822,6 +822,7 @@
const Eigen::Matrix<double, 2, 5> K = RBPBinv * APB.transpose();
plan_gains_[i].second = K.cast<float>();
P = AP * A_discrete - APB * K + Q;
+ CHECK_LT(P.norm(), 1e30) << "LQR calculations became unstable.";
}
}