Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-05-29 21:41:50 -07:00
parent 25eacfa226 ca05ffa1b9
commit 6c16e846fa
347 changed files with 18562 additions and 11557 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
wpimath/src/main/native/include/frc/system/NumericalJacobian.h
View File

@@ -34,6 +34,36 @@ auto NumericalJacobian(F&& f, const Vectord<Cols>& 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 <typename F>
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<States>& x,
[&](const Vectord<States>& 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 <typename F, typename... Args>
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<States>& x,
[&](const Vectord<Inputs>& 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 <typename F, typename... Args>
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