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

[wpimath] Add dynamic size support for numerical jacobian computation (#7959)

Browse Source
This commit is contained in:
Lucien Morey
2025-05-06 14:55:51 +10:00
committed by GitHub
parent b96264f042
commit e1da917270
2 changed files with 100 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
View File

@@ -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));
}