WPILibC++ 2024.3.2
NumericalJacobian.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include "frc/EigenCore.h"
8
9namespace frc {
10
11/**
12 * Returns numerical Jacobian with respect to x for f(x).
13 *
14 * @tparam Rows Number of rows in result of f(x).
15 * @tparam Cols Number of columns in result of f(x).
16 * @param f Vector-valued function from which to compute Jacobian.
17 * @param x Vector argument.
18 */
19template <int Rows, int Cols, typename F>
20auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
21 constexpr double kEpsilon = 1e-5;
23 result.setZero();
24
25 // It's more expensive, but +- epsilon will be more accurate
26 for (int i = 0; i < Cols; ++i) {
27 Vectord<Cols> dX_plus = x;
28 dX_plus(i) += kEpsilon;
29 Vectord<Cols> dX_minus = x;
30 dX_minus(i) -= kEpsilon;
31 result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
32 }
33
34 return result;
35}
36
37/**
38 * Returns numerical Jacobian with respect to x for f(x, u, ...).
39 *
40 * @tparam Rows Number of rows in result of f(x, u, ...).
41 * @tparam States Number of rows in x.
42 * @tparam Inputs Number of rows in u.
43 * @tparam F Function object type.
44 * @tparam Args... Types of remaining arguments to f(x, u, ...).
45 * @param f Vector-valued function from which to compute Jacobian.
46 * @param x State vector.
47 * @param u Input vector.
48 * @param args Remaining arguments to f(x, u, ...).
49 */
50template <int Rows, int States, int Inputs, typename F, typename... Args>
52 const Vectord<Inputs>& u, Args&&... args) {
53 return NumericalJacobian<Rows, States>(
54 [&](const Vectord<States>& x) { return f(x, u, args...); }, x);
55}
56
57/**
58 * Returns numerical Jacobian with respect to u for f(x, u, ...).
59 *
60 * @tparam Rows Number of rows in result of f(x, u, ...).
61 * @tparam States Number of rows in x.
62 * @tparam Inputs Number of rows in u.
63 * @tparam F Function object type.
64 * @tparam Args... Types of remaining arguments to f(x, u, ...).
65 * @param f Vector-valued function from which to compute Jacobian.
66 * @param x State vector.
67 * @param u Input vector.
68 * @param args Remaining arguments to f(x, u, ...).
69 */
70template <int Rows, int States, int Inputs, typename F, typename... Args>
72 const Vectord<Inputs>& u, Args&&... args) {
73 return NumericalJacobian<Rows, Inputs>(
74 [&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
75}
76
77} // namespace frc
Definition: AprilTagPoseEstimator.h:15
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21
auto NumericalJacobian(F &&f, const Vectord< Cols > &x)
Returns numerical Jacobian with respect to x for f(x).
Definition: NumericalJacobian.h:20
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
auto NumericalJacobianU(F &&f, const Vectord< States > &x, const Vectord< Inputs > &u, Args &&... args)
Returns numerical Jacobian with respect to u for f(x, u, ...).
Definition: NumericalJacobian.h:71
auto NumericalJacobianX(F &&f, const Vectord< States > &x, const Vectord< Inputs > &u, Args &&... args)
Returns numerical Jacobian with respect to x for f(x, u, ...).
Definition: NumericalJacobian.h:51
static constexpr const unit_t< compound_unit< charge::coulomb, inverse< substance::mol > > > F(N_A *e)