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

[wpilib] Add pose estimators (#2867)

Browse Source 
Pose and state estimators can filter latency-compensated global measurements and fuse them with state-space drivetrain model information to estimate robot position. They are drop-in replacements for the existing odometry classes.

Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Declan Freeman-Gleason
2020-11-28 17:35:35 -05:00
committed by GitHub
parent 3413bfc06a
commit bc8f338771
58 changed files with 4958 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/native/cpp/StateSpaceUtil.cpp
View File

@@ -9,6 +9,18 @@
namespace frc {
Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose) {
return frc::MakeMatrix<3, 1>(pose.Translation().X().to<double>(),
pose.Translation().Y().to<double>(),
pose.Rotation().Radians().to<double>());
}
Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose) {
return frc::MakeMatrix<4, 1>(pose.Translation().X().to<double>(),
pose.Translation().Y().to<double>(),
pose.Rotation().Cos(), pose.Rotation().Sin());
}
template <>
bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B) {