diff --git a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h index c4cb6959dd..c6f5719a31 100644 --- a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h +++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h @@ -34,6 +34,36 @@ auto NumericalJacobian(F&& f, const Vectord& x) { return result; } +/** + * Returns numerical Jacobian with respect to x for f(x). + * + * @param f Vector-valued function from which to compute Jacobian. + * @param x Vector argument. + */ + +template +Eigen::MatrixXd NumericalJacobian(F&& f, const Eigen::VectorXd& x) { + constexpr double kEpsilon = 1e-5; + Eigen::MatrixXd result; + + // It's more expensive, but +- epsilon will be more accurate + for (int i = 0; i < x.rows(); ++i) { + Eigen::VectorXd dX_plus = x; + dX_plus(i) += kEpsilon; + Eigen::VectorXd dX_minus = x; + dX_minus(i) -= kEpsilon; + Eigen::VectorXd partialDerivative = + (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0); + if (i == 0) { + result.resize(partialDerivative.rows(), x.rows()); + result.setZero(); + } + result.col(i) = partialDerivative; + } + + return result; +} + /** * Returns numerical Jacobian with respect to x for f(x, u, ...). * @@ -54,6 +84,23 @@ auto NumericalJacobianX(F&& f, const Vectord& x, [&](const Vectord& x) { return f(x, u, args...); }, x); } +/** + * Returns numerical Jacobian with respect to x for f(x, u, ...). + * + * @tparam F Function object type. + * @tparam Args... Types of remaining arguments to f(x, u, ...). + * @param f Vector-valued function from which to compute Jacobian. + * @param x State vector. + * @param u Input vector. + * @param args Remaining arguments to f(x, u, ...). + */ +template +auto NumericalJacobianX(F&& f, const Eigen::VectorXd& x, + const Eigen::VectorXd& u, Args&&... args) { + return NumericalJacobian( + [&](const Eigen::VectorXd& x) { return f(x, u, args...); }, x); +} + /** * Returns numerical Jacobian with respect to u for f(x, u, ...). * @@ -74,4 +121,21 @@ auto NumericalJacobianU(F&& f, const Vectord& x, [&](const Vectord& u) { return f(x, u, args...); }, u); } +/** + * Returns numerical Jacobian with respect to u for f(x, u, ...). + * + * @tparam F Function object type. + * @tparam Args... Types of remaining arguments to f(x, u, ...). + * @param f Vector-valued function from which to compute Jacobian. + * @param x State vector. + * @param u Input vector. + * @param args Remaining arguments to f(x, u, ...). + */ +template +auto NumericalJacobianU(F&& f, const Eigen::VectorXd& x, + const Eigen::VectorXd& u, Args&&... args) { + return NumericalJacobian( + [&](const Eigen::VectorXd& u) { return f(x, u, args...); }, u); +} + } // namespace frc diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp index 513684b755..0529fe285d 100644 --- a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp +++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp @@ -28,6 +28,25 @@ TEST(NumericalJacobianTest, Bu) { EXPECT_TRUE(newB.isApprox(B)); } +Eigen::VectorXd AxBuFn_DynamicSize(const Eigen::VectorXd& x, + const Eigen::VectorXd& u) { + return A * x + B * u; +} + +// Test that we can recover A from AxBuFn() pretty accurately +TEST(NumericalJacobianTest, Ax_DynamicSize) { + Eigen::MatrixXd newA = frc::NumericalJacobianX( + AxBuFn_DynamicSize, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); + EXPECT_TRUE(newA.isApprox(A)); +} + +// Test that we can recover B from AxBuFn() pretty accurately +TEST(NumericalJacobianTest, Bu_DynamicSize) { + Eigen::MatrixXd newB = frc::NumericalJacobianU( + AxBuFn_DynamicSize, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); + EXPECT_TRUE(newB.isApprox(B)); +} + frc::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}}; frc::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}}; @@ -49,3 +68,20 @@ TEST(NumericalJacobianTest, Du) { CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); EXPECT_TRUE(newD.isApprox(D)); } + +Eigen::VectorXd CxDuFn_DynamicSize(const Eigen::VectorXd& x, + const Eigen::VectorXd& u) { + return C * x + D * u; +} + +TEST(NumericalJacobianTest, Cx_DynamicSize) { + Eigen::MatrixXd newC = frc::NumericalJacobianX( + CxDuFn_DynamicSize, Eigen::VectorXd::Zero(4), Eigen::VectorXd::Zero(2)); + EXPECT_TRUE(newC.isApprox(C)); +} + +TEST(NumericalJacobianTest, Du_DynamicSize) { + Eigen::MatrixXd newD = frc::NumericalJacobianU( + CxDuFn_DynamicSize, Eigen::VectorXd::Zero(4), Eigen::VectorXd::Zero(2)); + EXPECT_TRUE(newD.isApprox(D)); +}