Squashed 'third_party/allwpilib/' changes from 83f1860047..f1a82828fe f1a82828fe [wpiutil] Add DataLog and DataLogManager Stop() (#5860) 2a04e12c6f [apriltag] AprilTagFieldLayout: Add accessors for origin and field dimensions (#5869) 33e0089afb Cleanup usages of std::function<void(void)> (#5864) d06fa633d5 [build] Fix protobuf generation when building with make (#5867) 049732afb8 [cscore] Make camera connection logging clearer (#5866) 87f7c19f90 [wpimath] Make InterpolatingDoubleTreeMap constructor public (#5865) 6b53ef47cf [wpimath] Don't recreate TrapezoidProfile in ProfiledPIDController calculate() (#5863) 8a3a268ae6 [commands] Add finallyDo with zero-arg lambda (#5862) 1c35d42cd0 [wpilib] Pop diagnostic for deprecated function use (#5859) ddc8db6c26 [wpimath] Add feedforward constant constructor to ElevatorSim (#5823) c6aff2c431 [upstream_utils] Update to LLVM 17.0.4 (#5855) a9c5b18a39 [build] Update OpenCV to 2024-4.8.0-2 (#5854) 9540b6922d [hal] Add CAN IDs for AndyMark and Vivid Hosting (#5852) 83a7d33c47 [glass] Improve display of protobuf/struct type strings (#5850) a4a8ad9c75 [commands] Make Java SelectCommand generic (#5849) 9eecf2a456 [build] Add CMake option to build Java sources jars (#5768) 9536a311cb [wpilib] Add support for the PS5 DualSense controller (#5257) 8d5e6737fc [wpilibc] SolenoidSim: Add virtual destructor (#5848) 07e13d60a2 [ntcore] Fix write_impl (#5847) 1713386869 [wpiutil] ProtobufMessageDatabase: Fix out-of-order Add() rebuild (#5845) 35472f5fc9 [ntcore] Fix a use-after-free in client close (#5844) ed168b522c [ntcore] Disable buf pool when asan is enabled (#5843) 3e7ba2cc6f [wpinet] WebSocket: Fix write behavior (#5841) 80c47da237 [sim] Disable the robot program when DS disconnects (#5818) abe1cec90c [wpilib] Update Usage Reporting ResourceType from NI Libraries (#5842) cdf981abba [glass] Fix position of data type in NT view (#5840) 04dcd80adb [build] Publish unit tests for examples (#5838) 49920234ac [build] Fix checkstyle rules to allow Windows paths (#5839) 366b715942 [wpilib] Fix SendableChooser test (#5835) 3ba501f947 [commands] Java: Fix CommandXboxController.leftTrigger() parameter order (#5831) ec569a58ef [wpimath] Make KalmanTypeFilter interface public (#5830) b91317fd36 [wpiutil] DataLog.addSchema(): Don't add into a set view (#5829) 2ab4fcbc24 [wpiutil] ProtobufMessageDatabase: Clear messages first (#5827) 98c14f1692 [wpimath] Add EKF/UKF u-y-R correct overload (#5832) 60bcdeded9 [ci] Disable java in sanitizer builds (#5833) c87f8fd538 [commands] Add DeferredCommand (#5566) ad80eb3a0b [ci] Update actions for comment-command (#5824) c7d6ad5a0b [ntcore] WebSocketConnection: Use weak capture (#5822) 8a8e220792 [simgui] Add 'Invalid' option for AllianceStation (#5820) cfc6a47f76 [sim] DS plugin: Fix off-by-one error when setting alliance station (#5819) 8efa586ace [ntcore] Don't check type string on publishing an entry (#5816) 23ea188e60 [glass] Add protobuf decode error log message (#5812) 928e87b4f4 [build] Add combined test meta-task (#5813) 63ef585d4b [wpiutil] Fix compilation of MathExtras.h on Windows with /sdl (#5809) b03a7668f9 [build] Windows CMake/vcpkg fixes (#5807) 3f08bcde54 [hal] Fix HAL AllianceStation on rio (#5811) 196d963dc4 [ntcore] Fix off-by-one error in stream write (#5810) f4cbcbc984 Fix typos (NFC) (#5804) ec0f7fefb0 [myrobot] Update the myRobot JRE (#5805) 3d618bdbfd [wpiutil] Fix Java struct array unpacking (#5801) 1fa7445667 [ntcore] Check for valid client in incoming text and binary (#5799) 269b9647da [ci] Update JDK for combine step (#5794) bee32f080e [docs] Add wpiunits to JavaDocs (#5793) 25dad5a531 [wpinet] TCPConnector_parallel: Don't use thread_local (#5791) 4a93581f1a [build] cmake: use default library type for libglassnt, libglass, wpigui, and imgui (#5797) abb2857e03 [wpilib] Counter: Fix default distance per pulse, add distance and rate to C++ (#5796) b14a61e1c0 [readme] Add link to QuickBuffers release page (#5795) cf54d9ccb7 [wpiutil, ntcore] Add structured data support (#5391) ecb7cfa9ef [wpimath] Add Exponential motion profile (#5720) 7c6fe56cf2 [ntcore] Fix crash on disconnect (#5788) 85147bf69e [wpinet] WebSocketSerializer: Fix UB (#5787) 244163acad [wpinet] uv::Stream::TryWrite(): Return 0 on EAGAIN (#5784) 820728503d [hal] Remove extra semicolon in RoboRioData (#5786) 45f307d87e [upstream_utils] Upgrade to LLVM 17.0.3 (#5785) 4ce4d63efc [wpilibj] Fix RobotBase.isSimulation() (#5783) 579007ceb3 [commands] Add requirements parameter to Commands.idle() (#5774) 3f3a169149 [wpilib] Make physics sim setState() functions public (#5779) 7501e4ac88 [wpilib] Close sim device in ADIS IMUs (#5776) 99630d2e78 [wpimath] Upgrade to EJML 0.43.1 (#5778) 02cbbc997d [wpimath] Make Vector-Vector binary operators return Vector (#5772) ed93889e17 [examples] Fix typo in TimesliceRobot example name (#5773) da70e4c262 [docs] Add jinja2 to CMake prerequisites (#5771) e814595ea7 [wpimath] Add ChassisSpeeds.fromRobotRelativeSpeeds() (#5744) f98c943445 [wpimath] LinearSystemId: Add DCMotorSystem overload (#5770) b3eb64b0f7 [wpiutil] ct_string: Use inline namespace for literals (#5767) 7d9ba256c2 Revert "[build] Add CMake option to build Java source jars (#5756)" (#5766) 1f6492e3d8 [sysid] Update JSON library usage (#5765) 638f04f626 [wpiutil] Add protobuf to thirdparty sources (#5746) 210255bfff [wpiutil] Update json to 3.11.2 (#5680) 896772c750 [wpimath] Add DCMotor functions for Kraken X60 and Neo Vortex (#5759) fd427f6c82 [wpimath] Fix hardcoded module count in SwerveDriveKinematics.resetHeading() (#5762) c0b4c6cce6 [wpimath] Add overloads for Transform2d and Transform3d (#5757) 9a0aafd8ab [examples] Make swerve examples multiply desired module speeds by cosine of heading error (#5758) 1c724884ca [build] Add CMake option to build Java source jars (#5756) 5b0db6b93e [ci] Forward CI as well (#5755) f8cbbbac12 [ci] Take 2 on passing GITHUB_REF (#5754) b9944be09c [ci] Pass GITHUB_REF to docker container (#5753) de5e4eda6c [build] Update apriltag, libssh, googletest for 2024 (#5752) 227e660e20 [upstream_utils] Upgrade to LLVM 17.0.2 (#5750) 36f94c9f21 [commands,romi,xrp] Add frcYear to vendordep (#5747) 741d166457 [glass] NT view: enhance array support (#5732) 1d23513945 [ntcore] Fix string array value comparison (#5745) ff1849052e [commands] Make command scheduling order consistent (#5470) 58e8474368 [build] Disable armsimulation unit test (#5739) fb07b0da49 [examples] Add XRP C++ Examples and Templates (#5743) 81893ad73d Run wpiformat with clang-format 17 (#5740) faa1e665ba [wpimath] Add ElevatorFeedforward.calculate(currentV, nextV) overload (#5715) a789632052 [build] Update to native utils 2024.3.1 (#5738) 8f60ab5182 [build] Update OpenCV to 2024-4.8.0-1 (#5737) 33243f982b [wpimath] Expand Quaternion class with additional operators (#5600) 420f2f7c80 [ntcore] Add RTT-only subprotocol (#5731) 2b63e35ded [ntcore] Fix moving outgoing queue to new period (#5735) be939cb636 [ntcore] Fix notification of SetDefaultEntryValue (#5733) 69a54de202 [build] Update enterprise plugin (#5730) fef03a3ff5 [commands] Clean up C++ includes after Requirements was added (#5719) 8b7c6852cf [ntcore] Networking improvements (#5659) 1d19e09ca9 [wpiutil] Set WPI_{UN}IGNORE_DEPRECATED to empty when all else fails (#5728) 58141d6eb5 [wpilib] Make BooleanEvent more consistent (#5436) 6576d9b474 [wpilib] SendableChooser: implement Sendable instead of NTSendable (#5718) a4030c670f [build] Update to gradle 8.4, enable win arm builds (#5727) 0960f11eba [wpinet] Revert removal of uv_clock_gettime() (#5723) cb1bd0a3be [wpiutil] Get more precise system time on Windows (#5722) 4831277ffe [wpigui] Fix loading a maximized window on second monitor (#5721) 3eb372c25a [wpiutil] SendableBuilder: Add PublishConst methods (#5158) 1fec8596a4 [ci] Fix -dirty version (#5716) f7e47d03f3 [build] Remove unnecessary CMake config installs (#5714) a331ed2374 [sysid] Add SysId (#5672) 8d2cbfce16 [wpiutil] DataLog: Stop logging if insufficient free space (#5699) 48facb9cef [ntcoreffi] Add DataLogManager (#5702) aecbcb08fc [ntcore] Correctly start DataLog for existing publishers (#5703) 5e295dfbda [wpiutil] DataLog: Limit total buffer allocation (#5700) c7c7e05d9d [ci] Unbreak combiner (#5698) c92bad52cb [wpilib] DataLogManager: Use system time valid function (#5697) d404af5f24 [wpilib] RobotController: Add isSystemTimeValid() (#5696) e56f1a3632 [ci] Run combine but skip all steps (#5695) 8f5bcad244 [ci] Use sccache for cmake builds (#5692) 703dedc4a6 [ci] Upgrade get-cmake action to fix node12 deprecation warning (#5694) c69a0d7504 [ci] Don't run example unit test that segfaults (#5693) 66358d103e Add menu items for online docs to GUI tools (#5689) 4be8384a76 [ci] Disable combine on PR builds (#5691) 90288f06a6 [ci] Fix Gradle disk space issues (#5688) 9e9583412e [wpigui] Make wpi::gui::OpenURL() fork the process first (#5687) d4fcd80b7b [ci] Gradle: Use container only for build step (#5684) 7b70e66772 [outlineviewer] Fix thirdparty library include sorting (#5683) 5f651df5d5 [build] Clean up Gradle configs (#5685) 65b26738d5 Add CMakeSettings.json to gitignore (#5682) d0305951ad Fix GitHub inline warnings (#5681) e8d4a20331 [build][cmake] Fix windows tests and re-enable CI tests (#5674) 2b58bbde0b [xrp] Add Reflectance sensor and rangefinder classes (#5673) dd5612fbee [json] Add forward definition header (#5676) eab44534c3 [wpimath] Remove unused SmallString include (#5677) 5ab54ff760 Replace wpi::raw_istream with wpi::MemoryBuffer (#5675) 1b6ec5a95d [wpiutil] Upgrade to LLVM 17.0.1 (#5482) 07a0d22fe6 [build] Build examples in CMake CI (#5667) 97021f074a [build] Upgrade imgui and implot (#5668) 87ce1e3761 [build] Fix wpilibNewCommands CMake install (#5671) 6ef94de9b5 [wpimath] Add tests for ArmFeedforward and ElevatorFeedforward (#5663) c395b29fb4 [wpinet] Add WebSocket::TrySendFrames() (#5607) c4643ba047 [romi/xrp] Fix version typo in vendordep json (#5664) 51dcb8b55a [examples] Make Romi/XRP Examples use appropriate vendordeps (#5665) daf7702007 [build] Test each example in a new environment (#5662) e67df8c180 [wpilib] Const-qualify EncoderSim getters (#5660) 7be290147c [wpiutil] Refactor SpanMatcher and TestPrinters from ntcore (#5658) 9fe258427a [commands] Add proxy factory to Commands (#5603) 633c5a8a22 [commands] Add C++ Requirements struct (#5504) b265a68eea [commands] Add interruptor parameter to onCommandInterrupt callbacks (#5461) e93c233d60 [ntcore] Compute Value memory size when creating value (#5657) 5383589f99 [wpinet] uv::Request: Return shared_ptr from Release() (#5656) 40b552be4a [wpinet] uv::Stream: Return error from TryWrite() (#5655) 202a75fe08 [wpinet] RequestImpl: Avoid infinite loop in shared_from_this() (#5654) 8896515eb7 [wpinet] uv::Buffer: Add bytes() accessor (#5653) ae59a2fba2 [wpinet] uv::Error: Change default error to 0 (#5652) 3b51ecc35b [wpiutil] SpanExtras: Add take_back and take_front (#5651) 17f1062885 Replace std::snprintf() with wpi::format_to_n_c_str() (#5645) bb39900353 [romi/xrp] Add Romi and XRP Vendordeps (#5644) cb99517838 [build] cmake: Use default install location on windows for dlls (#5580) 25b0622d4c [build] Add Windows CMake CI (#5516) 34e7849605 Add warning to development builds instructions (NFC) (#5646) e9e611c9d8 [cameraserver] Remove CameraServer.SetSize() (#5650) 94f58cc536 [wpilib] Remove Compressor.Enabled() (#5649) 4da5aee88a [wpimath] Remove SlewRateLimiter 2 argument constructor (#5648) 2e3ddf5502 Update versions in development builds instructions to 2024 (#5647) 19a8850fb1 [examples] Add TimesliceRobot templates (#3683) 9047682202 [sim] Add XRP-specific plugin (#5631) 575348b81c [wpilib] Use IsSimulation() consistently (#3534) 12e2043b77 [wpilib] Clean up Notifier (#5630) 4bac4dd0f4 [wpimath] Move PIDController from frc2 to frc namespace (#5640) 494cfd78c1 [wpiutil] Fix deprecation warning in LLVM for C++23 (#5642) 43a727e868 [apriltag] Make loadAprilTagFieldLayout throw an unchecked exception instead (#5629) ad4b017321 [ci] Use Ninja for faster builds (#5626) 4f2114d6f5 Fix warnings from GCC 13 release build (#5637) e7e927fe26 [build] Also compress debug info for CMake RelWithDebInfo build type (#5638) 205a40c895 [build] Specify zlib for debug info compression (#5636) 707444f000 [apriltag] Suppress -Wtype-limits warning in asserts from GCC 13 (#5635) 3b79cb6ed3 [commands] Revert SubsystemBase deprecation/removal (#5634) bc7f23a632 [build] Compress Linux debug info (#5633) 57b2d6f254 [build] Update to image 2024 v1.0 (#5625) 339ef1ea39 [wpilib] DataLogManager: Warn user if logging to RoboRIO 1 internal storage (#5617) 7a9a901a73 [build] Fix cmake config files (#5624) 298f8a6e33 [wpilib] Add Mechanism2d tests and make Java impl match C++ (#5527) d7ef817bae [apriltag] Update apriltag library (#5619) c3fb31fd0e [docs] Switch to Java 17 api docs (#5613) bd64f81cf9 [build] Run Google tests in release mode in CI (#5615) 66e6bd81ea [wpimath] Cleanup wpimath/algorithms.md (NFC) (#5621) 4fa56fd884 [build] Add missing find_dependency call (#5623) f63d958995 [build] Update to native utils 2024.2.0 (#5601) a9ab08f48b [wpimath] Rename ChassisSpeeds.fromDiscreteSpeeds() to discretize() (#5616) 8e05983a4a [wpimath] Add math docs to plant inversion feedforward internals (NFC) (#5618) 3a33ce918b [ntcore] Add missing StringMap include (#5620) a6157f184d [wpiutil] timestamp: Add ShutdownNowRio (#5610) e9f612f581 [build] Guard policy setting for CMake versions below 3.24 (#5612) 1a6df6fec6 [wpimath] Fix DARE Q decomposition (#5611) 9b3f7fb548 [build] Exclude IntelliJ folders from spotless XML (#5602) 814f18c7f5 [wpimath] Fix computation of C for DARE (A, C) detectability check (#5609) ac23f92451 [hal] Add GetTeamNumber (#5596) a750bee54d [wpimath] Use std::norm() in IsStabilizable() (#5599) 8e2465f8a0 [wpimath] Add arithmetic functions to wheel speeds classes (#5465) 10d4f5b5df [wpimath] Clean up notation in DARE precondition docs (#5595) b2dd59450b [hal] Fix unfinished/incorrect GetCPUTemp functions (#5598) 99f66b1e24 [wpimath] Replace frc/EigenCore.h typedefs with Eigen's where possible (#5597) 383289bc4b [build] Make custom CMake macros use lowercase (#5594) 45e7720ec1 [build] Add error message when downloading files in CMake (#5593) 4e0d785356 [wpimath] ChassisSpeeds: document that values aren't relative to the robot (NFC) (#5551) 3c04580a57 [commands] ProxyCommand: Use inner command name in unique_ptr constructor (#5570) cf19102c4a [commands] SelectCommand: Fix leakage and multiple composition bug (#5571) 171375f440 [ntcoreffi] Link to NI libraries (#5589) 89add5d05b Disable flaky tests (#5591) a8d4b162ab [ntcore] Remove RPC manual tests (#5590) 39a73b5b58 [commands] C++: Add CommandPtr supplier constructor to ProxyCommand (#5572) 36d514eae7 [commands] Refactor C++ ScheduleCommand to use SmallSet (#5568) 52297ffe29 [commands] Add idle command (#5555) 67043a8eeb [wpimath] Add angular jerk unit (#5582) 51b0fb1492 [wpimath] Fix incorrect header inclusion in angular_acceleration.h (#5587) b7657a8e28 [wpimath] Split WPIMathJNI into logical chunks (#5552) ea17f90f87 [build] Fix tool builds with multiple arm platforms installed (#5586) f1d7b05723 [wpimath] Clean up unit formatter (#5584) d7264ff597 Replace wpi::errs() usage with fmtlib (#5560) ab3bf39e0e [wpiutil] Upgrade to fmt 10.1.1 (#5585) 165ebe4c79 Upgrade to fmt 10.1.0 (#5326) 8e2a7fd306 Include thirdparty libraries with angle brackets (#5578) e322ab8e46 [wpimath] Fix docs for DARE ABQRN stabilizability check (NFC) (#5579) 360fb835f4 [upstream_utils] Handle edge case in filename matches (#5576) 9d86624c00 [build] Fix CMake configure warnings (#5577) 969979d6c7 [wpiutil] Update to foonathan memory 0.7-3 (#5573) 0d2d989e84 [wpimath] Update to gcem 1.17.0 (#5575) cf86af7166 [wpiutil] Update to mpack 1.1.1 (#5574) a0c029a35b [commands] Fix dangling SelectCommand documentation (NFC) (#5567) 349141b91b [upstream_utils] Document adding a patch (NFC) (#5432) 7889b35b67 [wpimath] Add RamseteController comparison to LTV controller docs (NFC) (#5559) b3ef536677 [build] Ignore nt/sim json files in spotless (#5565) ed895815b5 [build] Compile Java with UTF-8 encoding (#5564) 2e4ad35e36 [wpiutil] jni_util: Add JSpan and CriticalJSpan (#5554) 8f3d6a1d4b [wpimath] Remove discretizeAQTaylor() (#5562) 7c20fa1b18 [wpimath] Refactor DARE tests to reduce RAM usage at compile time (#5557) 89e738262c [ntcore] Limit buffer pool size to 64KB per connection (#5485) 96f7fa662e Upgrade Maven dependencies (#5553) 7a2d336d52 [wpinet] Leak multicast handles during windows shutdown (#5550) f9e2757d8f [wpimath] Use JDoubleArrayRef in all JNI functions (#5546) 0cf6e37dc1 [wpimath] Make LTV controller constructors use faster DARE solver (#5543) 6953a303b3 [build] Fix the windows build with fmt (#5544) 7a37e3a496 [wpimath] Correct Rotation3d::RotateBy doc comment (NFC) (#5541) 186b409e16 [wpimath] Remove internal Eigen header include (#5539) 03764dfe93 [wpimath] Add static matrix support to DARE solver (#5536) 394cfeadbd [wpimath] Use SDA algorithm instead of SSCA for DARE solver (#5526) a4b7fde767 [wpilib] Add mechanism specific SetState overloads to physics sims (#5534) 8121566258 [wpimath] Fix CoordinateSystem.convert() Transform3d overload (#5532) b542e01a0b [glass] Fix array crash when clearing existing workspace (#5535) e2e1b763b2 [wpigui] Fix PFD file dialogs not closing after window closing (#5530) 86d7bbc4e4 [examples] Add Java Examples and Templates for the XRP (#5529) e8b5d44752 [wpimath] Make Java Quaternion use doubles instead of Vector (#5525) 38c198fa64 [myRobot] Add apriltags to myRobot build (#5528) 00450c3548 [wpimath] Upgrade to EJML 0.42 (#5531) faf3cecd83 [wpimath] Don't copy Matrix and underlying storage in VecBuilder (#5524) 6b896a38dc [build] Don't enforce WITH_FLAT_INSTALL with MSVC (part 2) (#5517) c01814b80e [wpiutil] Add C API for DataLog (#5509) b5bd0771eb [wpimath] Document extrinsic vs intrinsic rotations (NFC) (#5508) 84ed8aec05 [build] Don't enforce WITH_FLAT_INSTALL with MSVC (#5515) 999f677d8c [ntcoreffi] Add WPI_Impl_SetupNowRio to exported symbols (#5510) 338f37d302 Fix header sorting of libssh (#5507) 75cbd9d6d0 [glass] Add background color selector to glass plots (#5506) e2c190487b [examples] Add flywheel bang-bang controller example (#4071) c52dad609e [wpinet] WebSocket: Send pong in response to ping (#5498) e2d17a24a6 [hal] Expose power rail disable and cpu temp functionality (#5477) 3ad5d2e42d [hal,wpiutil] Use HMB for FPGA Timestamps (#5499) b46a872494 [ntcore] Remove pImpl from implementation (#5480) d8c59ccc71 [wpimath] Add tests for MathUtil clamp() and interpolate() (#5501) 0552c8621d [glass,ov] Improve Glass and OutlineViewer title bar message (#5502) 90e37a129f [wpiutil,wpimath] Add generic InterpolatingTreeMap (#5372) d83a6edc20 [wpilib] Update GetMatchTime docs and units (#5232) 6db2c42966 [wpimath] Trajectory: Throw on empty lists of States (#5497) 21439b606c [wpimath] Disallow LTV controller max velocities above 15 m/s (#5495) 7496e0d208 [ntcore] Value: More efficiently store arrays (#5484) 0c93aded8a [wpimath] Change kinematics.ToTwist2d(end - start) to kinematics.ToTwist2d(start, end) (#5493) 815a8403e5 [wpimath] Give infeasible trajectory constraints a better exception message (#5492) 35a8b129d9 [wpimath] Add RotateBy() function to pose classes (#5491) 26d6e68c8f [upstream_utils] Add GCEM to CI (#5483) 6aa469ae45 [wpilib] Document how to create LinearSystem object for physics sim classes (NFC) (#5488) a01b6467d3 [wpimath] Link to docs on LQR and KF tolerances (#5486) d814f1d123 [wpimath] Fix copy-paste error from Pose2d docs (NFC) (#5490) 98f074b072 [wpimath] Add folder prefix to geometry includes (#5489) e9858c10e9 [glass] Add tooltips for NT settings (#5476) 12dda24f06 [examples] Fix C robot template not correctly looping (#5474) fc75d31755 [apriltag] Update apriltaglib (#5475) a95994fff6 [wpiutil] timestamp: Call FPGA functions directly (#5235) 2ba8fbb6f4 [wpimath] Improve documentation for SwerveModulePosition::operator- (#5468) b8cdf97621 [build] Prepare for Windows arm64 builds (#5390) 552f4b76b5 [wpimath] Add FOC-enabled Falcon constants to the DCMotor class (#5469) 1938251436 [examples] Add Feedforward to ElevatorProfiledPid (#5300) 873c2a6c10 [examples] Update ElevatorTrapezoidProfile example (#5466) 99b88be4f3 [wpilib] Reduce usage of NTSendable (#5434) d125711023 [hal] Fix Java REVPH faults bitfield (take 2) (#5464) c3fab7f1f2 [ntcore] Don't update timestamp when value is unchanged (#5356) 5ec7f18bdc [wpilib] EventLoop docs: Remove BooleanEvent references (NFC) (#5463) c065ae1fcf [wpiunits] Add subproject for a Java typesafe unit system (#5371) 44acca7c00 [wpiutil] Add ClassPreloader (#5365) 88b11832ec [hal] Fix Java REVPH faults bitfield (#5148) fb57d82e52 [ntcore] Enhance Java raw value support 3a6e40a44b [wpiutil] Enhance DataLog Java raw value support 8dae5af271 [wpiutil] Add compile-time string utilities (ct_string) (#5462) fc56f8049a [wpilib] DriverStation: Change alliance station to use optional (#5229) ef155438bd [build] Consume libuv via cmake config instead of via pkg-config (#5438) 86e91e6724 [wpimath] Refactor TrapezoidProfile API (#5457) 72a4543493 [wpilib] DutyCycleEncoderSim: Expand API (#5443) 657338715d [wpimath] Add ChassisSpeeds method to fix drifting during compound swerve drive maneuvers (#5425) 1af224c21b Add missing <functional> includes (#5459) 0b91ca6d5a [wpilib] SendableChooser: Add onChange listener (#5458) 6f7cdd460e [wpimath] Pose3d: Switch to JNI for exp and log (#5444) c69e34c80c [wpimath] ChassisSpeeds: Add arithmetic functions (#5293) 335e7dd89d [wpilib] Simulation: Add ctor parameter to set starting state of mechanism sims (#5288) 14f30752ab [wpilib] Deprecate Accelerometer and Gyro interfaces (#5445) 70b60e3a74 [commands] Trigger: Fix method names in requireNonNullParam (#5454) 593767c8c7 [wpimath] Improve Euler angle calculations in gimbal lock (#5437) daf022d3da [build] Make devImplementation inherit from implementation (#5450) 9b8d90b852 [examples] Convert the unitless joystick inputs to actual physical units (#5451) 1f6428ab63 [ntcore] Fix undefined comparison behavior when array is empty (#5448) 17eb9161cd Update code owners for removal of old commands (#5447) 3c4b58ae1e [wpinet] Upgrade to libuv 1.46.0 (#5446) aaea85ff16 [commands] Merge CommandBase into Command and SubsystemBase into Subsystem (#5392) 7ac932996a [ci] Use PAT for workflow dispatch (#5442) efe1987e8b [ci] Trigger pages repo workflow (#5441) 828bc5276f [wpiutil] Upgrade to LLVM 16.0.6 (#5435) 701df9eb87 [ci] Change documentation publish to single-commit (#5440) e5452e3f69 [wpiutil] Add WPICleaner and an example how to use it (#4850) 7a099cb02a [commands] Remove deprecated classes and functions (#5409) b250a03944 [wpilib] Add function to wait for DS Connection (#5230) a6463ed761 [wpiutil] Fix unused variable warning in release build (#5430) f031513470 [ntcore] NetworkTable::GetSubTables(): Remove duplicates (#5076) f8e74e2f7c [hal] Unify PWM simulation Speed, Position, and Raw (#5277) fd5699b240 Remove references to Drake (#5427) e2d385d80a [build] cmake: Respect USE_SYSTEM_FMTLIB (#5429) d37f990ce3 [hal] Fix HAL Relay/Main doc module (NFC) (#5422) a7a8b874ac [docs] Expand HAL_ENUM in doxygen docs (#5421) 3a61deedde [wpimath] Rotation2d: Only use gcem::hypot when constexpr evaluated (#5419) 96145de7db [examples] Fix formatting (NFC) (#5420) fffe6a7b9a [examples] Improve Pneumatics example coverage in Solenoid and RapidReactCmdBot examples (#4998) 6b5817836d [wpimath] Add tolerance for some tests (#5416) 3233883f3e [cscore] Fix warnings on macos arm (#5415) c4fc21838f [commands] Add ConditionalCommand getInterruptionBehavior (#5161) 89fc51f0d4 Add tests for SendableChooser and Command Sendable functionality (#5179) 663bf25aaf [docs] Generate docs for symbols in __cplusplus (#5412) fe32127ea8 [command] Clean up Command doc comments (NFC) (#5321) c1a01569b4 [wpilib][hal] PWM Raw using microseconds (#5283) 1fca519fb4 [wpiutil] Remove remnants of ghc fs and tcb_span libraries (#5411) 90602cc135 [github] Update issue template to collect more project info (#5090) 34412ac57e [build] Exclude files in bin from Spotless (#5410) 61aa60f0e3 [wpilib] Add robot callback that is called when the DS is initially connected (#5231) ebae341a91 [commands] Add test for subsystem registration and periodic (#5408) 5d3a133f9f Remove spaces in NOLINT comments (#5407) 3a0e484691 [wpimath] Fix clang-tidy warnings (#5403) eb3810c765 [wpiutil] Fix clang-tidy warnings (#5406) c4dc697192 [hal] WS Simulation: Add message filtering capability (#5395) 0eccc3f247 [ntcore] Fix clang-tidy warnings (#5405) f4dda4bac0 [hal] Add javadocs for JNI (NFC) (#5298) 1c20c69793 [cscore] Fix clang-tidy warnings (#5404) 1501607e48 [commands] Fix clang-tidy warnings (#5402) 991f4b0f62 [wpimath] PIDController: Add IZone (#5315) f5b0d1484b [wpimath] Add isNear method to MathUtil (#5353) 2ce248f66c [hal] Fix clang-tidy warnings (#5401) 5fc4aee2d2 [wpimath] SwerveDriveKinematics: Rename currentChassisSpeed to desiredChassisSpeed (#5393) 50b90ceb54 [wpimath] SwerveDriveKinematics: Add reset method (#5398) 316cd2a453 [commands] Notify DriverStationSim in CommandTestBaseWithParam (#5400) d4ea5fa902 [cscore] VideoMode: Add equals override (Java) (#5397) d6bd72d738 [wpimath] ProfiledPIDController: Add getConstraints (#5399) 25ad5017a9 [wpimath] Refactor kinematics, odometry, and pose estimator (#5355) 5c2addda0f [doc] Add missing pneumatics docs (NFC) (#5389) c3e04a6ea2 Fix loading tests on macos 12 (#5388) d5ed9fb859 [wpimath] Create separate archive with just units headers (#5383) 901ab693d4 [wpimath] Use UtilityClassTest for more utility classes (#5384) 9d53231b01 [wpilib] DataLogManager: Add warning for low storage space (#5364) d466933963 [wpiutil] Group doxygen into MPack module (#5380) 652d1c44e3 [wpiutil] Upgrade to macOS 12 to remove concept shims (#5379) 6414be0e5d [wpimath] Group units doxygen modules (#5382) 7ab5800487 [wpiutil] Fix docs typo in SmallVector (#5381) 59905ea721 Replace WPI_DEPRECATED() macro with [[deprecated]] attribute (#5373) 753cb49a5e [ntcore] Fix doxygen module in generated C types (NFC) (#5374) 1c00a52b67 [hal] Expose CAN timestamp base clock (#5357) 91cbcea841 Replace SFINAE with concepts (#5361) d57d1a4598 [wpimath] Remove unnecessary template argument from unit formatter (#5367) 5acc5e22aa [wpimath] Only compute eigenvalues with EigenSolvers (#5369) d3c9316a97 extend shuffleboard test timeout (#5377) 1ea868081a [ci] Fix /format command (#5376) 5fac18ff4a Update formatting to clang-format 16 (#5370) a94a998002 [wpimath] Generalize Eigen formatter (#5360) 125f6ea101 [wpimath] Make SwerveDriveKinematics::ToChassisSpeeds() take const-ref argument (#5363) 51066a5a8a [wpimath] Move unit formatters into units library (#5358) 282c032b60 [wpilibc] Add unit-aware Joystick.GetDirection() (#5319) 073d19cb69 [build] Fix CMake warning (#5359) 01490fc77b [wpiutil] DataLog: Add documentation for append methods (NFC) (#5348) c9b612c986 [wpilibcExamples] Make C++ state-space elevator KF and LQR match Java (#5346) eed1e6e3cb [wpimath] Replace DiscretizeAQTaylor() with DiscretizeAQ() (#5344) c976f40364 [readme] Document how to run examples in simulation (#5340) 4d28bdc19e [ci] Update Github Pages deploy action parameters (#5343) e0f851871f [ci] Fix github pages deploy version (#5342) 063c8cbedc Run wpiformat (NFC) (#5341) 96e41c0447 [ci] Update deploy and sshagent actions (#5338) fd294bdd71 [build] Fix compilation with GCC 13 (#5322) d223e4040b [dlt] Add delete without download functionality (#5329) abc19bcb43 [upstream_utils] Zero out commit hashes and show 40 digits in index hashes (#5336) e909f2e687 [build] Update gradle cache repo name (#5334) 52bd5b972d [wpimath] Rewrite DARE solver (#5328) 3876a2523a [wpimath] Remove unused MatrixImpl() function (#5330) c82fcb1975 [wpiutil] Add reflection based cleanup helper (#4919) 15ba95df7e [wpiutil] Use std::filesystem (#4941) 77c2124fc5 [wpimath] Remove Eigen's custom STL types (#4945) 27fb47ab10 [glass] Field2D: Embed standard field images (#5159) 102e4f2566 [wpilib] Remove deprecated and broken SPI methods (#5249) 463a90f1df [wpilib, hal] Add function to read the RSL state (#5312) 7a90475eec [wpilib] Update RobotBase documentation (NFC) (#5320) 218cfea16b [wpilib] DutyCycleEncoder: Fix reset behavior (#5287) 91392823ff [build] Update to gradle 8.1 (#5303) 258b7cc48b [wpilibj] Filesystem.getDeployDirectory(): Strip JNI path from user.dir (#5317) 26cc43bee1 [wpilib] Add documentation to SPI mode enum (NFC) (#5324) ac4da9b1cb [hal] Add HAL docs for Addressable LED (NFC) (#5304) 21d4244cf7 [wpimath] Fix DCMotor docs (NFC) (#5309) 1dff81bea7 [hal] Miscellaneous HAL doc fixes (NFC) (#5306) 7ce75574bf [wpimath] Upgrade to Drake v1.15.0 (#5310) 576bd646ae [hal] Add CANManufacturer for Redux Robotics (#5305) ee3b4621e5 [commands] Add onlyWhile and onlyIf (#5291) 40ca094686 [commands] Fix RepeatCommand calling end() twice (#5261) 9cbeb841f5 [rtns] Match imaging tool capitalization (#5265) a63d06ff77 [examples] Add constants to java gearsbot example (#5248) b6c43322a3 [wpilibc] XboxController: Add return tag to docs (NFC) (#5246) 5162d0001c [hal] Fix and document addressable LED timings (#5272) 90fabe9651 [wpilibj] Use method references in drive class initSendable() (#5251) 24828afd11 [wpimath] Fix desaturateWheelSpeeds to account for negative speeds (#5269) e099948a77 [wpimath] Clean up rank notation in docs (NFC) (#5274) fd2d8cb9c1 [hal] Use std::log2() for base-2 logarithm (#5278) ba8c64bcff [wpimath] Fix misspelled Javadoc parameters in pose estimators (NFC) (#5292) f53c6813d5 [wpimath] Patch Eigen warnings (#5290) 663703d370 [gitattributes] Mark json files as lf text files (#5256) aa34aacf6e [wpilib] Shuffleboard: Keep duplicates on SelectTab() (#5198) 63512bbbb8 [wpimath] Fix potential divide-by-zero in RKDP (#5242) 9227b2166e [wpilibj] DriverStation: Fix joystick data logs (#5240) fbf92e9190 [wpinet] ParallelTcpConnector: don't connect to duplicate addresses (#5169) 2108a61362 [ntcore] NT4 client: close timed-out connections (#5175) 0a66479693 [ntcore] Optimize scan of outgoing messages (#5227) b510c17ef6 [hal] Fix RobotController.getComments() mishandling quotes inside the comments string (#5197) e7a7eb2e93 [commands] WaitCommand: Remove subclass doc note (NFC) (#5200) a465f2d8f0 [examples] Shuffleboard: Correct parameter order (#5204) a3364422fa LICENSE.md: Bump year to 2023 (#5195) df3242a40a [wpimath] Fix NaN in C++ MakeCostMatrix() that takes an array (#5194) 00abb8c1e0 [commands] RamseteCommand: default-initialize m_prevSpeeds (#5188) c886273fd7 [wpilibj] DutyCycleEncoder.setDistancePerRotation(): fix simulation (#5147) 53b5fd2ace [ntcore] Use int64 for datalog type string (#5186) 56b758320f [wpilib] DataLogManager: increase time for datetime to be valid (#5185) 08f298e4cd [wpimath] Fix Pose3d log returning Twist3d NaN for theta between 1E-8 and 1E-7 (#5168) 6d0c5b19db [commands] CommandScheduler.isComposed: Remove incorrect throws clause (NFC) (#5183) 0d22cf5ff7 [wpilib] Fix enableLiveWindowInTest crashing in disabled (#5173) 32ec5b3f75 [wpilib] Add isTestEnabled and minor docs cleanup (#5172) e5c4c6b1a7 [wpimath] Fix invalid iterator access in TimeInterpolatableBuffer (#5138) 099d048d9e [wpimath] Fix Pose3d log returning Twist3d NaN for theta between 1E-9 and 1E-8 (#5143) 4af84a1c12 Fix Typos (NFC) (#5137) ce3686b80d [wpimath] Check LTV controller max velocity precondition (#5142) 4b0eecaee0 [commands] Subsystem: Add default command removal method (#5064) edf4ded412 [wpilib] PH: Revert to 5V rail being fixed 5V (#5122) 4c46b6aff9 [wpilibc] Fix DataLogManager crash on exit in sim (#5125) 490ca4a68a [wpilibc] Fix XboxController::GetBackButton doc (NFC) (#5131) cbb5b0b802 [hal] Simulation: Fix REV PH solenoids 8+ (#5132) bb7053d9ee [hal] Fix HAL_GetRuntimeType being slow on the roboRIO (#5130) 9efed9a533 Update .clang-format to c++20 (#5121) dbbfe1aed2 [wpilib] Use PH voltage to calc Analog pressure switch threshold (#5115) de65a135c3 [wpilib] DutyCycleEncoderSim: Add channel number constructor (#5118) 3e9788cdff [docs] Strip path from generated NT docs (#5119) ecb072724d [ntcore] Client::Disconnect(): actually close connection (#5113) 0d462a4561 [glass] NT view: Change string/string array to quoted (#5111) ba37986561 [ntcore] NetworkClient::Disconnect: Add null check (#5112) 25ab9cda92 [glass,ov] Provide menu item to create topic from root (#5110) 2f6251d4a6 [glass] Set default value when publishing new topic (#5109) e9a7bed988 [wpimath] Add timestamp getter to MathShared (#5091) 9cc14bbb43 [ntcore] Add stress test to dev executable (#5107) 8068369542 [wpinet] uv: Stop creating handles when closing loop (#5102) 805c837a42 [ntcore] Fix use-after-free in server (#5101) fd18577ba0 [commands] Improve documentation of addRequirements (NFC) (#5103) 74dea9f05e [wpimath] Fix exception for empty pose buffer in pose estimators (#5106) 9eef79d638 [wpilib] PneumaticHub: Document range of enableCompressorAnalog (NFC) (#5099) 843574a810 [ntcore] Use wpi::Now instead of loop time for transmit time 226ef35212 [wpinet] WebSocket: Reduce server send frame overhead b30664d630 [ntcore] Reduce initial connection overhead 804e5ce236 [examples] MecanumDrive: Fix axis comment in C++ example (NFC) (#5096) 49af88f2bb [examples] ArmSimulation: Fix flaky test (#5093) d56314f866 [wpiutil] Disable mock time on the Rio (#5092) 43975ac7cc [examples] ArmSimulation, ElevatorSimulation: Extract mechanism to class (#5052) 5483464158 [examples, templates] Improve descriptions (NFC) (#5051) 785e7dd85c [wpilibc] SendableChooser: static_assert copy- and default-constructibility (#5078) e57ded8c39 [ntcore] Improve disconnect error reporting (#5085) 01f0394419 [wpinet] Revert WebSocket: When Close() is called, call closed immediately (#5084) 59be120982 [wpimath] Fix Pose3d exp()/log() and add rotation vector constructor to Rotation3d (#5072) 37f065032f [wpilib] Refactor TimedRobot tests (#5068) 22a170bee7 [wpilib] Add Notifier test (#5070) 2f310a748c [wpimath] Fix DCMotor.getSpeed() (#5061) b43ec87f57 [wpilib] ElevatorSim: Fix WouldHitLimit methods (#5057) 19267bef0c [ntcore] Output warning on property set on unpublished topic (#5059) 84cbd48d84 [ntcore] Handle excludeSelf on SetDefault (#5058) 1f35750865 [cameraserver] Add GetInstance() to all functions (#5054) 8230fc631d [wpilib] Revert throw on nonexistent SimDevice name in SimDeviceSim (#5053) b879a6f8c6 [wpinet] WebSocket: When Close() is called, call closed immediately (#5047) 49459d3e45 [ntcore] Change wire timeout to fixed 1 second (#5048) 4079eabe9b [wpimath] Discard stale pose estimates (#5045) fe5d226a19 [glass] Fix option for debug-level NT logging (#5049) b7535252c2 [ntcore] Don't leak buffers in rare WS shutdown case (#5046) b61ac6db33 [ntcore] Add client disconnect function (#5022) 7b828ce84f [wpimath] Add nearest to Pose2d and Translation2d (#4882) 08a536291b [examples] Improvements to Elevator Simulation Example (#4937) 193a10d020 [wpigui] Limit frame rate to 120 fps by default (#5030) 7867bbde0e [wpilib] Clarify DS functions provided by FMS (NFC) (#5043) fa7c01b598 [glass] Add option for debug-level NT logging (#5007) 2b81610248 [wpiutil] Add msgpack to datalog Python example (#5032) a4a369b8da CONTRIBUTING.md: Add unicodeit CLI to math docs guidelines (#5031) d991f6e435 [wpilib] Throw on nonexistent SimDevice name in SimDeviceSim constructor (#5041) a27a047ae8 [hal] Check for null in getSimDeviceName JNI (#5038) 2f96cae31a [examples] Hatchbots: Add telemetry (#5011) 83ef8f9658 [simulation] GUI: Fix buffer overflow in joystick axes copy (#5036) 4054893669 [commands] Fix C++ Select() factory (#5024) f75acd11ce [commands] Use Timer.restart() (#5023) 8bf67b1b33 [wpimath] PIDController::Calculate(double, double): update setpoint flag (#5021) 49bb1358d8 [wpiutil] MemoryBuffer: Fix GetMemoryBufferForStream (#5017) 9c4c07c0f9 [wpiutil] Remove NDEBUG check for debug-level logging (#5018) 1a47cc2e86 [ntcore] Use full handle when subscribing (#5013) 7cd30cffbc Ignore networktables.json (#5006) 92aecab2ef [commands] Command controllers are not subclasses (NFC) (#5000) 8785bba080 [ntcore] Special-case default timestamps (#5003) 9e5b7b8040 [ntcore] Handle topicsonly followed by value subscribe (#4991) 917906530a [wpilib] Add Timer::Restart() (#4963) 00aa66e4fd [wpimath] Remove extraneous assignments from DiscretizeAB() (#4967) 893320544a [examples] C++ RamseteCommand: Fix units (#4954) b95d0e060d [wpilib] XboxController: Fix docs discrepancy (NFC) (#4993) 008232b43c [ntcore] Write empty persistent file if none found (#4996) 522be348f4 [examples] Rewrite tags (NFC) (#4961) d48a83dee2 [wpimath] Update Wikipedia links for quaternion to Euler angle conversion (NFC) (#4995) 504fa22143 [wpimath] Workaround intellisense Eigen issue (#4992) b2b25bf09f [commands] Fix docs inconsistency for toggleOnFalse(Command) (NFC) (#4978) ce3dc4eb3b [hal] Properly use control word that is in sync with DS data (#4989) 1ea48caa7d [wpilib] Fix C++ ADXRS450 and Java SPI gyro defs (#4988) fb101925a7 [build] Include wpimathjni in commands binaries (#4981) 657951f6dd [starter] Add a process starter for use by the installer for launching tools (#4931) a60ca9d71c [examples] Update AprilTag field load API usage (#4975) f8a45f1558 [wpimath] Remove print statements from tests (#4977) ecba8b99a8 [examples] Fix swapped arguments in MecanumControllerCommand example (#4976) e95e88fdf9 [examples] Add comment to drivedistanceoffboard example (#4877) 371d15dec3 [examples] Add Computer Vision Pose Estimation and Latency Compensation Example (#4901) cb9b8938af [sim] Enable docking in the GUI (#4960) 3b084ecbe0 [apriltag] AprilTagFieldLayout: Improve API shape for loading builtin JSONs (#4949) 27ba096ea1 [wpilib] Fix MOI calculation error in SingleJointedArmSim (#4968) 42c997a3c4 [wpimath] Fix Pose3d exponential and clean up Pose3d logarithm (#4970) 5f1a025f27 [wpilibj] Fix typo in MecanumDrive docs (NFC) (#4969) 0ebf79b54c [wpimath] Fix typo in Pose3d::Exp() docs (NFC) (#4966) a8c465f3fb [wpimath] HolonomicDriveController: Add getters for the controllers (#4948) a7b1ab683d [wpilibc] Add unit test for fast deconstruction of GenericHID (#4953) bd6479dc29 [build] Add Spotless for JSON (#4956) 5cb0340a8c [hal, wpilib] Load joystick values upon code initialization (#4950) ab0e8c37a7 [readme] Update build requirements (NFC) (#4947) b74ac1c645 [build] Add apriltag to C++ cmake example builds (#4944) cf1a411acf [examples] Add example programs for AprilTags detection (#4932) 1e05b21ab5 [wpimath] Fix PID atSetpoint to not return true prematurely (#4906) e5a6197633 [wpimath] Fix SwerveDriveKinematics not initializing a new array each time (#4942) 039edcc23f [ntcore] Queue current value on subscriber creation (#4938) f7f19207e0 [wpimath] Allow multiple vision measurements from same timestamp (#4917) befd12911c [commands] Delete UB-causing rvalue variants of CommandPtr methods (#4923) 34519de60a [commands] Fix spacing in command composition exception (#4924) dc4355c031 [hal] Add handle constructor and name getters for sim devices (#4925) 53d8d33bca [hal, wpilibj] Add missing distance per pulse functions to EncoderSim (#4928) 530ae40614 [apriltag] Explain what April tag poses represent (NFC) (#4930) 79f565191e [examples] DigitalCommunication, I2CCommunication: Add tests (#4865) 2cd9be413f [wpilib, examples] Cleanup PotentiometerPID, Ultrasonic, UltrasonicPID examples (#4893) babb0c1fcf [apriltag] Add 2023 field layout JSON (#4912) 330ba45f9c [wpimath] Fix swerve kinematics util classes equals function (#4907) 51272ef6b3 [fieldImages] Add 2023 field (#4915) 0d105ab771 [commands] Deduplicate command test utils (#4897) cf4235ea36 [wpiutil] Guard MSVC pragma in SymbolExports.h (#4911) 2d4b7b9147 [build] Update opencv version in opencv.gradle (#4909) aec6f3d506 [ntcore] Fix client flush behavior (#4903) bfe346c76a [build] Fix cmake java resources (#4898) Change-Id: Ia1dd90fe42c6cd5df281b8a5b710e136f54355f4 git-subtree-dir: third_party/allwpilib git-subtree-split: f1a82828fed8950f9a3f1586c44327027627a0c8 Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

commit: b13e13f43101a4dd3b9665015eca2d3770eb734a [log] [tgz]
author: James Kuszmaul <jabukuszmaul+collab@gmail.com> Wed Nov 22 20:44:04 2023 -0800
committer: James Kuszmaul <jabukuszmaul+collab@gmail.com> Wed Nov 22 20:44:04 2023 -0800
tree: a8824b99468c742c110c5e1371cbaf04d0f3eccd
parent: cf32412550e07365bb2afb6faed18e0e4c0ff7c1 [diff]
diff --git a/wpimath/.styleguide b/wpimath/.styleguide
index 6ae8bbf..257b50b 100644
--- a/wpimath/.styleguide
+++ b/wpimath/.styleguide

@@ -34,14 +34,14 @@
 }
 
 includeOtherLibs {
+  ^Eigen/
   ^fmt/
+  ^gcem/
+  ^gtest/
+  ^unsupported/
   ^wpi/
 }
 
 includeProject {
-  ^gcem/
-  ^drake/
-  ^Eigen/
   ^units/
-  ^unsupported/
 }

diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt
index e3d90cd..997ec46 100644
--- a/wpimath/CMakeLists.txt
+++ b/wpimath/CMakeLists.txt

@@ -3,8 +3,59 @@
 include(SubDirList)
 include(CompileWarnings)
 include(AddTest)
+include(DownloadAndCheck)
 
-file(GLOB wpimath_jni_src src/main/native/cpp/jni/WPIMathJNI.cpp)
+# workaround for makefiles - for some reason parent directories aren't created.
+file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/protobuf")
+file(GLOB wpimath_proto_src src/main/proto/*.proto)
+protobuf_generate_cpp(WPIMATH_PROTO_SRCS WPIMATH_PROTO_HDRS PROTOC_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/protobuf" PROTOS ${wpimath_proto_src})
+
+function(quickbuf_generate SRCS JAVA_PACKAGE)
+  if(NOT ARGN)
+    message(SEND_ERROR "Error: PROTOBUF_GENERATE_QUICKBUF() called without any proto files")
+    return()
+  endif()
+
+  set(_generated_srcs_all)
+  foreach(_proto ${ARGN})
+    get_filename_component(_abs_file ${_proto} ABSOLUTE)
+    get_filename_component(_abs_dir ${_abs_file} DIRECTORY)
+    get_filename_component(_basename ${_proto} NAME_WLE)
+    file(RELATIVE_PATH _rel_dir ${CMAKE_CURRENT_SOURCE_DIR} ${_abs_dir})
+
+    # convert to QuickBuffers Java case (geometry2d -> Geometry2D)
+    string(REGEX MATCHALL "[A-Za-z_]+|[0-9]+" _name_components ${_basename})
+    set(_name_components_out)
+    foreach(_part ${_name_components})
+      string(SUBSTRING ${_part} 0 1 _first_letter)
+      string(TOUPPER ${_first_letter} _first_letter)
+      string(REGEX REPLACE "^.(.*)" "${_first_letter}\\1" _part_out "${_part}")
+      list(APPEND _name_components_out ${_part_out})
+    endforeach()
+    list(JOIN _name_components_out "" _basename_title)
+
+    set(_generated_src "${CMAKE_CURRENT_BINARY_DIR}/quickbuf/${JAVA_PACKAGE}/${_basename_title}.java")
+
+    list(APPEND _generated_srcs_all ${_generated_src})
+
+    add_custom_command(
+      OUTPUT ${_generated_src}
+      COMMAND protobuf::protoc
+      ARGS --plugin=protoc-gen-quickbuf=${Quickbuf_EXECUTABLE} --quickbuf_out=gen_descriptors=true:${CMAKE_CURRENT_BINARY_DIR}/quickbuf -I${_abs_dir} ${_abs_file}
+      DEPENDS ${_abs_file} protobuf::protoc
+      COMMENT "Running quickbuf protocol buffer compiler on ${_proto}"
+      VERBATIM )
+  endforeach()
+
+  set(${SRCS} ${_generated_srcs_all} PARENT_SCOPE)
+endfunction()
+
+file(GLOB wpimath_jni_src src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
+                          src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
+                          src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
+                          src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
+                          src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
+                          src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp)
 
 # Java bindings
 if (WITH_JAVA)
@@ -13,34 +64,38 @@
   include(UseJava)
   set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.41.jar")
+  quickbuf_generate(WPIMATH_QUICKBUF_SRCS "edu/wpi/first/math/proto" ${wpimath_proto_src})
+
+  if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.43.1.jar")
       set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
       set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml")
 
       message(STATUS "Downloading EJML jarfiles...")
 
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-cdense/0.41/ejml-cdense-0.41.jar"
-          "${JAR_ROOT}/ejml-cdense-0.41.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-core/0.41/ejml-core-0.41.jar"
-          "${JAR_ROOT}/ejml-core-0.41.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-ddense/0.41/ejml-ddense-0.41.jar"
-          "${JAR_ROOT}/ejml-ddense-0.41.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-dsparse/0.41/ejml-dsparse-0.41.jar"
-          "${JAR_ROOT}/ejml-dsparse-0.41.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-fdense/0.41/ejml-fdense-0.41.jar"
-          "${JAR_ROOT}/ejml-fdense-0.41.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-simple/0.41/ejml-simple-0.41.jar"
-          "${JAR_ROOT}/ejml-simple-0.41.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-zdense/0.41/ejml-zdense-0.41.jar"
-          "${JAR_ROOT}/ejml-zdense-0.41.jar")
+      download_and_check("${BASE_URL}org/ejml/ejml-cdense/0.43.1/ejml-cdense-0.43.1.jar"
+          "${JAR_ROOT}/ejml-cdense-0.43.1.jar")
+      download_and_check("${BASE_URL}org/ejml/ejml-core/0.43.1/ejml-core-0.43.1.jar"
+          "${JAR_ROOT}/ejml-core-0.43.1.jar")
+      download_and_check("${BASE_URL}org/ejml/ejml-ddense/0.43.1/ejml-ddense-0.43.1.jar"
+          "${JAR_ROOT}/ejml-ddense-0.43.1.jar")
+      download_and_check("${BASE_URL}org/ejml/ejml-dsparse/0.43.1/ejml-dsparse-0.43.1.jar"
+          "${JAR_ROOT}/ejml-dsparse-0.43.1.jar")
+      download_and_check("${BASE_URL}org/ejml/ejml-fdense/0.43.1/ejml-fdense-0.43.1.jar"
+          "${JAR_ROOT}/ejml-fdense-0.43.1.jar")
+      download_and_check("${BASE_URL}org/ejml/ejml-simple/0.43.1/ejml-simple-0.43.1.jar"
+          "${JAR_ROOT}/ejml-simple-0.43.1.jar")
+      download_and_check("${BASE_URL}org/ejml/ejml-zdense/0.43.1/ejml-zdense-0.43.1.jar"
+          "${JAR_ROOT}/ejml-zdense-0.43.1.jar")
 
       message(STATUS "All files downloaded.")
   endif()
 
   file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
   file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+  file(GLOB QUICKBUF_JAR
+        ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar)
 
-  set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS})
+  set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS} ${QUICKBUF_JAR})
 
   execute_process(COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/generate_numbers.py ${WPILIB_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
   if(NOT (generateResult EQUAL "0"))
@@ -55,12 +110,7 @@
 
   file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${WPILIB_BINARY_DIR}/wpimath/generated/*.java)
 
-  if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
-    set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
-    add_jar(wpimath_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath)
-  else()
-    add_jar(wpimath_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers)
-  endif()
+  add_jar(wpimath_jar ${JAVA_SOURCES} ${WPIMATH_QUICKBUF_SRCS} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers)
 
   get_property(WPIMATH_JAR_FILE TARGET wpimath_jar PROPERTY JAR_FILE)
   install(FILES ${WPIMATH_JAR_FILE} DESTINATION "${java_lib_dest}")
@@ -73,28 +123,56 @@
 
   set_property(TARGET wpimathjni PROPERTY FOLDER "libraries")
 
-  if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
-    target_include_directories(wpimathjni PRIVATE ${JNI_INCLUDE_DIRS})
-    target_include_directories(wpimathjni PRIVATE "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
-  else()
-    target_link_libraries(wpimathjni PRIVATE wpimath_jni_headers)
-  endif()
+  target_link_libraries(wpimathjni PRIVATE wpimath_jni_headers)
   add_dependencies(wpimathjni wpimath_jar)
 
-  if (MSVC)
-    install(TARGETS wpimathjni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
-  endif()
-
-  install(TARGETS wpimathjni EXPORT wpimathjni DESTINATION "${main_lib_dest}")
+  install(TARGETS wpimathjni EXPORT wpimathjni)
 
 endif()
 
-file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp
-                                     src/main/native/thirdparty/drake/src/*.cpp)
+if (WITH_JAVA_SOURCE)
+  find_package(Java REQUIRED)
+  include(UseJava)
+  file(GLOB WPIMATH_SOURCES src/main/java/edu/wpi/first/math/*.java ${WPILIB_BINARY_DIR}/wpimath/generated/main/java/edu/wpi/first/math/Nat.java)
+  file(GLOB WPIMATH_CONTROLLER_SOURCES src/main/java/edu/wpi/first/math/controller/*.java)
+  file(GLOB WPIMATH_ESTIMATOR_SOURCES src/main/java/edu/wpi/first/math/estimator/*.java)
+  file(GLOB WPIMATH_FILTER_SOURCES src/main/java/edu/wpi/first/math/filter/*.java)
+  file(GLOB WPIMATH_GEOMETRY_SOURCES src/main/java/edu/wpi/first/math/geometry/*.java)
+  file(GLOB WPIMATH_INTERPOLATION_SOURCES src/main/java/edu/wpi/first/math/interpolation/*.java)
+  file(GLOB WPIMATH_KINEMATICS_SOURCES src/main/java/edu/wpi/first/math/kinematics/*.java)
+  file(GLOB WPIMATH_NUMBERS_SOURCES ${WPILIB_BINARY_DIR}/wpimath/generated/main/java/edu/wpi/first/math/numbers/*.java)
+  file(GLOB WPIMATH_SPLINE_SOURCES src/main/java/edu/wpi/first/math/spline/*.java)
+  file(GLOB WPIMATH_SYSTEM_SOURCES src/main/java/edu/wpi/first/math/system/*.java)
+  file(GLOB WPIMATH_SYSTEM_PLANT_SOURCES src/main/java/edu/wpi/first/math/system/plant/*.java)
+  file(GLOB WPIMATH_TRAJECTORY_SOURCES src/main/java/edu/wpi/first/math/trajectory/*.java)
+  file(GLOB WPIMATH_TRAJECTORY_CONSTRAINT_SOURCES src/main/java/edu/wpi/first/math/trajectory/constraint/*.java)
+  add_jar(wpimath_src_jar
+  RESOURCES NAMESPACE "edu/wpi/first/math" ${WPIMATH_SOURCES}
+  NAMESPACE "edu/wpi/first/math/controller" ${WPIMATH_CONTROLLER_SOURCES}
+  NAMESPACE "edu/wpi/first/math/estimator" ${WPIMATH_ESTIMATOR_SOURCES}
+  NAMESPACE "edu/wpi/first/math/filter" ${WPIMATH_FILTER_SOURCES}
+  NAMESPACE "edu/wpi/first/math/geometry" ${WPIMATH_GEOMETRY_SOURCES}
+  NAMESPACE "edu/wpi/first/math/interpolation" ${WPIMATH_INTERPOLATION_SOURCES}
+  NAMESPACE "edu/wpi/first/math/kinematics" ${WPIMATH_KINEMATICS_SOURCES}
+  NAMESPACE "edu/wpi/first/math/spline" ${WPIMATH_SPLINE_SOURCES}
+  NAMESPACE "edu/wpi/first/math/system" ${WPIMATH_SYSTEM_SOURCES}
+  NAMESPACE "edu/wpi/first/math/system/plant" ${WPIMATH_SYSTEM_PLANT_SOURCES}
+  NAMESPACE "edu/wpi/first/math/trajectory" ${WPIMATH_TRAJECTORY_SOURCES}
+  NAMESPACE "edu/wpi/first/math/trajectory/constraint" ${WPIMATH_TRAJECTORY_CONSTRAINT_SOURCES}
+  NAMESPACE "edu/wpi/first/math/util" src/main/java/edu/wpi/first/math/util/Units.java
+  OUTPUT_NAME wpimath-sources)
+
+  get_property(WPIMATH_SRC_JAR_FILE TARGET wpimath_src_jar PROPERTY JAR_FILE)
+  install(FILES ${WPIMATH_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+  set_property(TARGET wpimath_src_jar PROPERTY FOLDER "java")
+endif()
+
+file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp)
 list(REMOVE_ITEM wpimath_native_src ${wpimath_jni_src})
 
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS FALSE)
-add_library(wpimath ${wpimath_native_src})
+add_library(wpimath ${wpimath_native_src} ${WPIMATH_PROTO_SRCS})
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
 set_target_properties(wpimath PROPERTIES DEBUG_POSTFIX "d")
 
@@ -118,10 +196,6 @@
     target_link_libraries (wpimath Eigen3::Eigen)
 endif()
 
-install(DIRECTORY src/main/native/thirdparty/drake/include/ DESTINATION "${include_dest}/wpimath")
-target_include_directories(wpimath SYSTEM PUBLIC
-                           $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/drake/include>)
-
 install(DIRECTORY src/main/native/thirdparty/gcem/include/ DESTINATION "${include_dest}/wpimath")
 target_include_directories(wpimath SYSTEM PUBLIC
                           $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/gcem/include>)
@@ -129,13 +203,10 @@
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpimath")
 target_include_directories(wpimath PUBLIC
                             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/protobuf>
                             $<INSTALL_INTERFACE:${include_dest}/wpimath>)
 
-install(TARGETS wpimath EXPORT wpimath DESTINATION "${main_lib_dest}")
-
-if (WITH_JAVA AND MSVC)
-    install(TARGETS wpimath RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
-endif()
+install(TARGETS wpimath EXPORT wpimath)
 
 if (WITH_FLAT_INSTALL)
     set (wpimath_config_dir ${wpilib_dest})

diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md
index 04cdf94..fc2904c 100644
--- a/wpimath/algorithms.md
+++ b/wpimath/algorithms.md

@@ -39,7 +39,7 @@
   pr⁻¹p = q
   p²r⁻¹ = q
   p² = qr
-  p = sqrt(qr)
+  p = √(qr)
 ```
 
 Now solve for the Kalman gain.
@@ -48,16 +48,16 @@
   K = PCᵀ(CPCᵀ + R)⁻¹
   K = P(P + R)⁻¹
   k = p(p + r)⁻¹
-  k = sqrt(qr)(sqrt(qr) + r)⁻¹
-  k = sqrt(qr)/(sqrt(qr) + r)
+  k = √(qr)(√(qr) + r)⁻¹
+  k = √(qr)/(√(qr) + r)
 ```
 
-Multiply by sqrt(q/r)/sqrt(q/r).
+Multiply by √(q/r)/√(q/r).
 
 ```
-  k = q/(q + r sqrt(q/r))
-  k = q/(q + sqrt(qr²/r))
-  k = q/(q + sqrt(qr))
+  k = q/(q + r √(q/r))
+  k = q/(q + √(qr²/r))
+  k = q/(q + √(qr))
 ```
 
 ### Corner cases
@@ -65,16 +65,16 @@
 For q = 0 and r ≠ 0,
 
 ```
-  k = 0/(0 + sqrt(0))
+  k = 0/(0 + √0)
   k = 0/0
 ```
 
 Apply L'Hôpital's rule to k with respect to q.
 
 ```
-  k = 1/(1 + r/(2sqrt(qr)))
-  k = 2sqrt(qr)/(2sqrt(qr) + r)
-  k = 2sqrt(0)/(2sqrt(0) + r)
+  k = 1/(1 + r/(2 √(qr)))
+  k = 2 √(qr)/(2 √(qr) + r)
+  k = 2 √(0)/(2 √0 + r)
   k = 0/r
   k = 0
 ```
@@ -82,7 +82,424 @@
 For q ≠ 0 and r = 0,
 
 ```
-  k = q / (q + sqrt(0))
+  k = q / (q + √0)
   k = q / q
   k = 1
 ```
+
+## Quaternion to Euler angle conversion
+
+### Conventions
+
+We'll use the extrinsic X-Y-Z rotation order for Euler angles. The direction of rotation is CCW looking into the positive axis. If you point your right thumb along the positive axis direction, your fingers curl in the direction of rotation.
+
+The angles are a\_x around the X-axis, a\_y around the Y-axis, and a\_z around the Z-axis, with the following constraints:
+
+```
+  -π ≤ a_x ≤ π
+  -π/2 ≤ a_y ≤ π/2
+  -π ≤ a_z ≤ π
+```
+
+The coordinate system is right-handed. If you point your right thumb along the +Z axis, your fingers curl from the +X axis to the +Y axis.
+
+The quaternion imaginary numbers are defined as follows:
+
+```
+  îĵ = k̂
+  ĵk̂ = î
+  k̂î = ĵ
+  îĵ = -k̂
+  k̂ĵ = -î
+  îk̂ = -ĵ
+  î² = ĵ² = k̂² = -1
+```
+
+### Quaternion representation of axis rotations
+
+We will take it as given that a rotation by θ radians around a normalized vector v is represented with the quaternion cos(θ/2) + sin(θ/2) (v\_x î + v\_y ĵ + v\_z k̂).
+
+### Derivation
+
+For convenience, we'll define the following variables:
+
+```
+  c_x = cos(a_x/2)
+  s_x = sin(a_x/2)
+  c_y = cos(a_y/2)
+  s_y = sin(a_y/2)
+  c_z = cos(a_z/2)
+  s_z = sin(a_z/2)
+```
+
+We can calculate the quaternion corresponding to a set of Euler angles by applying each rotation in sequence. Recall that quaternions are composed with left multiplication, like matrices.
+
+```
+  q = (cos(a_z/2) + sin(a_z/2) k̂)(cos(a_y/2) + sin(a_y/2) ĵ)(cos(a_x/2) + sin(a_x/2) î)
+  q = (c_z + s_z k̂)(c_y + s_y ĵ)(c_x + s_x î)
+  q = (c_y c_z - s_y s_z î + s_y c_z ĵ + c_y s_z k̂)(c_x + s_x î)
+    = (c_x c_y c_z + s_x s_y s_z)
+      + (s_x c_y c_z - c_x s_y s_z) î
+      + (c_x s_y c_z + s_x c_y s_z) ĵ
+      + (c_x c_y s_z - s_x s_y c_z) k̂
+```
+
+Letting q = q\_w + q\_x î + q\_y ĵ + q\_z k̂, we can extract the components of the quaternion:
+
+```
+  q_w = c_x c_y c_z + s_x s_y s_z
+  q_x = s_x c_y c_z - c_x s_y s_z
+  q_y = c_x s_y c_z + s_x c_y s_z
+  q_z = c_x c_y s_z - s_x s_y c_z
+```
+
+### Solving for a\_y
+
+Solving for sin(a\_y):
+
+```
+  sin(a_y) = 2 c_y s_y
+  sin(a_y) = 2 (c_x² c_y s_y + s_x² c_y s_y)
+  sin(a_y) = 2 (c_x² c_y s_y c_z² + c_x² c_y s_y s_z²
+              + s_x² c_y s_y c_z² + s_x² c_y s_y s_z²)
+  sin(a_y) = 2 (c_x² c_y s_y c_z² + s_x² c_y s_y s_z²
+              + s_x² c_y s_y c_z² + c_x² c_y s_y s_z²)
+  sin(a_y) = 2 (c_x² c_y s_y c_z² + c_x s_x c_y² c_z s_z
+              + c_x s_x s_y² c_z s_z + s_x² c_y s_y s_z²
+              - c_x s_x c_y² c_z s_z + s_x² c_y s_y c_z²
+              + c_x² c_y s_y s_z² - c_x s_x s_y² c_z s_z)
+  sin(a_y) = 2 ((c_x c_y c_z + s_x s_y s_z)(c_x s_y c_z + s_x c_y s_z)
+              - (s_x c_y c_z - c_x s_y s_z)(c_x c_y s_z - s_x s_y c_z))
+  sin(a_y) = 2 (q_w q_y - q_x q_z)
+```
+
+Then solving for a\_y:
+
+```
+  a_y = sin⁻¹(sin(a_y))
+  a_y = sin⁻¹(2 (q_w q_y - q_x q_z))
+```
+
+### Solving for a\_x and a\_z
+
+Solving for cos(a\_x) cos(a\_y):
+
+```
+  cos(a_x) cos(a_y) = (cos²(a_x/2) - sin²(a_x/2))(cos²(a_y/2) - sin²(a_y/2))
+  cos(a_x) cos(a_y) = (c_x² - s_x²)(c_y² - s_y²)
+  cos(a_x) cos(a_y) = c_x² c_y² - c_x² s_y² - s_x² c_y² + s_x² s_y²
+  cos(a_x) cos(a_y) = c_x² (1 - s_y²) - c_x² s_y² - s_x² c_y² + s_x² (1 - c_y²)
+  cos(a_x) cos(a_y) = c_x² - c_x² s_y² - c_x² s_y² - s_x² c_y² + s_x² - s_x² c_y²
+  cos(a_x) cos(a_y) = c_x² + s_x² - 2 (c_x² s_y² + s_x² c_y²)
+  cos(a_x) cos(a_y) = 1 - 2 (c_x² s_y² + s_x² c_y²)
+  cos(a_x) cos(a_y) = 1 - 2 (c_x² s_y² c_z² + c_x² s_y² s_z²
+                           + s_x² c_y² c_z² + s_x² c_y² s_z²)
+  cos(a_x) cos(a_y) = 1 - 2 (s_x² c_y² c_z² + c_x² s_y² s_z²
+                           + c_x² s_y² c_z² + s_x² c_y² s_z²)
+  cos(a_x) cos(a_y) = 1 - 2 (s_x² c_y² c_z² - 2 c_x s_x c_y s_y c_z s_z + c_x² s_y² s_z²
+                           + c_x² s_y² c_z² + 2 c_x s_x c_y s_y c_z s_z + s_x² c_y² s_z²)
+  cos(a_x) cos(a_y) = 1 - 2 ((s_x c_y c_z - c_x s_y s_z)² + (c_x s_y c_z + s_x c_y s_z)²)
+  cos(a_x) cos(a_y) = 1 - 2 (q_x² + q_y²)
+```
+
+Solving for sin(a\_x) cos(a\_y):
+
+```
+  sin(a_x) cos(a_y) = (2 cos(a_x/2) sin(a_x/2))(cos²(a_y/2) - sin²(a_y/2))
+  sin(a_x) cos(a_y) = (2 c_x s_x)(c_y² - s_y²)
+  sin(a_x) cos(a_y) = 2 (c_x s_x c_y² - c_x s_x s_y²)
+  sin(a_x) cos(a_y) = 2 (c_x s_x c_y² c_z² + c_x s_x c_y² s_z²
+                       - c_x s_x s_y² c_z² - c_x s_x s_y² s_z²)
+  sin(a_x) cos(a_y) = 2 (c_s s_x c_y² c_z² - c_x s_x s_y² s_z²
+                       - c_x s_x s_y² c_z² + c_x s_x c_y² s_z²)
+  sin(a_x) cos(a_y) = 2 (c_x s_x c_y² c_z² - c_x² c_y s_y c_z s_z
+                       + s_x² c_y s_y c_z s_z - c_x s_x s_y² s_z²
+                       + c_x² c_y s_y c_z s_z - c_x s_x s_y² c_z²
+                       + c_x s_x c_y² s_z² - s_x² c_y s_y c_z s_z)
+  sin(a_x) cos(a_y) = 2 ((c_x c_y c_z + s_x s_y s_z)(s_x c_y c_z - c_x s_y s_z)
+                       + (c_x s_y c_z + s_x c_y s_z)(c_x c_y s_z - s_x s_y c_z))
+  sin(a_x) cos(a_y) = 2 (q_w q_x + q_y q_z)
+```
+
+Similarly, solving for cos(a\_z) cos(a\_y):
+
+```
+  cos(a_z) cos(a_y) = (cos²(a_z/2) - sin²(a_z/2))(cos²(a_y/2) - sin²(a_y/2))
+  cos(a_z) cos(a_y) = (c_z² - s_z²)(c_y² - s_y²)
+  cos(a_z) cos(a_y) = c_y² c_z² - s_y² c_z² - c_y² s_z² + s_y² s_z²
+  cos(a_z) cos(a_y) = c_y² (1 - s_z²) - s_y² c_z² - c_y² s_z² + s_y² (1 - c_z²)
+  cos(a_z) cos(a_y) = c_y² - c_y² s_z² - s_y² c_z² - c_y² s_z² + s_y² - s_y² c_z²
+  cos(a_z) cos(a_y) = c_y² + s_y² - 2 (c_y² s_z² + s_y² c_z²)
+  cos(a_z) cos(a_y) = 1 - 2 (c_y² s_z² + s_y² c_z²)
+  cos(a_z) cos(a_y) = 1 - 2 (c_x² c_y² s_z² + s_x² c_y² s_z²
+                           + c_x² s_y² c_z² + s_x² s_y² c_z²)
+  cos(a_z) cos(a_y) = 1 - 2 (c_x² s_y² c_z² + s_x² c_y² s_z²
+                           + c_x² c_y² s_z² + s_x² s_y² c_z²)
+  cos(a_z) cos(a_y) = 1 - 2 (c_x² s_y² c_z² + 2 c_x s_x c_y s_y c_z s_z + s_x² c_y² s_z²
+                           + c_x² c_y² s_z² - 2 c_x s_x c_y s_y c_z s_z + s_x² s_y² c_z²)
+  cos(a_z) cos(a_y) = 1 - 2 ((c_x s_y c_z + s_x c_y s_z)² + (c_x c_y s_z - s_x s_y c_z)²)
+  cos(a_z) cos(a_y) = 1 - 2 (q_y² + q_z²)
+```
+
+Similarly, solving for sin(a\_z) cos(a\_y):
+
+```
+  sin(a_z) cos(a_y) = (2 cos(a_z/2) sin(a_z/2))(cos²(a_y/2) - sin²(a_y/2))
+  sin(a_z) cos(a_y) = (2 c_z s_z)(c_y² - s_y²)
+  sin(a_z) cos(a_y) = 2 (c_y² c_z s_z - s_y² c_z s_z)
+  sin(a_z) cos(a_y) = 2 (c_x² c_y² c_z s_z + s_x² c_y² c_z s_z
+                       - c_x² s_y² c_z s_z - s_x² s_y² c_z s_z)
+  sin(a_z) cos(a_y) = 2 (c_x² c_y² c_z s_z - s_x² s_y² c_z s_z
+                       + s_x² c_y² c_z s_z - c_x² s_y² c_z s_z)
+  sin(a_z) cos(a_y) = 2 (c_x² c_y² c_z s_z - c_x s_x c_y s_y c_z²
+                       + c_x s_x c_y s_y s_z² - s_x² s_y² c_z s_z
+                       + c_x s_x c_y s_y c_z² + s_x² c_y² c_z s_z
+                       - c_x² s_y² c_z s_z - c_x s_x c_y s_y s_z²)
+  sin(a_z) cos(a_y) = 2 ((c_x c_y c_z + s_x s_y s_z)(c_x c_y s_z - s_x s_y c_z)
+                       + (s_x c_y c_z - c_x s_y s_z)(c_x s_y c_z + s_x c_y s_z))
+  sin(a_z) cos(a_y) = 2 (q_w q_z + q_x q_y)
+```
+
+Solving for a\_x and a\_z:
+
+```
+  a_x = atan2(sin(a_x), cos(a_x))
+  a_z = atan2(sin(a_z), cos(a_z))
+```
+
+If cos(a\_y) > 0:
+
+```
+  a_x = atan2(sin(a_x) cos(a_y), cos(a_x) cos(a_y))
+  a_z = atan2(sin(a_z) cos(a_y), cos(a_z) cos(a_y))
+  a_x = atan2(2 (q_w q_x + q_y q_z), 1 - 2 (q_x² + q_y²))
+  a_z = atan2(2 (q_w q_z + q_x q_y), 1 - 2 (q_y² + q_z²))
+```
+
+Because -π/2 ≤ a\_y ≤ π/2, cos(a\_y) ≥ 0. Therefore, the only remaining case is cos(a\_y) = 0, whose only solutions in that range are a\_y = ±π/2.
+
+```
+  a_y = ±π/2
+  a_y/2 = ±π/4
+  cos(a_y/2) = √2/2
+  c_y = √2/2
+  sin(a_y/2) = ±√2/2
+  s_y = ±√2/2
+```
+
+Plugging into the quaternion components:
+
+```
+  q_w = c_x c_y c_z + s_x s_y s_z
+  q_x = s_x c_y c_z - c_x s_y s_z
+  q_y = c_x s_y c_z + s_x c_y s_z
+  q_z = c_x c_y s_z - s_x s_y c_z
+  q_w = √2/2 c_x c_z ± √2/2 s_x s_z
+  q_x = √2/2 s_x c_z ∓ √2/2 c_x s_z
+  q_y = ±√2/2 c_x c_z + √2/2 s_x s_z
+  q_z = √2/2 c_x s_z ∓ √2/2 s_x c_z
+  q_w = √2/2 (c_x c_z ± s_x s_z)
+  q_x = √2/2 (s_x c_z ∓ c_x s_z)
+  q_y = √2/2 (± c_x c_z + s_x s_z)
+  q_z = √2/2 (c_x s_z ∓ s_x c_z)
+  q_w = √2/2 cos(a_z/2 ∓ a_x/2)
+  q_x = √2/2 sin(a_x/2 ∓ a_z/2)
+  q_y = √2/2 -cos(a_x/2 ∓ a_z/2)
+  q_z = √2/2 sin(a_z/2 ∓ a_x/2)
+```
+
+In either case only the sum or the difference between a\_x and a\_z can be determined. We'll pick the solution where a\_x = 0.
+
+```
+  q_w = √2/2 cos(a_z/2 ∓ 0)
+  q_w = √2/2 cos(a_z/2)
+  cos(a_z/2) = √2 q_w
+  q_z = √2/2 sin(a_z/2 ∓ 0)
+  q_z = √2/2 sin(a_z/2)
+  sin(a_z/2) = √2 q_z
+  cos(a_z) = cos²(a_z/2) - sin²(a_z/2)
+  cos(a_z) = (√2 q_w)² - (√2 q_z)²
+  cos(a_z) = 2 q_w² - 2 q_z²
+  cos(a_z) = 2 (q_w² - q_z²)
+  sin(a_z) = 2 cos(a_z/2) sin(a_z/2)
+  sin(a_z) = 2 (√2 q_w) (√2 q_z)
+  sin(a_z) = 4 q_w q_z
+  a_z = atan2(4 q_w q_z, 2 (q_w² - q_z²))
+  a_z = atan2(2 q_w q_z, q_w² - q_z²)
+```
+
+### Determining if cos(a\_y) ≈ 0
+
+When calculating a\_x:
+
+```
+  cos(a_y) ≈ 0
+  cos²(a_y) ≈ 0
+  cos²(a_x) cos²(a_y) + sin²(a_x) cos²(a_y) ≈ 0
+  (cos(a_x) cos(a_y))² + (sin(a_x) cos(a_y))² ≈ 0
+```
+
+Note that this reuses the cos(a\_x) cos(a\_y) and sin(a\_x) cos(a\_y) terms needed to calculate a\_x.
+
+When calculating a\_z:
+
+```
+  cos(a_y) ≈ 0
+  cos²(a_y) ≈ 0
+  cos²(a_y) cos²(a_z) + cos²(a_y) sin²(a_z) ≈ 0
+  (cos(a_y) cos(a_z))² + (cos(a_y) sin(a_z))² ≈ 0
+```
+
+Note that this reuses the cos(a\_y) cos(a\_z) and cos(a\_y) sin(a\_z) terms needed to calculate a\_z.
+
+## Quaternion Exponential
+
+We will take it as given that a quaternion has scalar and vector components `𝑞 = s + 𝑣⃗`, with vector component 𝑣⃗ consisting of a unit vector and magnitude `𝑣⃗ = θ * v̂`.
+
+```
+𝑞 = s + 𝑣⃗
+
+𝑣⃗ = θ * v̂
+
+exp(𝑞) = exp(s + 𝑣⃗)
+exp(𝑞) = exp(s) * exp(𝑣⃗)
+exp(𝑞) = exp(s) * exp(θ * v̂)
+```
+
+Applying euler's identity:
+
+```
+exp(θ * v̂) = cos(θ) + sin(θ) * v̂
+```
+
+Gives us:
+```
+exp(𝑞) = exp(s) * [cos(θ) + sin(θ) * v̂]
+```
+
+Rearranging `𝑣⃗ = θ * v̂` we can solve for v̂: `v̂ = 𝑣⃗ / θ`
+
+```
+exp(𝑞) = exp(s) * [cos(θ) + sin(θ) / θ * 𝑣⃗]
+```
+
+## Quaternion Logarithm
+
+We will take it as a given that for a given quaternion of the form `𝑞 = s + 𝑣⃗`, we can calculate the exponential: `exp(𝑞) = exp(s) * [cos(θ) + sin(θ) / θ * 𝑣⃗]` where `θ = ||𝑣⃗||`.
+
+Additionally, `exp(log(𝑞)) = q` for a given value of `log(𝑞)`. There are multiple solutions to `log(𝑞)` caused by the imaginary axes in 𝑣⃗, discussed here: https://en.wikipedia.org/wiki/Complex_logarithm
+
+We will demonstrate the principal solution of `log(𝑞)` satisfying `exp(log(𝑞)) = q`.
+This being `log(𝑞) = log(||𝑞||) + atan2(θ, s) / θ * 𝑣⃗`, is the principal solution to `log(𝑞)` because the function `atan2(θ, s)` returns the principal value corresponding to its arguments.
+
+Proof: `log(𝑞) = log(||𝑞||) + atan2(θ, s) / θ * 𝑣⃗` satisfies `exp(log(𝑞)) = q`.
+
+```
+exp(log(𝑞)) = exp(log(||𝑞||) + atan2(θ, s) / θ * 𝑣⃗)
+
+
+exp(log(𝑞)) = exp(log(||𝑞||)) * exp(atan2(θ, s) / θ * 𝑣⃗)
+
+Substitutions:
+𝑣⃗ = θ * v̂:
+exp(log(||𝑞||)) = ||𝑞||
+exp(log(𝑞)) = ||𝑞|| * exp(atan2(θ, s) * v̂)
+
+exp(log(𝑞)) = ||𝑞|| * [cos(atan2(θ, s)) + sin(atan2(θ, s)) * v̂]
+
+Substitutions:
+cos(atan2(θ, s)) = s / √(θ² + s²)
+sin(atan2(θ, s)) = θ / √(θ² + s²)
+
+exp(log(𝑞)) = ||𝑞|| * [s / √(θ² + s²) + θ / √(θ² + s²) * v̂]
+
+√(θ² + s²) = ||𝑞||
+
+exp(log(𝑞)) = ||𝑞|| * [s / ||𝑞|| + θ / ||𝑞|| * v̂]
+exp(log(𝑞)) = s + θ * v̂
+
+exp(log(𝑞)) = s + 𝑣⃗
+
+exp(log(𝑞)) = 𝑞
+```
+
+## Unit Quaternion in SO(3) from Rotation Vector in 𝖘𝖔(3)
+
+We will take it as a given that members of 𝖘𝖔(3) take the form `𝑣⃗ = θ * v̂`, representing a rotation θ around a unit axis v̂.
+
+We additionally take it as a given that quaternions in SO(3) are of the form `𝑞 = cos(θ / 2) + sin(θ / 2) * v̂`, representing a rotation of θ around unit axis v̂.
+
+```
+θ = ||𝑣⃗||
+v̂ = 𝑣⃗ / θ
+
+𝑞 = cos(θ / 2) + sin(θ / 2) * v̂
+𝑞 = cos(||𝑣⃗|| / 2) + sin(||𝑣⃗|| / 2) / ||𝑣⃗|| * 𝑣⃗
+```
+
+## Rotation vector in 𝖘𝖔(3) from Unit Quaternion in SO(3)
+
+We will take it as a given that members of 𝖘𝖔(3) take the form  `𝑟⃗ = θ * r̂`, representing a rotation θ around a unit axis r̂.
+
+We additionally take it as a given that quaternions in SO(3) are of the form `𝑞 = s + 𝑣⃗ = cos(θ / 2) + sin(θ / 2) * v̂`, representing a rotation of θ around unit axis v̂.
+
+```
+s + 𝑣⃗ = cos(θ / 2) + sin(θ / 2) * v̂
+s = cos(θ / 2)
+𝑣⃗ = sin(θ / 2) * v̂
+||𝑣⃗|| = sin(θ / 2)
+
+θ / 2 = atan2(||𝑣⃗||, s)
+θ = 2 * atan2(||𝑣⃗||, s)
+
+r̂ = 𝑣⃗ / ||𝑣⃗||
+
+𝑟⃗ = θ * r̂
+𝑟⃗ = 2 * atan2(||𝑣⃗||, s) / ||𝑣⃗|| * 𝑣⃗
+```
+
+## Closed form solution for an Exponential Motion Profile
+
+### [Derivation of continuous-time model](wpimath/algorithms/docs/ExponentialProfileModel.py)
+
+
+### Heuristic for input direction in Exponential Profile
+
+Demonstration: https://www.desmos.com/calculator/3jamollwrk
+
+The fastest path possible for an exponential profile (and the placement of the inflection point) depend on boundary conditions.
+
+Specifically, the placement (xf, vf) relative to the possible trajectories that cross through (x0, v0) decides this. There are two possible trajectories to take from the initial state. In the desmos demo these are colored Green and Purple, which arise from applying +input and -input from the initial state respectively. Red and Yellow trajectories arise from applying -input and +input respectively from terminal conditions.
+
+In order to reach the terminal state from the initial state by following Green in the +v direction, the second step is following Red in the -v direction.
+Likewise, Purple must be followed in the -v direction, and then Yellow must be followed in the +v direction.
+
+The specific conditions surrounding this decision are fourfold:
+- A: v0 >= 0
+- B: vf >= 0
+- C: vf >= x1_ps(vf, U)
+- D: vf >= x1_ps(vf, -U)
+
+Where x1_ps(v, U) follows the Green line, and x1_ps(v, -U) follows the Purple line.
+
+This creates a decision table:
+| v0>=0 | vf>=0 | vf>=x1_ps(vf,U) | vf>=x1_ps(vf,-U) | Output Sign |
+|-------|-------|-----------------|------------------|------------:|
+| False | False | False           | False            |          -1 |
+| False | False | False           | True             |           1 |
+| False | False | True            | False            |           1 |
+| False | False | True            | True             |           1 |
+| False | True  | False           | False            |          -1 |
+| False | True  | False           | True             |          -1 |
+| False | True  | True            | False            |           1 |
+| False | True  | True            | True             |           1 |
+| True  | False | False           | False            |          -1 |
+| True  | False | False           | True             |           1 |
+| True  | False | True            | False            |          -1 |
+| True  | False | True            | True             |           1 |
+| True  | True  | False           | False            |          -1 |
+| True  | True  | False           | True             |          -1 |
+| True  | True  | True            | False            |          -1 |
+| True  | True  | True            | True             |           1 |
+
+Which is equivalent to `-1 if (A & ~D) | (B & ~C) | (~C & ~D) else 1`.

diff --git a/wpimath/algorithms/ExponentialProfileModel.py b/wpimath/algorithms/ExponentialProfileModel.py
new file mode 100644
index 0000000..cc8fbc0
--- /dev/null
+++ b/wpimath/algorithms/ExponentialProfileModel.py

@@ -0,0 +1,98 @@
+from sympy import *
+from sympy.logic.boolalg import *
+
+init_printing()
+
+U, A, B, t, x0, xf, v0, vf, c1, c2, v, V, kV, kA = symbols(
+    "U, A, B t, x0, xf, v0, vf, C1, C2, v, V, kV, kA"
+)
+
+x = symbols("x", cls=Function)
+
+# Exponential profiles are derived from a differential equation: ẍ - A * ẋ = B * U
+diffeq = Eq(x(t).diff(t, t) - A * x(t).diff(t), B * U)
+
+x = dsolve(diffeq).rhs
+dx = x.diff(t)
+
+x = x.subs(
+    [
+        (c1, solve(Eq(x.subs(t, 0), x0), c1)[0]),
+        (c2, solve(Eq(dx.subs(t, 0), v0), c2)[0]),
+    ]
+)
+
+print(f"General Solution: {x}")
+
+# We need two specific solutions to this equation for an Exponential Profile:
+# One that passes through (x0, v0) and has input U
+# Another that passes through (xf, vf) and has input -U
+
+# x1 is for the accelerate step
+x1 = x.subs({x0: x0, v0: v0, U: U})
+
+dx1 = x1.diff(t)
+t1_eqn = solve(Eq(dx1, v), t)[0]
+# x1 in phase space (input v, output x)
+x1_ps = x1.subs(t, t1_eqn)
+
+
+# x2 is for the decelerate step
+x2 = x.subs({x0: xf, v0: vf, U: -U})
+
+dx2 = x2.diff(t)
+t2_eqn = solve(Eq(dx2, v), t)[0]
+# x2 in phase space (input v, output x)
+x2_ps = x2.subs(t, t2_eqn)
+
+# The point at which we switch from input U to -U is the inflection point.
+# In phase space, this is a point (x, v) where x1(v) = x2(v)
+# For now, we will just solve for +U and assume inflection velocity is positive.
+# The other possible solutions are -v_soln, and the solutions to v_equality.subs(U, -U)
+equality = simplify(Eq(x1_ps, x2_ps).expand()).expand()
+equality = Eq(equality.lhs - x0 + v0 / A - v / A, equality.rhs - x0 + v0 / A - v / A)
+equality = Eq(
+    equality.lhs
+    - B * U * log(A * v / (A * vf - B * U) - B * U / (A * vf - B * U)) / A**2,
+    equality.rhs
+    - B * U * log(A * v / (A * vf - B * U) - B * U / (A * vf - B * U)) / A**2,
+)
+equality = Eq(equality.lhs / (-B * U / A / A), equality.rhs / (-B * U / A / A))
+equality = Eq(exp(equality.lhs.simplify()), exp(equality.rhs.simplify()))
+equality = Eq(
+    equality.lhs * (A * v0 + B * U) * (A * vf - B * U),
+    equality.rhs * (A * v0 + B * U) * (A * vf - B * U),
+)
+equality = Eq(-equality.lhs.expand() + equality.rhs, 0)
+
+# This is a quadratic equation of the form ax^2 + c = 0
+v_equality = equality
+
+# solve, take positive result
+v_soln = solve(v_equality, v)[0]
+
+# With this information, we can calculate the inflection point (x, v)
+# and calculate the times that x1 and x2 reach the inflection point
+inflection_x = x1_ps.subs(v, v_soln)
+inflection_t1 = t1_eqn.subs(v, v_soln)
+inflection_t2 = t2_eqn.subs(v, v_soln)
+
+# inflection_t2 < 0 because in order for the profile to get to
+# the inflection point from the terminal state, it must go back in time.
+totalTime = inflection_t1 - inflection_t2
+
+print(f"x1: {expand(simplify(x1))}")
+print(f"x2: {expand(simplify(x2))}")
+print(f"dx1: {expand(simplify(dx1))}")
+print(f"dx2: {expand(simplify(dx2))}")
+print(f"t1: {expand(simplify(t1_eqn))}")
+print(f"t2: {expand(simplify(t2_eqn))}")
+print(f"x1 phase space: {expand(simplify(x1.subs(t, t1_eqn)))}")
+print(f"x2 phase space: {expand(simplify(x2.subs(t, t2_eqn)))}")
+print(f"vi equality: {v_equality}")
+
+
+a, b, c, d = symbols("a, b, c, d")
+
+expression = SOPform([a, b, c, d], minterms=[0, 4, 5, 8, 10, 12, 13, 14])
+print(f"Truth Table Expression: {expression}")

diff --git a/wpimath/build.gradle b/wpimath/build.gradle
index 9e7c3a8..09df80c 100644
--- a/wpimath/build.gradle
+++ b/wpimath/build.gradle

@@ -9,30 +9,11 @@
 
     nativeName = 'wpimath'
     devMain = 'edu.wpi.first.math.DevMain'
-
-    splitSetup = {
-        it.sources {
-            drakeCpp(CppSourceSet) {
-                source {
-                    srcDirs 'src/main/native/thirdparty/drake/src'
-                    include '**/*.cpp'
-                }
-                exportedHeaders {
-                    srcDirs 'src/main/native/thirdparty/drake/include',
-                            'src/main/native/thirdparty/eigen/include',
-                            'src/main/native/thirdparty/gcem/include'
-                }
-            }
-        }
-    }
 }
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
 
 cppHeadersZip {
-    from('src/main/native/thirdparty/drake/include') {
-        into '/'
-    }
     from('src/main/native/thirdparty/eigen/include') {
         into '/'
     }
@@ -47,7 +28,6 @@
             it.sources.each {
                 it.exportedHeaders {
                     srcDirs 'src/main/native/include',
-                            'src/main/native/thirdparty/drake/include',
                             'src/main/native/thirdparty/eigen/include',
                             'src/main/native/thirdparty/gcem/include'
                 }
@@ -57,10 +37,11 @@
 }
 
 dependencies {
-    api "org.ejml:ejml-simple:0.41"
-    api "com.fasterxml.jackson.core:jackson-annotations:2.12.4"
-    api "com.fasterxml.jackson.core:jackson-core:2.12.4"
-    api "com.fasterxml.jackson.core:jackson-databind:2.12.4"
+    api "org.ejml:ejml-simple:0.43.1"
+    api "com.fasterxml.jackson.core:jackson-annotations:2.15.2"
+    api "com.fasterxml.jackson.core:jackson-core:2.15.2"
+    api "com.fasterxml.jackson.core:jackson-databind:2.15.2"
+    api "us.hebi.quickbuf:quickbuf-runtime:1.3.2"
 }
 
 def wpilibNumberFileInput = file("src/generate/GenericNumber.java.jinja")
@@ -125,3 +106,25 @@
 sourceSets.main.java.srcDir "${buildDir}/generated/java"
 compileJava.dependsOn generateNumbers
 compileJava.dependsOn generateNat
+
+task unitsHeaders(type: Zip) {
+    destinationDirectory = file("$buildDir/outputs")
+    archiveBaseName = zipBaseName
+    archiveClassifier = "units"
+
+    from(licenseFile) {
+        into '/'
+    }
+
+    ext.includeDirs = [
+        project.file('src/main/native/include/units')
+    ]
+
+    ext.includeDirs.each {
+        from(it) {
+            into '/units'
+        }
+    }
+}
+
+addTaskToCopyAllOutputs(unitsHeaders)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/DARE.java b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
new file mode 100644
index 0000000..ad07d05
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/DARE.java

@@ -0,0 +1,239 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import org.ejml.simple.SimpleMatrix;
+
+public final class DARE {
+  private DARE() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met.
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R) {
+    var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+    WPIMathJNI.dareDetailABQR(
+        A.getStorage().getDDRM().getData(),
+        B.getStorage().getDDRM().getData(),
+        Q.getStorage().getDDRM().getData(),
+        R.getStorage().getDDRM().getData(),
+        A.getNumCols(),
+        B.getNumCols(),
+        S.getStorage().getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This is equivalent to solving the original DARE:
+   *
+   * <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+   *
+   * <p>where A₂ and Q₂ are a change of variables:
+   *
+   * <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q₂ isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A₂, B) pair isn't stabilizable
+   *   <li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met.
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @param N State-input cross-term cost matrix.
+   * @return Solution of DARE.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, Inputs> N) {
+    var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+    WPIMathJNI.dareDetailABQRN(
+        A.getStorage().getDDRM().getData(),
+        B.getStorage().getDDRM().getData(),
+        Q.getStorage().getDDRM().getData(),
+        R.getStorage().getDDRM().getData(),
+        N.getStorage().getDDRM().getData(),
+        A.getNumCols(),
+        B.getNumCols(),
+        S.getStorage().getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R) {
+    var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+    WPIMathJNI.dareABQR(
+        A.getStorage().getDDRM().getData(),
+        B.getStorage().getDDRM().getData(),
+        Q.getStorage().getDDRM().getData(),
+        R.getStorage().getDDRM().getData(),
+        A.getNumCols(),
+        B.getNumCols(),
+        S.getStorage().getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This is equivalent to solving the original DARE:
+   *
+   * <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+   *
+   * <p>where A₂ and Q₂ are a change of variables:
+   *
+   * <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @param N State-input cross-term cost matrix.
+   * @return Solution of DARE.
+   * @throws IllegalArgumentException if Q₂ isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A₂, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A₂, C) pair where Q₂ = CᵀC isn't detectable.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, Inputs> N) {
+    var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+    WPIMathJNI.dareABQRN(
+        A.getStorage().getDDRM().getData(),
+        B.getStorage().getDDRM().getData(),
+        Q.getStorage().getDDRM().getData(),
+        R.getStorage().getDDRM().getData(),
+        N.getStorage().getDDRM().getData(),
+        A.getNumCols(),
+        B.getNumCols(),
+        S.getStorage().getDDRM().getData());
+    return S;
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
deleted file mode 100644
index 55bc5b2..0000000
--- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java
+++ /dev/null

@@ -1,116 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.math;
-
-import org.ejml.simple.SimpleMatrix;
-
-public final class Drake {
-  private Drake() {}
-
-  /**
-   * Solves the discrete algebraic Riccati equation.
-   *
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @param Q State cost matrix.
-   * @param R Input cost matrix.
-   * @return Solution of DARE.
-   */
-  public static SimpleMatrix discreteAlgebraicRiccatiEquation(
-      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
-    var S = new SimpleMatrix(A.numRows(), A.numCols());
-    WPIMathJNI.discreteAlgebraicRiccatiEquation(
-        A.getDDRM().getData(),
-        B.getDDRM().getData(),
-        Q.getDDRM().getData(),
-        R.getDDRM().getData(),
-        A.numCols(),
-        B.numCols(),
-        S.getDDRM().getData());
-    return S;
-  }
-
-  /**
-   * Solves the discrete algebraic Riccati equation.
-   *
-   * @param <States> Number of states.
-   * @param <Inputs> Number of inputs.
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @param Q State cost matrix.
-   * @param R Input cost matrix.
-   * @return Solution of DARE.
-   */
-  public static <States extends Num, Inputs extends Num>
-      Matrix<States, States> discreteAlgebraicRiccatiEquation(
-          Matrix<States, States> A,
-          Matrix<States, Inputs> B,
-          Matrix<States, States> Q,
-          Matrix<Inputs, Inputs> R) {
-    return new Matrix<>(
-        discreteAlgebraicRiccatiEquation(
-            A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
-  }
-
-  /**
-   * Solves the discrete algebraic Riccati equation.
-   *
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @param Q State cost matrix.
-   * @param R Input cost matrix.
-   * @param N State-input cross-term cost matrix.
-   * @return Solution of DARE.
-   */
-  public static SimpleMatrix discreteAlgebraicRiccatiEquation(
-      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
-    // See
-    // https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
-    // for the change of variables used here.
-    var scrA = A.minus(B.mult(R.solve(N.transpose())));
-    var scrQ = Q.minus(N.mult(R.solve(N.transpose())));
-
-    var S = new SimpleMatrix(A.numRows(), A.numCols());
-    WPIMathJNI.discreteAlgebraicRiccatiEquation(
-        scrA.getDDRM().getData(),
-        B.getDDRM().getData(),
-        scrQ.getDDRM().getData(),
-        R.getDDRM().getData(),
-        A.numCols(),
-        B.numCols(),
-        S.getDDRM().getData());
-    return S;
-  }
-
-  /**
-   * Solves the discrete algebraic Riccati equation.
-   *
-   * @param <States> Number of states.
-   * @param <Inputs> Number of inputs.
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @param Q State cost matrix.
-   * @param R Input cost matrix.
-   * @param N State-input cross-term cost matrix.
-   * @return Solution of DARE.
-   */
-  public static <States extends Num, Inputs extends Num>
-      Matrix<States, States> discreteAlgebraicRiccatiEquation(
-          Matrix<States, States> A,
-          Matrix<States, Inputs> B,
-          Matrix<States, States> Q,
-          Matrix<Inputs, Inputs> R,
-          Matrix<States, Inputs> N) {
-    // See
-    // https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
-    // for the change of variables used here.
-    var scrA = A.minus(B.times(R.solve(N.transpose())));
-    var scrQ = Q.minus(N.times(R.solve(N.transpose())));
-
-    return new Matrix<>(
-        discreteAlgebraicRiccatiEquation(
-            scrA.getStorage(), B.getStorage(), scrQ.getStorage(), R.getStorage()));
-  }
-}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
index 483dad3..3c2b843 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java

@@ -20,4 +20,11 @@
    * @param count the usage count
    */
   void reportUsage(MathUsageId id, int count);
+
+  /**
+   * Get the current time.
+   *
+   * @return Time in seconds
+   */
+  double getTimestamp();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
index 0dbc03d..d6e3e96 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math;
 
+import edu.wpi.first.util.WPIUtilJNI;
+
 public final class MathSharedStore {
   private static MathShared mathShared;
 
@@ -23,6 +25,11 @@
 
             @Override
             public void reportUsage(MathUsageId id, int count) {}
+
+            @Override
+            public double getTimestamp() {
+              return WPIUtilJNI.now() * 1.0e-6;
+            }
           };
     }
     return mathShared;
@@ -56,4 +63,13 @@
   public static void reportUsage(MathUsageId id, int count) {
     getMathShared().reportUsage(id, count);
   }
+
+  /**
+   * Get the time.
+   *
+   * @return The time in seconds.
+   */
+  public static double getTimestamp() {
+    return getMathShared().getTimestamp();
+  }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
index 95ed5bf..3785780 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java

@@ -145,4 +145,67 @@
   public static double interpolate(double startValue, double endValue, double t) {
     return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
   }
+
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  public static double inverseInterpolate(double startValue, double endValue, double q) {
+    double totalRange = endValue - startValue;
+    if (totalRange <= 0) {
+      return 0.0;
+    }
+    double queryToStart = q - startValue;
+    if (queryToStart <= 0) {
+      return 0.0;
+    }
+    return queryToStart / totalRange;
+  }
+
+  /**
+   * Checks if the given value matches an expected value within a certain tolerance.
+   *
+   * @param expected The expected value
+   * @param actual The actual value
+   * @param tolerance The allowed difference between the actual and the expected value
+   * @return Whether or not the actual value is within the allowed tolerance
+   */
+  public static boolean isNear(double expected, double actual, double tolerance) {
+    if (tolerance < 0) {
+      throw new IllegalArgumentException("Tolerance must be a non-negative number!");
+    }
+    return Math.abs(expected - actual) < tolerance;
+  }
+
+  /**
+   * Checks if the given value matches an expected value within a certain tolerance. Supports
+   * continuous input for cases like absolute encoders.
+   *
+   * <p>Continuous input means that the min and max value are considered to be the same point, and
+   * tolerances can be checked across them. A common example would be for absolute encoders: calling
+   * isNear(2, 359, 5, 0, 360) returns true because 359 is 1 away from 360 (which is treated as the
+   * same as 0) and 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
+   * given tolerance of 5.
+   *
+   * @param expected The expected value
+   * @param actual The actual value
+   * @param tolerance The allowed difference between the actual and the expected value
+   * @param min Smallest value before wrapping around to the largest value
+   * @param max Largest value before wrapping around to the smallest value
+   * @return Whether or not the actual value is within the allowed tolerance
+   */
+  public static boolean isNear(
+      double expected, double actual, double tolerance, double min, double max) {
+    if (tolerance < 0) {
+      throw new IllegalArgumentException("Tolerance must be a non-negative number!");
+    }
+    // Max error is exactly halfway between the min and max
+    double errorBound = (max - min) / 2.0;
+    double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
+    return Math.abs(error) < tolerance;
+  }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
index b8e7c28..b7a86fa 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java

@@ -42,6 +42,18 @@
    * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
    * provided generic bounds match the shape of the provided {@link Matrix}.
    *
+   * @param rows The number of rows of the matrix.
+   * @param columns The number of columns of the matrix.
+   * @param storage The double array to back this value.
+   */
+  public Matrix(Nat<R> rows, Nat<C> columns, double[] storage) {
+    this.m_storage = new SimpleMatrix(rows.getNum(), columns.getNum(), true, storage);
+  }
+
+  /**
+   * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
+   * provided generic bounds match the shape of the provided {@link Matrix}.
+   *
    * <p>NOTE:It is not recommend to use this constructor unless the {@link SimpleMatrix} API is
    * absolutely necessary due to the desired function not being accessible through the {@link
    * Matrix} wrapper.
@@ -80,7 +92,7 @@
    * @return The number of columns, according to the internal storage.
    */
   public final int getNumCols() {
-    return this.m_storage.numCols();
+    return this.m_storage.getNumCols();
   }
 
   /**
@@ -89,7 +101,7 @@
    * @return The number of rows, according to the internal storage.
    */
   public final int getNumRows() {
-    return this.m_storage.numRows();
+    return this.m_storage.getNumRows();
   }
 
   /**
@@ -222,7 +234,7 @@
    *
    * <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
    *
-   * @param other The other {@link Matrix} to preform element multiplication on.
+   * @param other The other {@link Matrix} to perform element multiplication on.
    * @return The element by element multiplication of "this" and other.
    */
   public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
@@ -582,7 +594,7 @@
     SimpleMatrix temp = m_storage.copy();
 
     CholeskyDecomposition_F64<DMatrixRMaj> chol =
-        DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+        DecompositionFactory_DDRM.chol(temp.getNumRows(), lowerTriangular);
     if (!chol.decompose(temp.getMatrix())) {
       // check that the input is not all zeros -- if they are, we special case and return all
       // zeros.
@@ -592,7 +604,7 @@
         isZeros &= Math.abs(matDatum) < 1e-6;
       }
       if (isZeros) {
-        return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
+        return new Matrix<>(new SimpleMatrix(temp.getNumRows(), temp.getNumCols()));
       }
 
       throw new RuntimeException(

diff --git a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
index e1680eb..487faf4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java

@@ -68,7 +68,7 @@
 
   private static Pair<SimpleMatrix, SimpleMatrix> pade3(SimpleMatrix A) {
     double[] b = new double[] {120, 60, 12, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
 
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
@@ -78,7 +78,7 @@
 
   private static Pair<SimpleMatrix, SimpleMatrix> pade5(SimpleMatrix A) {
     double[] b = new double[] {30240, 15120, 3360, 420, 30, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix A4 = A2.mult(A2);
 
@@ -90,7 +90,7 @@
 
   private static Pair<SimpleMatrix, SimpleMatrix> pade7(SimpleMatrix A) {
     double[] b = new double[] {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix A4 = A2.mult(A2);
     SimpleMatrix A6 = A4.mult(A2);
@@ -108,7 +108,7 @@
         new double[] {
           17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
         };
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix A4 = A2.mult(A2);
     SimpleMatrix A6 = A4.mult(A2);
@@ -149,7 +149,7 @@
           182,
           1
         };
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
 
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix A4 = A2.mult(A2);
@@ -213,7 +213,7 @@
     SimpleMatrix temp = src.copy();
 
     CholeskyDecomposition_F64<DMatrixRMaj> chol =
-        DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+        DecompositionFactory_DDRM.chol(temp.getNumRows(), lowerTriangular);
     if (!chol.decompose(temp.getMatrix())) {
       // check that the input is not all zeros -- if they are, we special case and return all
       // zeros.
@@ -223,7 +223,7 @@
         isZeros &= Math.abs(matDatum) < 1e-6;
       }
       if (isZeros) {
-        return new SimpleMatrix(temp.numRows(), temp.numCols());
+        return new SimpleMatrix(temp.getNumRows(), temp.getNumCols());
       }
 
       throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
@@ -239,8 +239,8 @@
    * @return the exponential of A.
    */
   public static SimpleMatrix exp(SimpleMatrix A) {
-    SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
-    WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
+    SimpleMatrix toReturn = new SimpleMatrix(A.getNumRows(), A.getNumRows());
+    WPIMathJNI.exp(A.getDDRM().getData(), A.getNumRows(), toReturn.getDDRM().getData());
     return toReturn;
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
index a041845..27ed9e0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java

@@ -15,7 +15,7 @@
   private static Random rand = new Random();
 
   private StateSpaceUtil() {
-    // Utility class
+    throw new UnsupportedOperationException("This is a utility class!");
   }
 
   /**
@@ -90,7 +90,7 @@
    * Returns true if (A, B) is a stabilizable pair.
    *
    * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
-   * absolute values less than one, where an eigenvalue is uncontrollable if rank(λI - A, B) %3C n
+   * absolute values less than one, where an eigenvalue is uncontrollable if rank([λI - A, B]) %3C n
    * where n is the number of states.
    *
    * @param <States> Num representing the size of A.
@@ -108,7 +108,7 @@
    * Returns true if (A, C) is a detectable pair.
    *
    * <p>(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute
-   * values less than one, where an eigenvalue is unobservable if rank(λI - A; C) %3C n where n is
+   * values less than one, where an eigenvalue is unobservable if rank([λI - A; C]) %3C n where n is
    * the number of states.
    *
    * @param <States> Num representing the size of A.

diff --git a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
index 670611a..9ba91b0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java

@@ -14,6 +14,8 @@
 import edu.wpi.first.math.numbers.N7;
 import edu.wpi.first.math.numbers.N8;
 import edu.wpi.first.math.numbers.N9;
+import java.util.Objects;
+import org.ejml.simple.SimpleMatrix;
 
 /**
  * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
@@ -26,7 +28,15 @@
   }
 
   private Vector<N> fillVec(double... data) {
-    return new Vector<>(fill(data));
+    if (Objects.requireNonNull(data).length != this.m_rows.getNum()) {
+      throw new IllegalArgumentException(
+          "Invalid vector data provided. Wanted "
+              + this.m_rows.getNum()
+              + " vector, but got "
+              + data.length
+              + " elements");
+    }
+    return new Vector<>(new SimpleMatrix(this.m_rows.getNum(), 1, true, data));
   }
 
   /**

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index 1aebdde..bc46741 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.math;
 
 import edu.wpi.first.math.numbers.N1;
+import java.util.Objects;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -63,6 +64,26 @@
   }
 
   /**
+   * Adds the given vector to this vector.
+   *
+   * @param value The vector to add.
+   * @return The resultant vector.
+   */
+  public final Vector<R> plus(Vector<R> value) {
+    return new Vector<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
+   * Subtracts the given vector to this vector.
+   *
+   * @param value The vector to add.
+   * @return The resultant vector.
+   */
+  public final Vector<R> minus(Vector<R> value) {
+    return new Vector<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
    * Returns the dot product of this vector with another.
    *
    * @param other The other vector.

diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 40a5c63..ef47a3b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

@@ -43,8 +43,25 @@
     libraryLoaded = true;
   }
 
+  // DARE wrappers
+
   /**
-   * Solves the discrete alegebraic Riccati equation.
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met. Solves the discrete
+   * alegebraic Riccati equation.
    *
    * @param A Array containing elements of A in row-major order.
    * @param B Array containing elements of B in row-major order.
@@ -54,10 +71,148 @@
    * @param inputs Number of inputs in B matrix.
    * @param S Array storage for DARE solution.
    */
-  public static native void discreteAlgebraicRiccatiEquation(
+  public static native void dareDetailABQR(
       double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
 
   /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met.
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param N Array containing elements of N in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   */
+  public static native void dareDetailABQRN(
+      double[] A,
+      double[] B,
+      double[] Q,
+      double[] R,
+      double[] N,
+      int states,
+      int inputs,
+      double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static native void dareABQR(
+      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param N Array containing elements of N in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static native void dareABQRN(
+      double[] A,
+      double[] B,
+      double[] Q,
+      double[] R,
+      double[] N,
+      int states,
+      int inputs,
+      double[] S);
+
+  // Eigen wrappers
+
+  /**
    * Computes the matrix exp.
    *
    * @param src Array of elements of the matrix to be exponentiated.
@@ -77,6 +232,109 @@
   public static native void pow(double[] src, int rows, double exponent, double[] dst);
 
   /**
+   * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
+   * matrix.
+   *
+   * @param mat Array of elements of the matrix to be updated.
+   * @param lowerTriangular Whether mat is lower triangular.
+   * @param rows How many rows there are.
+   * @param vec Vector to use for the rank update.
+   * @param sigma Sigma value to use for the rank update.
+   */
+  public static native void rankUpdate(
+      double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
+
+  /**
+   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
+   *
+   * @param A Array of elements of the A matrix.
+   * @param Arows Number of rows of the A matrix.
+   * @param Acols Number of rows of the A matrix.
+   * @param B Array of elements of the B matrix.
+   * @param Brows Number of rows of the B matrix.
+   * @param Bcols Number of rows of the B matrix.
+   * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
+   */
+  public static native void solveFullPivHouseholderQr(
+      double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
+
+  // Pose3d wrappers
+
+  /**
+   * Obtain a Pose3d from a (constant curvature) velocity.
+   *
+   * <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz].
+   *
+   * @param poseX The pose's translational X component.
+   * @param poseY The pose's translational Y component.
+   * @param poseZ The pose's translational Z component.
+   * @param poseQw The pose quaternion's W component.
+   * @param poseQx The pose quaternion's X component.
+   * @param poseQy The pose quaternion's Y component.
+   * @param poseQz The pose quaternion's Z component.
+   * @param twistDx The twist's dx value.
+   * @param twistDy The twist's dy value.
+   * @param twistDz The twist's dz value.
+   * @param twistRx The twist's rx value.
+   * @param twistRy The twist's ry value.
+   * @param twistRz The twist's rz value.
+   * @return The new pose as a double array.
+   */
+  public static native double[] expPose3d(
+      double poseX,
+      double poseY,
+      double poseZ,
+      double poseQw,
+      double poseQx,
+      double poseQy,
+      double poseQz,
+      double twistDx,
+      double twistDy,
+      double twistDz,
+      double twistRx,
+      double twistRy,
+      double twistRz);
+
+  /**
+   * Returns a Twist3d that maps the starting pose to the end pose.
+   *
+   * <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
+   *
+   * @param startX The starting pose's translational X component.
+   * @param startY The starting pose's translational Y component.
+   * @param startZ The starting pose's translational Z component.
+   * @param startQw The starting pose quaternion's W component.
+   * @param startQx The starting pose quaternion's X component.
+   * @param startQy The starting pose quaternion's Y component.
+   * @param startQz The starting pose quaternion's Z component.
+   * @param endX The ending pose's translational X component.
+   * @param endY The ending pose's translational Y component.
+   * @param endZ The ending pose's translational Z component.
+   * @param endQw The ending pose quaternion's W component.
+   * @param endQx The ending pose quaternion's X component.
+   * @param endQy The ending pose quaternion's Y component.
+   * @param endQz The ending pose quaternion's Z component.
+   * @return The twist that maps start to end as a double array.
+   */
+  public static native double[] logPose3d(
+      double startX,
+      double startY,
+      double startZ,
+      double startQw,
+      double startQx,
+      double startQy,
+      double startQz,
+      double endX,
+      double endY,
+      double endZ,
+      double endQw,
+      double endQx,
+      double endQy,
+      double endQz);
+
+  // StateSpaceUtil wrappers
+
+  /**
    * Returns true if (A, B) is a stabilizable pair.
    *
    * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
@@ -91,6 +349,8 @@
    */
   public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
 
+  // Trajectory wrappers
+
   /**
    * Loads a Pathweaver JSON.
    *
@@ -125,19 +385,6 @@
    */
   public static native String serializeTrajectory(double[] elements);
 
-  /**
-   * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
-   * matrix.
-   *
-   * @param mat Array of elements of the matrix to be updated.
-   * @param lowerTriangular Whether mat is lower triangular.
-   * @param rows How many rows there are.
-   * @param vec Vector to use for the rank update.
-   * @param sigma Sigma value to use for the rank update.
-   */
-  public static native void rankUpdate(
-      double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
-
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
@@ -149,18 +396,4 @@
       extractOnStaticLoad.set(load);
     }
   }
-
-  /**
-   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
-   *
-   * @param A Array of elements of the A matrix.
-   * @param Arows Number of rows of the A matrix.
-   * @param Acols Number of rows of the A matrix.
-   * @param B Array of elements of the B matrix.
-   * @param Brows Number of rows of the B matrix.
-   * @param Bcols Number of rows of the B matrix.
-   * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
-   */
-  public static native void solveFullPivHouseholderQr(
-      double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
index 385c5c0..a2a5944 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java

@@ -181,6 +181,9 @@
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
     var rDot = (nextR.minus(r)).div(m_dt);
 
+    // ṙ = f(r) + Bu
+    // Bu = ṙ − f(r)
+    // u = B⁺(ṙ − f(r))
     m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
 
     m_r = nextR;

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
index 9f4dd86..cfe7c2b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
 /**
  * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
  * against the force of gravity).
@@ -54,6 +58,48 @@
   }
 
   /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity The next velocity setpoint.
+   * @param dtSeconds Time between velocity setpoints in seconds.
+   * @return The computed feedforward.
+   */
+  public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
+    // Discretize the affine model.
+    //
+    //   dx/dt = Ax + Bu + c
+    //   dx/dt = Ax + B(u + B⁺c)
+    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
+    //
+    // Solve for uₖ.
+    //
+    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+    //
+    // For an elevator with the model
+    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
+    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
+    // assuming sgn(x) is a constant for the duration of the step.
+    //
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
+    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
+
+    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
+    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
+
+    return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
+  }
+
+  /**
    * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
    * zero).
    *

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
index e43c6ff..87148a4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java

@@ -139,4 +139,31 @@
   public void setEnabled(boolean enabled) {
     m_enabled = enabled;
   }
+
+  /**
+   * Returns the heading controller.
+   *
+   * @return heading ProfiledPIDController
+   */
+  public ProfiledPIDController getThetaController() {
+    return m_thetaController;
+  }
+
+  /**
+   * Returns the x controller.
+   *
+   * @return X PIDController
+   */
+  public PIDController getXController() {
+    return m_xController;
+  }
+
+  /**
+   * Returns the y controller.
+   *
+   * @return Y PIDController
+   */
+  public PIDController getYController() {
+    return m_yController;
+  }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
index 46254a3..2be2a47 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.InterpolatingMatrixTreeMap;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.MathUtil;
@@ -15,14 +16,22 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.trajectory.Trajectory;
 
 /**
  * The linear time-varying differential drive controller has a similar form to the LQR, but the
- * model used to compute the controller gain is the nonlinear model linearized around the
- * drivetrain's current state. We precomputed gains for important places in our state-space, then
- * interpolated between them with a LUT to save computational resources.
+ * model used to compute the controller gain is the nonlinear differential drive model linearized
+ * around the drivetrain's current state. We precompute gains for important places in our
+ * state-space, then interpolate between them with a lookup table to save computational resources.
+ *
+ * <p>This controller has a flat hierarchy with pose and wheel velocity references and voltage
+ * outputs. This is different from a Ramsete controller's nested hierarchy where the top-level
+ * controller has a pose reference and chassis velocity command outputs, and the low-level
+ * controller has wheel velocity references and voltage outputs. Flat hierarchies are easier to tune
+ * in one shot. Furthermore, this controller is more optimal in the "least-squares error" sense than
+ * a controller based on Ramsete.
  *
  * <p>See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used
  * shown in theorem 8.7.4.
@@ -55,12 +64,18 @@
   /**
    * Constructs a linear time-varying differential drive controller.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param plant The differential drive velocity plant.
    * @param trackwidth The distance between the differential drive's left and right wheels in
    *     meters.
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Discretization timestep in seconds.
+   * @throws IllegalArgumentException if max velocity of plant with 12 V input &lt;= 0 m/s or &gt;=
+   *     15 m/s.
    */
   public LTVDifferentialDriveController(
       LinearSystem<N2, N2, N2> plant,
@@ -127,15 +142,39 @@
             .times(-1.0)
             .get(0, 0);
 
+    if (maxV <= 0.0) {
+      throw new IllegalArgumentException(
+          "Max velocity of plant with 12 V input must be greater than 0 m/s.");
+    }
+    if (maxV >= 15.0) {
+      throw new IllegalArgumentException(
+          "Max velocity of plant with 12 V input must be less than 15 m/s.");
+    }
+
     for (double velocity = -maxV; velocity < maxV; velocity += 0.01) {
       // The DARE is ill-conditioned if the velocity is close to zero, so don't
       // let the system stop.
       if (Math.abs(velocity) < 1e-4) {
-        m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N5()));
+        A.set(State.kY.value, State.kHeading.value, 1e-4);
       } else {
         A.set(State.kY.value, State.kHeading.value, velocity);
-        m_table.put(velocity, new LinearQuadraticRegulator<N5, N2, N5>(A, B, Q, R, dt).getK());
       }
+
+      var discABPair = Discretization.discretizeAB(A, B, dt);
+      var discA = discABPair.getFirst();
+      var discB = discABPair.getSecond();
+
+      var S = DARE.dareDetail(discA, discB, Q, R);
+
+      // K = (BᵀSB + R)⁻¹BᵀSA
+      m_table.put(
+          velocity,
+          discB
+              .transpose()
+              .times(S)
+              .times(discB)
+              .plus(R)
+              .solve(discB.transpose().times(S).times(discA)));
     }
   }
 

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
index 701f21b..7e3391f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.InterpolatingMatrixTreeMap;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
@@ -15,12 +16,16 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.trajectory.Trajectory;
 
 /**
  * The linear time-varying unicycle controller has a similar form to the LQR, but the model used to
- * compute the controller gain is the nonlinear model linearized around the drivetrain's current
- * state.
+ * compute the controller gain is the nonlinear unicycle model linearized around the drivetrain's
+ * current state.
+ *
+ * <p>This controller is a roughly drop-in replacement for {@link RamseteController} with more
+ * optimal feedback gains in the "least-squares error" sense.
  *
  * <p>See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used
  * shown in theorem 8.9.1.
@@ -66,6 +71,7 @@
    * @param dt Discretization timestep in seconds.
    * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
    *     table. The default is 9 m/s.
+   * @throws IllegalArgumentException if maxVelocity &lt;= 0.
    */
   public LTVUnicycleController(double dt, double maxVelocity) {
     this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
@@ -74,6 +80,10 @@
   /**
    * Constructs a linear time-varying unicycle controller.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Discretization timestep in seconds.
@@ -85,14 +95,26 @@
   /**
    * Constructs a linear time-varying unicycle controller.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Discretization timestep in seconds.
    * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
    *     table. The default is 9 m/s.
+   * @throws IllegalArgumentException if maxVelocity &lt;= 0 m/s or &gt;= 15 m/s.
    */
   public LTVUnicycleController(
       Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
+    if (maxVelocity <= 0.0) {
+      throw new IllegalArgumentException("Max velocity must be greater than 0 m/s.");
+    }
+    if (maxVelocity >= 15.0) {
+      throw new IllegalArgumentException("Max velocity must be less than 15 m/s.");
+    }
+
     // The change in global pose for a unicycle is defined by the following
     // three equations.
     //
@@ -135,11 +157,26 @@
       // The DARE is ill-conditioned if the velocity is close to zero, so don't
       // let the system stop.
       if (Math.abs(velocity) < 1e-4) {
-        m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N3()));
+        A.set(State.kY.value, State.kHeading.value, 1e-4);
       } else {
         A.set(State.kY.value, State.kHeading.value, velocity);
-        m_table.put(velocity, new LinearQuadraticRegulator<N3, N2, N3>(A, B, Q, R, dt).getK());
       }
+
+      var discABPair = Discretization.discretizeAB(A, B, dt);
+      var discA = discABPair.getFirst();
+      var discB = discABPair.getSecond();
+
+      var S = DARE.dareDetail(discA, discB, Q, R);
+
+      // K = (BᵀSB + R)⁻¹BᵀSA
+      m_table.put(
+          velocity,
+          discB
+              .transpose()
+              .times(S)
+              .times(discB)
+              .plus(R)
+              .solve(discB.transpose().times(S).times(discA)));
     }
   }
 

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
index a762851..b0f23a8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java

@@ -139,6 +139,9 @@
    * @return The calculated feedforward.
    */
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    // rₖ₊₁ = Arₖ + Buₖ
+    // Buₖ = rₖ₊₁ − Arₖ
+    // uₖ = B⁺(rₖ₊₁ − Arₖ)
     m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
 
     m_r = nextR;

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
index dce1748..658be69 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.math.controller;
 
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Num;
@@ -35,6 +35,10 @@
   /**
    * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param plant The plant being controlled.
    * @param qelms The maximum desired error tolerance for each state.
    * @param relms The maximum desired control effort for each input.
@@ -57,6 +61,10 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param A Continuous system matrix of the plant being controlled.
    * @param B Continuous input matrix of the plant being controlled.
    * @param qelms The maximum desired error tolerance for each state.
@@ -111,7 +119,7 @@
       throw new IllegalArgumentException(msg);
     }
 
-    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+    var S = DARE.dare(discA, discB, Q, R);
 
     // K = (BᵀSB + R)⁻¹BᵀSA
     m_K =
@@ -150,7 +158,7 @@
     var discA = discABPair.getFirst();
     var discB = discABPair.getSecond();
 
-    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+    var S = DARE.dare(discA, discB, Q, R, N);
 
     // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
     m_K =

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
index 5e82909..1d98900 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java

@@ -24,6 +24,9 @@
   // Factor for "derivative" control
   private double m_kd;
 
+  // The error range where "integral" control applies
+  private double m_iZone = Double.POSITIVE_INFINITY;
+
   // The period (in seconds) of the loop that calls the controller
   private final double m_period;
 
@@ -55,6 +58,9 @@
   private double m_setpoint;
   private double m_measurement;
 
+  private boolean m_haveMeasurement;
+  private boolean m_haveSetpoint;
+
   /**
    * Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
    * 0.02 seconds.
@@ -139,6 +145,22 @@
   }
 
   /**
+   * Sets the IZone range. When the absolute value of the position error is greater than IZone, the
+   * total accumulated error will reset to zero, disabling integral gain until the absolute value of
+   * the position error is less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
+   * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   */
+  public void setIZone(double iZone) {
+    if (iZone < 0) {
+      throw new IllegalArgumentException("IZone must be a non-negative number!");
+    }
+    m_iZone = iZone;
+  }
+
+  /**
    * Get the Proportional coefficient.
    *
    * @return proportional coefficient
@@ -166,6 +188,15 @@
   }
 
   /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  public double getIZone() {
+    return m_iZone;
+  }
+
+  /**
    * Returns the period of this controller.
    *
    * @return the period of the controller.
@@ -199,6 +230,7 @@
    */
   public void setSetpoint(double setpoint) {
     m_setpoint = setpoint;
+    m_haveSetpoint = true;
 
     if (m_continuous) {
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -227,7 +259,9 @@
    * @return Whether the error is within the acceptable bounds.
    */
   public boolean atSetpoint() {
-    return Math.abs(m_positionError) < m_positionTolerance
+    return m_haveMeasurement
+        && m_haveSetpoint
+        && Math.abs(m_positionError) < m_positionTolerance
         && Math.abs(m_velocityError) < m_velocityTolerance;
   }
 
@@ -321,6 +355,7 @@
    */
   public double calculate(double measurement, double setpoint) {
     m_setpoint = setpoint;
+    m_haveSetpoint = true;
     return calculate(measurement);
   }
 
@@ -333,6 +368,7 @@
   public double calculate(double measurement) {
     m_measurement = measurement;
     m_prevError = m_positionError;
+    m_haveMeasurement = true;
 
     if (m_continuous) {
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -343,7 +379,10 @@
 
     m_velocityError = (m_positionError - m_prevError) / m_period;
 
-    if (m_ki != 0) {
+    // If the absolute value of the position error is greater than IZone, reset the total error
+    if (Math.abs(m_positionError) > m_iZone) {
+      m_totalError = 0;
+    } else if (m_ki != 0) {
       m_totalError =
           MathUtil.clamp(
               m_totalError + m_positionError * m_period,
@@ -360,6 +399,7 @@
     m_prevError = 0;
     m_totalError = 0;
     m_velocityError = 0;
+    m_haveMeasurement = false;
   }
 
   @Override
@@ -368,6 +408,7 @@
     builder.addDoubleProperty("p", this::getP, this::setP);
     builder.addDoubleProperty("i", this::getI, this::setI);
     builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
     builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
index 02cc17d..80311f5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java

@@ -22,9 +22,11 @@
   private PIDController m_controller;
   private double m_minimumInput;
   private double m_maximumInput;
+
+  private TrapezoidProfile.Constraints m_constraints;
+  private TrapezoidProfile m_profile;
   private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
   private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
-  private TrapezoidProfile.Constraints m_constraints;
 
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
@@ -52,6 +54,7 @@
       double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
     m_controller = new PIDController(Kp, Ki, Kd, period);
     m_constraints = constraints;
+    m_profile = new TrapezoidProfile(m_constraints);
     instances++;
 
     SendableRegistry.add(this, "ProfiledPIDController", instances);
@@ -99,6 +102,19 @@
   }
 
   /**
+   * Sets the IZone range. When the absolute value of the position error is greater than IZone, the
+   * total accumulated error will reset to zero, disabling integral gain until the absolute value of
+   * the position error is less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
+   * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   */
+  public void setIZone(double iZone) {
+    m_controller.setIZone(iZone);
+  }
+
+  /**
    * Gets the proportional coefficient.
    *
    * @return proportional coefficient
@@ -126,6 +142,15 @@
   }
 
   /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  public double getIZone() {
+    return m_controller.getIZone();
+  }
+
+  /**
    * Gets the period of this controller.
    *
    * @return The period of the controller.
@@ -197,6 +222,16 @@
    */
   public void setConstraints(TrapezoidProfile.Constraints constraints) {
     m_constraints = constraints;
+    m_profile = new TrapezoidProfile(m_constraints);
+  }
+
+  /**
+   * Get the velocity and acceleration constraints for this controller.
+   *
+   * @return Velocity and acceleration constraints.
+   */
+  public TrapezoidProfile.Constraints getConstraints() {
+    return m_constraints;
   }
 
   /**
@@ -312,8 +347,7 @@
       m_setpoint.position = setpointMinDistance + measurement;
     }
 
-    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
-    m_setpoint = profile.calculate(getPeriod());
+    m_setpoint = m_profile.calculate(getPeriod(), m_goal, m_setpoint);
     return m_controller.calculate(measurement, m_setpoint.position);
   }
 
@@ -391,6 +425,7 @@
     builder.addDoubleProperty("p", this::getP, this::setP);
     builder.addDoubleProperty("i", this::getI, this::setI);
     builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
     builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
index b5fe591..7918f9f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java

@@ -4,22 +4,15 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Map;
-import java.util.Objects;
 
 /**
  * This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse
@@ -34,15 +27,7 @@
  * <p>{@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as
  * you want; if you never call it then this class will behave exactly like regular encoder odometry.
  */
-public class DifferentialDrivePoseEstimator {
-  private final DifferentialDriveKinematics m_kinematics;
-  private final DifferentialDriveOdometry m_odometry;
-  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
-  private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
-  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
-      TimeInterpolatableBuffer.createBuffer(1.5);
-
+public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDriveWheelPositions> {
   /**
    * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
    * vision measurements.
@@ -96,44 +81,12 @@
       Pose2d initialPoseMeters,
       Matrix<N3, N1> stateStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_kinematics = kinematics;
-    m_odometry =
+    super(
+        kinematics,
         new DifferentialDriveOdometry(
-            gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters);
-
-    for (int i = 0; i < 3; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
-    }
-
-    // Initialize vision R
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-  }
-
-  /**
-   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
-   * vision measurements after the autonomous period, or to change trust as distance to a vision
-   * target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
-   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
-   *     theta]ᵀ, with units in meters and radians.
-   */
-  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    var r = new double[3];
-    for (int i = 0; i < 3; ++i) {
-      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
-    }
-
-    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-    // and C = I. See wpimath/algorithms.md.
-    for (int row = 0; row < 3; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
-      } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
-      }
-    }
+            gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
+        stateStdDevs,
+        visionMeasurementStdDevs);
   }
 
   /**
@@ -152,111 +105,10 @@
       double leftPositionMeters,
       double rightPositionMeters,
       Pose2d poseMeters) {
-    // Reset state estimate and error covariance
-    m_odometry.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters);
-    m_poseBuffer.clear();
-  }
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  public Pose2d getEstimatedPosition() {
-    return m_odometry.getPoseMeters();
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * DifferentialDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)} then you
-   *     must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
-   *     the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
-   *     you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
-   *     or sync the epochs.
-   */
-  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
-    var sample = m_poseBuffer.getSample(timestampSeconds);
-
-    if (sample.isEmpty()) {
-      return;
-    }
-
-    // Step 2: Measure the twist between the odometry pose and the vision pose.
-    var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
-    // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
-    // Step 4: Convert back to Twist2d.
-    var scaledTwist =
-        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
-    // Step 5: Reset Odometry to state at sample with vision adjustment.
-    m_odometry.resetPosition(
-        sample.get().gyroAngle,
-        sample.get().leftMeters,
-        sample.get().rightMeters,
-        sample.get().poseMeters.exp(scaledTwist));
-
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
-    // pose buffer and correct odometry.
-    for (Map.Entry<Double, InterpolationRecord> entry :
-        m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
-      updateWithTime(
-          entry.getKey(),
-          entry.getValue().gyroAngle,
-          entry.getValue().leftMeters,
-          entry.getValue().rightMeters);
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * DifferentialDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * <p>Note that the vision measurement standard deviations passed into this method will continue
-   * to apply to future measurements until a subsequent call to {@link
-   * DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)}, then you
-   *     must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
-   *     the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that
-   *     you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
-   *     in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
-   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
-   *     the vision pose measurement less.
-   */
-  public void addVisionMeasurement(
-      Pose2d visionRobotPoseMeters,
-      double timestampSeconds,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+    resetPosition(
+        gyroAngle,
+        new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters),
+        poseMeters);
   }
 
   /**
@@ -270,8 +122,8 @@
    */
   public Pose2d update(
       Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
-    return updateWithTime(
-        WPIUtilJNI.now() * 1.0e-6, gyroAngle, distanceLeftMeters, distanceRightMeters);
+    return update(
+        gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
   }
 
   /**
@@ -289,98 +141,9 @@
       Rotation2d gyroAngle,
       double distanceLeftMeters,
       double distanceRightMeters) {
-    m_odometry.update(gyroAngle, distanceLeftMeters, distanceRightMeters);
-    m_poseBuffer.addSample(
+    return updateWithTime(
         currentTimeSeconds,
-        new InterpolationRecord(
-            getEstimatedPosition(), gyroAngle, distanceLeftMeters, distanceRightMeters));
-
-    return getEstimatedPosition();
-  }
-
-  /**
-   * Represents an odometry record. The record contains the inputs provided as well as the pose that
-   * was observed based on these inputs, as well as the previous record and its inputs.
-   */
-  private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
-    // The pose observed given the current sensor inputs and the previous pose.
-    private final Pose2d poseMeters;
-
-    // The current gyro angle.
-    private final Rotation2d gyroAngle;
-
-    // The distance traveled by the left encoder.
-    private final double leftMeters;
-
-    // The distance traveled by the right encoder.
-    private final double rightMeters;
-
-    /**
-     * Constructs an Interpolation Record with the specified parameters.
-     *
-     * @param pose The pose observed given the current sensor inputs and the previous pose.
-     * @param gyro The current gyro angle.
-     * @param leftMeters The distance traveled by the left encoder.
-     * @param rightMeters The distanced traveled by the right encoder.
-     */
-    private InterpolationRecord(
-        Pose2d poseMeters, Rotation2d gyro, double leftMeters, double rightMeters) {
-      this.poseMeters = poseMeters;
-      this.gyroAngle = gyro;
-      this.leftMeters = leftMeters;
-      this.rightMeters = rightMeters;
-    }
-
-    /**
-     * Return the interpolated record. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param endValue The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    @Override
-    public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
-      if (t < 0) {
-        return this;
-      } else if (t >= 1) {
-        return endValue;
-      } else {
-        // Find the new left distance.
-        var left_lerp = MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t);
-
-        // Find the new right distance.
-        var right_lerp = MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t);
-
-        // Find the new gyro angle.
-        var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
-        // Create a twist to represent this change based on the interpolated sensor inputs.
-        Twist2d twist = m_kinematics.toTwist2d(left_lerp - leftMeters, right_lerp - rightMeters);
-        twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
-        return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, left_lerp, right_lerp);
-      }
-    }
-
-    @Override
-    public boolean equals(Object obj) {
-      if (this == obj) {
-        return true;
-      }
-      if (!(obj instanceof InterpolationRecord)) {
-        return false;
-      }
-      InterpolationRecord record = (InterpolationRecord) obj;
-      return Objects.equals(gyroAngle, record.gyroAngle)
-          && Double.compare(leftMeters, record.leftMeters) == 0
-          && Double.compare(rightMeters, record.rightMeters) == 0
-          && Objects.equals(poseMeters, record.poseMeters);
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(gyroAngle, leftMeters, rightMeters, poseMeters);
-    }
+        gyroAngle,
+        new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
index 5f9e52e..59323b2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -59,6 +59,10 @@
   /**
    * Constructs an extended Kalman filter.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states a Nat representing the number of states.
    * @param inputs a Nat representing the number of inputs.
    * @param outputs a Nat representing the number of outputs.
@@ -93,6 +97,10 @@
   /**
    * Constructs an extended Kalman filter.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states a Nat representing the number of states.
    * @param inputs a Nat representing the number of inputs.
    * @param outputs a Nat representing the number of outputs.
@@ -138,15 +146,14 @@
         NumericalJacobian.numericalJacobianX(
             outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
 
-    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
     final var discA = discPair.getFirst();
     final var discQ = discPair.getSecond();
 
     final var discR = Discretization.discretizeR(m_contR, dtSeconds);
 
     if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
-      m_initP =
-          Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR);
+      m_initP = DARE.dare(discA.transpose(), C.transpose(), discQ, discR);
     } else {
       m_initP = new Matrix<>(states, states);
     }
@@ -260,7 +267,7 @@
     final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
 
     // Find discrete A and Q
-    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
     final var discA = discPair.getFirst();
     final var discQ = discPair.getSecond();
 
@@ -286,24 +293,14 @@
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
-   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
-   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
-   * of this function).
+   * <p>This is useful for when the measurement noise covariances vary.
    *
-   * @param <Rows> Number of rows in the result of f(x, u).
-   * @param rows Number of rows in the result of f(x, u).
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
-   * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param contR Continuous measurement noise covariance matrix.
+   * @param R Continuous measurement noise covariance matrix.
    */
-  public <Rows extends Num> void correct(
-      Nat<Rows> rows,
-      Matrix<Inputs, N1> u,
-      Matrix<Rows, N1> y,
-      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
-      Matrix<Rows, Rows> contR) {
-    correct(rows, u, y, h, contR, Matrix::minus, Matrix::plus);
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    correct(m_outputs, u, y, m_h, R, m_residualFuncY, m_addFuncX);
   }
 
   /**
@@ -318,7 +315,30 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param contR Continuous measurement noise covariance matrix.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public <Rows extends Num> void correct(
+      Nat<Rows> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<Rows, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
+      Matrix<Rows, Rows> R) {
+    correct(rows, u, y, h, R, Matrix::minus, Matrix::plus);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
    * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
    *     subtracts them.)
    * @param addFuncX A function that adds two state vectors.
@@ -328,11 +348,11 @@
       Matrix<Inputs, N1> u,
       Matrix<Rows, N1> y,
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
-      Matrix<Rows, Rows> contR,
+      Matrix<Rows, Rows> R,
       BiFunction<Matrix<Rows, N1>, Matrix<Rows, N1>, Matrix<Rows, N1>> residualFuncY,
       BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
     final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
-    final var discR = Discretization.discretizeR(contR, m_dtSeconds);
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
 
     final var S = C.times(m_P).times(C.transpose()).plus(discR);
 

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
index 24d6d91..08e3270 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -43,6 +43,10 @@
   /**
    * Constructs a state-space observer with the given plant.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states A Nat representing the states of the system.
    * @param outputs A Nat representing the outputs of the system.
    * @param plant The plant used for the prediction step.
@@ -65,7 +69,7 @@
     var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
     var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
 
-    var pair = Discretization.discretizeAQTaylor(plant.getA(), contQ, dtSeconds);
+    var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
     var discA = pair.getFirst();
     var discQ = pair.getSecond();
 
@@ -87,9 +91,7 @@
       throw new IllegalArgumentException(msg);
     }
 
-    var P =
-        new Matrix<>(
-            Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR));
+    var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
 
     // S = CPCᵀ + R
     var S = C.times(P).times(C.transpose()).plus(discR);

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
index 7a100f5..7b14839 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java

@@ -8,7 +8,7 @@
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
 
-interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+public interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
   Matrix<States, States> getP();
 
   double getP(int i, int j);

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
index 448b8d3..59bbb32 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java

@@ -4,23 +4,15 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
 import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Map;
-import java.util.Objects;
 
 /**
  * This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
@@ -33,15 +25,7 @@
  * <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
  * want; if you never call it, then this class will behave mostly like regular encoder odometry.
  */
-public class MecanumDrivePoseEstimator {
-  private final MecanumDriveKinematics m_kinematics;
-  private final MecanumDriveOdometry m_odometry;
-  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
-  private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
-  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
-      TimeInterpolatableBuffer.createBuffer(1.5);
-
+public class MecanumDrivePoseEstimator extends PoseEstimator<MecanumDriveWheelPositions> {
   /**
    * Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
    * vision measurements.
@@ -90,294 +74,10 @@
       Pose2d initialPoseMeters,
       Matrix<N3, N1> stateStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_kinematics = kinematics;
-    m_odometry = new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
-
-    for (int i = 0; i < 3; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
-    }
-
-    // Initialize vision R
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-  }
-
-  /**
-   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
-   * vision measurements after the autonomous period, or to change trust as distance to a vision
-   * target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
-   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
-   *     theta]ᵀ, with units in meters and radians.
-   */
-  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    var r = new double[3];
-    for (int i = 0; i < 3; ++i) {
-      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
-    }
-
-    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-    // and C = I. See wpimath/algorithms.md.
-    for (int row = 0; row < 3; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
-      } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
-      }
-    }
-  }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>The gyroscope angle does not need to be reset in the user's robot code. The library
-   * automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances driven by each wheel.
-   * @param poseMeters The position on the field that your robot is at.
-   */
-  public void resetPosition(
-      Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
-    // Reset state estimate and error covariance
-    m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
-    m_poseBuffer.clear();
-  }
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  public Pose2d getEstimatedPosition() {
-    return m_odometry.getPoseMeters();
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * MecanumDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)}
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
-   *     timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.)
-   *     This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
-   *     your time source or sync the epochs.
-   */
-  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
-    var sample = m_poseBuffer.getSample(timestampSeconds);
-
-    if (sample.isEmpty()) {
-      return;
-    }
-
-    // Step 2: Measure the twist between the odometry pose and the vision pose.
-    var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
-    // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
-    // Step 4: Convert back to Twist2d.
-    var scaledTwist =
-        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
-    // Step 5: Reset Odometry to state at sample with vision adjustment.
-    m_odometry.resetPosition(
-        sample.get().gyroAngle,
-        sample.get().wheelPositions,
-        sample.get().poseMeters.exp(scaledTwist));
-
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
-    // pose buffer and correct odometry.
-    for (Map.Entry<Double, InterpolationRecord> entry :
-        m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
-      updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * MecanumDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * <p>Note that the vision measurement standard deviations passed into this method will continue
-   * to apply to future measurements until a subsequent call to {@link
-   * MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)},
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
-   *     timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
-   *     This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
-   *     your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
-   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
-   *     the vision pose measurement less.
-   */
-  public void addVisionMeasurement(
-      Pose2d visionRobotPoseMeters,
-      double timestampSeconds,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
-   * loop.
-   *
-   * @param gyroAngle The current gyro angle.
-   * @param wheelPositions The distances driven by each wheel.
-   * @return The estimated pose of the robot in meters.
-   */
-  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelPositions);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
-   * loop.
-   *
-   * @param currentTimeSeconds Time at which this method was called, in seconds.
-   * @param gyroAngle The current gyroscope angle.
-   * @param wheelPositions The distances driven by each wheel.
-   * @return The estimated pose of the robot in meters.
-   */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
-    m_odometry.update(gyroAngle, wheelPositions);
-
-    m_poseBuffer.addSample(
-        currentTimeSeconds,
-        new InterpolationRecord(
-            getEstimatedPosition(),
-            gyroAngle,
-            new MecanumDriveWheelPositions(
-                wheelPositions.frontLeftMeters,
-                wheelPositions.frontRightMeters,
-                wheelPositions.rearLeftMeters,
-                wheelPositions.rearRightMeters)));
-
-    return getEstimatedPosition();
-  }
-
-  /**
-   * Represents an odometry record. The record contains the inputs provided as well as the pose that
-   * was observed based on these inputs, as well as the previous record and its inputs.
-   */
-  private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
-    // The pose observed given the current sensor inputs and the previous pose.
-    private final Pose2d poseMeters;
-
-    // The current gyro angle.
-    private final Rotation2d gyroAngle;
-
-    // The distances traveled by each wheel encoder.
-    private final MecanumDriveWheelPositions wheelPositions;
-
-    /**
-     * Constructs an Interpolation Record with the specified parameters.
-     *
-     * @param pose The pose observed given the current sensor inputs and the previous pose.
-     * @param gyro The current gyro angle.
-     * @param wheelPositions The distances traveled by each wheel encoder.
-     */
-    private InterpolationRecord(
-        Pose2d poseMeters, Rotation2d gyro, MecanumDriveWheelPositions wheelPositions) {
-      this.poseMeters = poseMeters;
-      this.gyroAngle = gyro;
-      this.wheelPositions = wheelPositions;
-    }
-
-    /**
-     * Return the interpolated record. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param endValue The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    @Override
-    public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
-      if (t < 0) {
-        return this;
-      } else if (t >= 1) {
-        return endValue;
-      } else {
-        // Find the new wheel distances.
-        var wheels_lerp =
-            new MecanumDriveWheelPositions(
-                MathUtil.interpolate(
-                    this.wheelPositions.frontLeftMeters,
-                    endValue.wheelPositions.frontLeftMeters,
-                    t),
-                MathUtil.interpolate(
-                    this.wheelPositions.frontRightMeters,
-                    endValue.wheelPositions.frontRightMeters,
-                    t),
-                MathUtil.interpolate(
-                    this.wheelPositions.rearLeftMeters, endValue.wheelPositions.rearLeftMeters, t),
-                MathUtil.interpolate(
-                    this.wheelPositions.rearRightMeters,
-                    endValue.wheelPositions.rearRightMeters,
-                    t));
-
-        // Find the distance travelled between this measurement and the interpolated measurement.
-        var wheels_delta =
-            new MecanumDriveWheelPositions(
-                wheels_lerp.frontLeftMeters - this.wheelPositions.frontLeftMeters,
-                wheels_lerp.frontRightMeters - this.wheelPositions.frontRightMeters,
-                wheels_lerp.rearLeftMeters - this.wheelPositions.rearLeftMeters,
-                wheels_lerp.rearRightMeters - this.wheelPositions.rearRightMeters);
-
-        // Find the new gyro angle.
-        var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
-        // Create a twist to represent this change based on the interpolated sensor inputs.
-        Twist2d twist = m_kinematics.toTwist2d(wheels_delta);
-        twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
-        return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, wheels_lerp);
-      }
-    }
-
-    @Override
-    public boolean equals(Object obj) {
-      if (this == obj) {
-        return true;
-      }
-      if (!(obj instanceof InterpolationRecord)) {
-        return false;
-      }
-      InterpolationRecord record = (InterpolationRecord) obj;
-      return Objects.equals(gyroAngle, record.gyroAngle)
-          && Objects.equals(wheelPositions, record.wheelPositions)
-          && Objects.equals(poseMeters, record.poseMeters);
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(gyroAngle, wheelPositions, poseMeters);
-    }
+    super(
+        kinematics,
+        new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
+        stateStdDevs,
+        visionMeasurementStdDevs);
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
new file mode 100644
index 0000000..bb30f25
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java

@@ -0,0 +1,333 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.kinematics.Kinematics;
+import edu.wpi.first.math.kinematics.Odometry;
+import edu.wpi.first.math.kinematics.WheelPositions;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Map;
+import java.util.NoSuchElementException;
+import java.util.Objects;
+
+/**
+ * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder
+ * measurements. Robot code should not use this directly- Instead, use the particular type for your
+ * drivetrain (e.g., {@link DifferentialDrivePoseEstimator}). It is intended to be a drop-in
+ * replacement for {@link Odometry}; in fact, if you never call {@link
+ * PoseEstimator#addVisionMeasurement} and only call {@link PoseEstimator#update} then this will
+ * behave exactly the same as Odometry.
+ *
+ * <p>{@link PoseEstimator#update} should be called every robot loop.
+ *
+ * <p>{@link PoseEstimator#addVisionMeasurement} can be called as infrequently as you want; if you
+ * never call it then this class will behave exactly like regular encoder odometry.
+ */
+public class PoseEstimator<T extends WheelPositions<T>> {
+  private final Kinematics<?, T> m_kinematics;
+  private final Odometry<T> m_odometry;
+  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+  private final Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
+
+  private static final double kBufferDuration = 1.5;
+  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+      TimeInterpolatableBuffer.createBuffer(kBufferDuration);
+
+  /**
+   * Constructs a PoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param odometry A correctly-configured odometry object for your drivetrain.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public PoseEstimator(
+      Kinematics<?, T> kinematics,
+      Odometry<T> odometry,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    m_kinematics = kinematics;
+    m_odometry = odometry;
+
+    for (int i = 0; i < 3; ++i) {
+      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+    }
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    var r = new double[3];
+    for (int i = 0; i < 3; ++i) {
+      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+    }
+
+    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+    // and C = I. See wpimath/algorithms.md.
+    for (int row = 0; row < 3; ++row) {
+      if (m_q.get(row, 0) == 0.0) {
+        m_visionK.set(row, row, 0.0);
+      } else {
+        m_visionK.set(
+            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+      }
+    }
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param poseMeters The position on the field that your robot is at.
+   */
+  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
+    // Reset state estimate and error covariance
+    m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
+    m_poseBuffer.clear();
+  }
+
+  /**
+   * Gets the estimated robot pose.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return m_odometry.getPoseMeters();
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+   * while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * PoseEstimator#update} every loop.
+   *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link
+   *     PoseEstimator#updateWithTime(double,Rotation2d,WheelPositions)} then you must use a
+   *     timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
+   *     epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you
+   *     should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or
+   *     sync the epochs.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
+    try {
+      if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+        return;
+      }
+    } catch (NoSuchElementException ex) {
+      return;
+    }
+
+    // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
+    var sample = m_poseBuffer.getSample(timestampSeconds);
+
+    if (sample.isEmpty()) {
+      return;
+    }
+
+    // Step 2: Measure the twist between the odometry pose and the vision pose.
+    var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+    // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
+    // gain matrix representing how much we trust vision measurements compared to our current pose.
+    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
+
+    // Step 4: Convert back to Twist2d.
+    var scaledTwist =
+        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+
+    // Step 5: Reset Odometry to state at sample with vision adjustment.
+    m_odometry.resetPosition(
+        sample.get().gyroAngle,
+        sample.get().wheelPositions,
+        sample.get().poseMeters.exp(scaledTwist));
+
+    // Step 6: Record the current pose to allow multiple measurements from the same timestamp
+    m_poseBuffer.addSample(
+        timestampSeconds,
+        new InterpolationRecord(
+            getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions));
+
+    // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
+    // pose buffer and correct odometry.
+    for (Map.Entry<Double, InterpolationRecord> entry :
+        m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
+      updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
+    }
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+   * while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * PoseEstimator#update} every loop.
+   *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * PoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link #updateWithTime}, then you must use a
+   *     timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
+   *     epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you
+   *     should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in
+   *     this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The current encoder readings.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+    return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param currentTimeSeconds Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The current encoder readings.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) {
+    m_odometry.update(gyroAngle, wheelPositions);
+    m_poseBuffer.addSample(
+        currentTimeSeconds,
+        new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy()));
+
+    return getEstimatedPosition();
+  }
+
+  /**
+   * Represents an odometry record. The record contains the inputs provided as well as the pose that
+   * was observed based on these inputs, as well as the previous record and its inputs.
+   */
+  private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
+    // The pose observed given the current sensor inputs and the previous pose.
+    private final Pose2d poseMeters;
+
+    // The current gyro angle.
+    private final Rotation2d gyroAngle;
+
+    // The current encoder readings.
+    private final T wheelPositions;
+
+    /**
+     * Constructs an Interpolation Record with the specified parameters.
+     *
+     * @param poseMeters The pose observed given the current sensor inputs and the previous pose.
+     * @param gyro The current gyro angle.
+     * @param wheelPositions The current encoder readings.
+     */
+    private InterpolationRecord(Pose2d poseMeters, Rotation2d gyro, T wheelPositions) {
+      this.poseMeters = poseMeters;
+      this.gyroAngle = gyro;
+      this.wheelPositions = wheelPositions;
+    }
+
+    /**
+     * Return the interpolated record. This object is assumed to be the starting position, or lower
+     * bound.
+     *
+     * @param endValue The upper bound, or end.
+     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+     * @return The interpolated value.
+     */
+    @Override
+    public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
+      if (t < 0) {
+        return this;
+      } else if (t >= 1) {
+        return endValue;
+      } else {
+        // Find the new wheel distances.
+        var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t);
+
+        // Find the new gyro angle.
+        var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+        // Create a twist to represent the change based on the interpolated sensor inputs.
+        Twist2d twist = m_kinematics.toTwist2d(wheelPositions, wheelLerp);
+        twist.dtheta = gyroLerp.minus(gyroAngle).getRadians();
+
+        return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp);
+      }
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (this == obj) {
+        return true;
+      }
+      if (!(obj instanceof PoseEstimator.InterpolationRecord)) {
+        return false;
+      }
+      var record = (PoseEstimator<?>.InterpolationRecord) obj;
+      return Objects.equals(gyroAngle, record.gyroAngle)
+          && Objects.equals(wheelPositions, record.wheelPositions)
+          && Objects.equals(poseMeters, record.poseMeters);
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(gyroAngle, wheelPositions, poseMeters);
+    }
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
index 57451a0..f778b62 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java

@@ -4,24 +4,16 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Arrays;
-import java.util.Map;
-import java.util.Objects;
 
 /**
  * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
@@ -33,15 +25,8 @@
  * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
  * want; if you never call it, then this class will behave as regular encoder odometry.
  */
-public class SwerveDrivePoseEstimator {
-  private final SwerveDriveKinematics m_kinematics;
-  private final SwerveDriveOdometry m_odometry;
-  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveDriveWheelPositions> {
   private final int m_numModules;
-  private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
-  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
-      TimeInterpolatableBuffer.createBuffer(1.5);
 
   /**
    * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
@@ -91,43 +76,13 @@
       Pose2d initialPoseMeters,
       Matrix<N3, N1> stateStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_kinematics = kinematics;
-    m_odometry = new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters);
-
-    for (int i = 0; i < 3; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
-    }
+    super(
+        kinematics,
+        new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
+        stateStdDevs,
+        visionMeasurementStdDevs);
 
     m_numModules = modulePositions.length;
-
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-  }
-
-  /**
-   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
-   * vision measurements after the autonomous period, or to change trust as distance to a vision
-   * target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
-   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
-   *     theta]ᵀ, with units in meters and radians.
-   */
-  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    var r = new double[3];
-    for (int i = 0; i < 3; ++i) {
-      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
-    }
-
-    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-    // and C = I. See wpimath/algorithms.md.
-    for (int row = 0; row < 3; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
-      } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
-      }
-    }
   }
 
   /**
@@ -142,106 +97,7 @@
    */
   public void resetPosition(
       Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
-    // Reset state estimate and error covariance
-    m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
-    m_poseBuffer.clear();
-  }
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  public Pose2d getEstimatedPosition() {
-    return m_odometry.getPoseMeters();
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * SwerveDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])} then you
-   *     must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
-   *     the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
-   *     you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
-   *     or sync the epochs.
-   */
-  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
-    var sample = m_poseBuffer.getSample(timestampSeconds);
-
-    if (sample.isEmpty()) {
-      return;
-    }
-
-    // Step 2: Measure the twist between the odometry pose and the vision pose.
-    var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
-    // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
-    // Step 4: Convert back to Twist2d.
-    var scaledTwist =
-        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
-    // Step 5: Reset Odometry to state at sample with vision adjustment.
-    m_odometry.resetPosition(
-        sample.get().gyroAngle,
-        sample.get().modulePositions,
-        sample.get().poseMeters.exp(scaledTwist));
-
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
-    // pose buffer and correct odometry.
-    for (Map.Entry<Double, InterpolationRecord> entry :
-        m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
-      updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().modulePositions);
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * SwerveDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * <p>Note that the vision measurement standard deviations passed into this method will continue
-   * to apply to future measurements until a subsequent call to {@link
-   * SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])}, then
-   *     you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
-   *     timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
-   *     This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
-   *     your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
-   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
-   *     the vision pose measurement less.
-   */
-  public void addVisionMeasurement(
-      Pose2d visionRobotPoseMeters,
-      double timestampSeconds,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters);
   }
 
   /**
@@ -253,7 +109,7 @@
    * @return The estimated pose of the robot in meters.
    */
   public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, modulePositions);
+    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
   }
 
   /**
@@ -267,118 +123,19 @@
    */
   public Pose2d updateWithTime(
       double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    if (modulePositions.length != m_numModules) {
+    return updateWithTime(
+        currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+  }
+
+  @Override
+  public Pose2d updateWithTime(
+      double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) {
+    if (wheelPositions.positions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");
     }
 
-    var internalModulePositions = new SwerveModulePosition[m_numModules];
-
-    for (int i = 0; i < m_numModules; i++) {
-      internalModulePositions[i] =
-          new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
-    }
-
-    m_odometry.update(gyroAngle, internalModulePositions);
-
-    m_poseBuffer.addSample(
-        currentTimeSeconds,
-        new InterpolationRecord(getEstimatedPosition(), gyroAngle, internalModulePositions));
-
-    return getEstimatedPosition();
-  }
-
-  /**
-   * Represents an odometry record. The record contains the inputs provided as well as the pose that
-   * was observed based on these inputs, as well as the previous record and its inputs.
-   */
-  private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
-    // The pose observed given the current sensor inputs and the previous pose.
-    private final Pose2d poseMeters;
-
-    // The current gyro angle.
-    private final Rotation2d gyroAngle;
-
-    // The distances and rotations measured at each module.
-    private final SwerveModulePosition[] modulePositions;
-
-    /**
-     * Constructs an Interpolation Record with the specified parameters.
-     *
-     * @param pose The pose observed given the current sensor inputs and the previous pose.
-     * @param gyro The current gyro angle.
-     * @param wheelPositions The distances and rotations measured at each wheel.
-     */
-    private InterpolationRecord(
-        Pose2d poseMeters, Rotation2d gyro, SwerveModulePosition[] modulePositions) {
-      this.poseMeters = poseMeters;
-      this.gyroAngle = gyro;
-      this.modulePositions = modulePositions;
-    }
-
-    /**
-     * Return the interpolated record. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param endValue The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    @Override
-    public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
-      if (t < 0) {
-        return this;
-      } else if (t >= 1) {
-        return endValue;
-      } else {
-        // Find the new wheel distances.
-        var modulePositions = new SwerveModulePosition[m_numModules];
-
-        // Find the distance travelled between this measurement and the interpolated measurement.
-        var moduleDeltas = new SwerveModulePosition[m_numModules];
-
-        for (int i = 0; i < m_numModules; i++) {
-          double ds =
-              MathUtil.interpolate(
-                  this.modulePositions[i].distanceMeters,
-                  endValue.modulePositions[i].distanceMeters,
-                  t);
-          Rotation2d theta =
-              this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t);
-          modulePositions[i] = new SwerveModulePosition(ds, theta);
-          moduleDeltas[i] =
-              new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta);
-        }
-
-        // Find the new gyro angle.
-        var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
-        // Create a twist to represent this change based on the interpolated sensor inputs.
-        Twist2d twist = m_kinematics.toTwist2d(moduleDeltas);
-        twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
-        return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, modulePositions);
-      }
-    }
-
-    @Override
-    public boolean equals(Object obj) {
-      if (this == obj) {
-        return true;
-      }
-      if (!(obj instanceof InterpolationRecord)) {
-        return false;
-      }
-      InterpolationRecord record = (InterpolationRecord) obj;
-      return Objects.equals(gyroAngle, record.gyroAngle)
-          && Arrays.equals(modulePositions, record.modulePositions)
-          && Objects.equals(poseMeters, record.poseMeters);
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(gyroAngle, Arrays.hashCode(modulePositions), poseMeters);
-    }
+    return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
index d668564..a5de856 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java

@@ -64,6 +64,10 @@
   /**
    * Constructs an Unscented Kalman Filter.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states A Nat representing the number of states.
    * @param outputs A Nat representing the number of outputs.
    * @param f A vector-valued function of x and u that returns the derivative of the state vector.
@@ -100,6 +104,10 @@
    * custom functions for arithmetic can be useful if you have angles in the state or measurements,
    * because they allow you to correctly account for the modular nature of angle arithmetic.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states A Nat representing the number of states.
    * @param outputs A Nat representing the number of outputs.
    * @param f A vector-valued function of x and u that returns the derivative of the state vector.
@@ -331,7 +339,7 @@
     // Discretize Q before projecting mean and covariance forward
     Matrix<States, States> contA =
         NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
-    var discQ = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
+    var discQ = Discretization.discretizeAQ(contA, m_contQ, dtSeconds).getSecond();
     var squareRootDiscQ = discQ.lltDecompose(true);
 
     var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
@@ -373,6 +381,19 @@
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
+   * <p>This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    correct(m_outputs, u, y, m_h, R, m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
    * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
    * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
    * of this function).
@@ -382,7 +403,7 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param R Continuous measurement noise covariance matrix.
    */
   public <R extends Num> void correct(
       Nat<R> rows,
@@ -411,7 +432,7 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param R Continuous measurement noise covariance matrix.
    * @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
    *     a given set of weights.
    * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it

diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
index 8da45e9..3472198 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.math.filter;
 
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.math.MathSharedStore;
 
 /**
  * A simple debounce filter for boolean streams. Requires that the boolean change value from
@@ -60,11 +60,11 @@
   }
 
   private void resetTimer() {
-    m_prevTimeSeconds = WPIUtilJNI.now() * 1e-6;
+    m_prevTimeSeconds = MathSharedStore.getTimestamp();
   }
 
   private boolean hasElapsed() {
-    return (WPIUtilJNI.now() * 1e-6) - m_prevTimeSeconds >= m_debounceTimeSeconds;
+    return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds;
   }
 
   /**

diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
index 668b8b1..6c34f35 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java

@@ -4,8 +4,8 @@
 
 package edu.wpi.first.math.filter;
 
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.util.WPIUtilJNI;
 
 /**
  * A class that limits the rate of change of an input value. Useful for implementing voltage,
@@ -33,21 +33,7 @@
     m_positiveRateLimit = positiveRateLimit;
     m_negativeRateLimit = negativeRateLimit;
     m_prevVal = initialValue;
-    m_prevTime = WPIUtilJNI.now() * 1e-6;
-  }
-
-  /**
-   * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
-   * -rateLimit and initial value.
-   *
-   * @param rateLimit The rate-of-change limit, in units per second.
-   * @param initalValue The initial value of the input.
-   * @deprecated Use SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double
-   *     initalValue) instead.
-   */
-  @Deprecated(since = "2023", forRemoval = true)
-  public SlewRateLimiter(double rateLimit, double initalValue) {
-    this(rateLimit, -rateLimit, initalValue);
+    m_prevTime = MathSharedStore.getTimestamp();
   }
 
   /**
@@ -67,7 +53,7 @@
    * @return The filtered value, which will not change faster than the slew rate.
    */
   public double calculate(double input) {
-    double currentTime = WPIUtilJNI.now() * 1e-6;
+    double currentTime = MathSharedStore.getTimestamp();
     double elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
         MathUtil.clamp(
@@ -85,6 +71,6 @@
    */
   public void reset(double value) {
     m_prevVal = value;
-    m_prevTime = WPIUtilJNI.now() * 1e-6;
+    m_prevTime = MathSharedStore.getTimestamp();
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
index 5733177..9e827c3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java

@@ -124,7 +124,9 @@
    */
   public static Transform3d convert(
       Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
+    var coordRot = from.m_rotation.minus(to.m_rotation);
     return new Transform3d(
-        convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to));
+        convert(transform.getTranslation(), from, to),
+        coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot)));
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
index bce832e..bc7f536 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -9,7 +9,15 @@
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** Represents a 2D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
@@ -136,6 +144,16 @@
   }
 
   /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d rotateBy(Rotation2d other) {
+    return new Pose2d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
+  /**
    * Transforms the pose by the given transformation and returns the new pose. See + operator for
    * the matrix multiplication performed.
    *
@@ -238,6 +256,23 @@
     return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
   }
 
+  /**
+   * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same
+   * distance from this pose, return the one with the closest rotation component.
+   *
+   * @param poses The list of poses to find the nearest.
+   * @return The nearest Pose2d from the list.
+   */
+  public Pose2d nearest(List<Pose2d> poses) {
+    return Collections.min(
+        poses,
+        Comparator.comparing(
+                (Pose2d other) -> this.getTranslation().getDistance(other.getTranslation()))
+            .thenComparing(
+                (Pose2d other) ->
+                    Math.abs(this.getRotation().minus(other.getRotation()).getRadians())));
+  }
+
   @Override
   public String toString() {
     return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
@@ -275,4 +310,83 @@
       return this.exp(scaledTwist);
     }
   }
+
+  public static final class AStruct implements Struct<Pose2d> {
+    @Override
+    public Class<Pose2d> getTypeClass() {
+      return Pose2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Pose2d";
+    }
+
+    @Override
+    public int getSize() {
+      return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Translation2d translation;Rotation2d rotation";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+    }
+
+    @Override
+    public Pose2d unpack(ByteBuffer bb) {
+      Translation2d translation = Translation2d.struct.unpack(bb);
+      Rotation2d rotation = Rotation2d.struct.unpack(bb);
+      return new Pose2d(translation, rotation);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Pose2d value) {
+      Translation2d.struct.pack(bb, value.m_translation);
+      Rotation2d.struct.pack(bb, value.m_rotation);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Pose2d, ProtobufPose2d> {
+    @Override
+    public Class<Pose2d> getTypeClass() {
+      return Pose2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufPose2d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
+    }
+
+    @Override
+    public ProtobufPose2d createMessage() {
+      return ProtobufPose2d.newInstance();
+    }
+
+    @Override
+    public Pose2d unpack(ProtobufPose2d msg) {
+      return new Pose2d(
+          Translation2d.proto.unpack(msg.getTranslation()),
+          Rotation2d.proto.unpack(msg.getRotation()));
+    }
+
+    @Override
+    public void pack(ProtobufPose2d msg, Pose2d value) {
+      Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+      Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
index 8e3a3fe..2ea7094 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -8,14 +8,14 @@
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** Represents a 3D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
@@ -68,7 +68,10 @@
   }
 
   /**
-   * Transforms the pose by the given transformation and returns the new transformed pose.
+   * Transforms the pose by the given transformation and returns the new transformed pose. The
+   * transform is applied relative to the pose's frame. Note that this differs from {@link
+   * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
+   * origin.
    *
    * @param other The transform to transform the pose by.
    * @return The transformed pose.
@@ -156,8 +159,21 @@
   }
 
   /**
-   * Transforms the pose by the given transformation and returns the new pose. See + operator for
-   * the matrix multiplication performed.
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by, which is applied extrinsically (from the
+   *     global frame).
+   * @return The rotated pose.
+   */
+  public Pose3d rotateBy(Rotation3d other) {
+    return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new transformed pose. The
+   * transform is applied relative to the pose's frame. Note that this differs from {@link
+   * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
+   * origin.
    *
    * @param other The transform to transform the pose by.
    * @return The transformed pose.
@@ -200,36 +216,28 @@
    * @return The new pose of the robot.
    */
   public Pose3d exp(Twist3d twist) {
-    final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz));
-    final var OmegaSq = Omega.times(Omega);
-
-    double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz;
-
-    // Get left Jacobian of SO3. See first line in right column of
-    // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
-    Matrix<N3, N3> J;
-    if (thetaSq < 1E-9 * 1E-9) {
-      // J = I + 0.5ω
-      J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
-    } else {
-      double theta = Math.sqrt(thetaSq);
-      // J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
-      J =
-          Matrix.eye(Nat.N3())
-              .plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
-              .plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
-    }
-
-    // Get translation component
-    final var translation =
-        J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
-
-    final var transform =
-        new Transform3d(
-            new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
-            new Rotation3d(twist.rx, twist.ry, twist.rz));
-
-    return this.plus(transform);
+    var quaternion = this.getRotation().getQuaternion();
+    double[] resultArray =
+        WPIMathJNI.expPose3d(
+            this.getX(),
+            this.getY(),
+            this.getZ(),
+            quaternion.getW(),
+            quaternion.getX(),
+            quaternion.getY(),
+            quaternion.getZ(),
+            twist.dx,
+            twist.dy,
+            twist.dz,
+            twist.rx,
+            twist.ry,
+            twist.rz);
+    return new Pose3d(
+        resultArray[0],
+        resultArray[1],
+        resultArray[2],
+        new Rotation3d(
+            new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
   }
 
   /**
@@ -240,50 +248,31 @@
    * @return The twist that maps this to end.
    */
   public Twist3d log(Pose3d end) {
-    final var transform = end.relativeTo(this);
-
-    final var rotVec = transform.getRotation().getQuaternion().toRotationVector();
-
-    final var Omega = rotationVectorToMatrix(rotVec);
-    final var OmegaSq = Omega.times(Omega);
-
-    double thetaSq =
-        rotVec.get(0, 0) * rotVec.get(0, 0)
-            + rotVec.get(1, 0) * rotVec.get(1, 0)
-            + rotVec.get(2, 0) * rotVec.get(2, 0);
-
-    // Get left Jacobian inverse of SO3. See fourth line in right column of
-    // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
-    Matrix<N3, N3> Jinv;
-    if (thetaSq < 1E-9 * 1E-9) {
-      // J⁻¹ = I − 0.5ω + 1/12 ω²
-      Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
-    } else {
-      double theta = Math.sqrt(thetaSq);
-      double halfTheta = 0.5 * theta;
-
-      // J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
-      Jinv =
-          Matrix.eye(Nat.N3())
-              .minus(Omega.times(0.5))
-              .plus(
-                  OmegaSq.times(
-                      (1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
-    }
-
-    // Get dtranslation component
-    final var dtranslation =
-        Jinv.times(
-            new MatBuilder<>(Nat.N3(), Nat.N1())
-                .fill(transform.getX(), transform.getY(), transform.getZ()));
-
+    var thisQuaternion = this.getRotation().getQuaternion();
+    var endQuaternion = end.getRotation().getQuaternion();
+    double[] resultArray =
+        WPIMathJNI.logPose3d(
+            this.getX(),
+            this.getY(),
+            this.getZ(),
+            thisQuaternion.getW(),
+            thisQuaternion.getX(),
+            thisQuaternion.getY(),
+            thisQuaternion.getZ(),
+            end.getX(),
+            end.getY(),
+            end.getZ(),
+            endQuaternion.getW(),
+            endQuaternion.getX(),
+            endQuaternion.getY(),
+            endQuaternion.getZ());
     return new Twist3d(
-        dtranslation.get(0, 0),
-        dtranslation.get(1, 0),
-        dtranslation.get(2, 0),
-        rotVec.get(0, 0),
-        rotVec.get(1, 0),
-        rotVec.get(2, 0));
+        resultArray[0],
+        resultArray[1],
+        resultArray[2],
+        resultArray[3],
+        resultArray[4],
+        resultArray[5]);
   }
 
   /**
@@ -335,30 +324,82 @@
     }
   }
 
-  /**
-   * Applies the hat operator to a rotation vector.
-   *
-   * <p>It takes a rotation vector and returns the corresponding matrix representation of the Lie
-   * algebra element (a 3x3 rotation matrix).
-   *
-   * @param rotation The rotation vector.
-   * @return The rotation vector as a 3x3 rotation matrix.
-   */
-  private Matrix<N3, N3> rotationVectorToMatrix(Vector<N3> rotation) {
-    // Given a rotation vector <a, b, c>,
-    //         [ 0 -c  b]
-    // Omega = [ c  0 -a]
-    //         [-b  a  0]
-    return new MatBuilder<>(Nat.N3(), Nat.N3())
-        .fill(
-            0.0,
-            -rotation.get(2, 0),
-            rotation.get(1, 0),
-            rotation.get(2, 0),
-            0.0,
-            -rotation.get(0, 0),
-            -rotation.get(1, 0),
-            rotation.get(0, 0),
-            0.0);
+  public static final class AStruct implements Struct<Pose3d> {
+    @Override
+    public Class<Pose3d> getTypeClass() {
+      return Pose3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Pose3d";
+    }
+
+    @Override
+    public int getSize() {
+      return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Translation3d translation;Rotation3d rotation";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+    }
+
+    @Override
+    public Pose3d unpack(ByteBuffer bb) {
+      Translation3d translation = Translation3d.struct.unpack(bb);
+      Rotation3d rotation = Rotation3d.struct.unpack(bb);
+      return new Pose3d(translation, rotation);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Pose3d value) {
+      Translation3d.struct.pack(bb, value.m_translation);
+      Rotation3d.struct.pack(bb, value.m_rotation);
+    }
   }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Pose3d, ProtobufPose3d> {
+    @Override
+    public Class<Pose3d> getTypeClass() {
+      return Pose3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufPose3d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
+    }
+
+    @Override
+    public ProtobufPose3d createMessage() {
+      return ProtobufPose3d.newInstance();
+    }
+
+    @Override
+    public Pose3d unpack(ProtobufPose3d msg) {
+      return new Pose3d(
+          Translation3d.proto.unpack(msg.getTranslation()),
+          Rotation3d.proto.unpack(msg.getRotation()));
+    }
+
+    @Override
+    public void pack(ProtobufPose3d msg, Pose3d value) {
+      Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+      Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
index cadfaa4..23fd26b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java

@@ -11,18 +11,30 @@
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Quaternion {
-  private final double m_r;
-  private final Vector<N3> m_v;
+  // Scalar r in versor form
+  private final double m_w;
+
+  // Vector v in versor form
+  private final double m_x;
+  private final double m_y;
+  private final double m_z;
 
   /** Constructs a quaternion with a default angle of 0 degrees. */
   public Quaternion() {
-    m_r = 1.0;
-    m_v = VecBuilder.fill(0.0, 0.0, 0.0);
+    m_w = 1.0;
+    m_x = 0.0;
+    m_y = 0.0;
+    m_z = 0.0;
   }
 
   /**
@@ -39,8 +51,52 @@
       @JsonProperty(required = true, value = "X") double x,
       @JsonProperty(required = true, value = "Y") double y,
       @JsonProperty(required = true, value = "Z") double z) {
-    m_r = w;
-    m_v = VecBuilder.fill(x, y, z);
+    m_w = w;
+    m_x = x;
+    m_y = y;
+    m_z = z;
+  }
+
+  /**
+   * Adds another quaternion to this quaternion entrywise.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion sum.
+   */
+  public Quaternion plus(Quaternion other) {
+    return new Quaternion(
+        getW() + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
+  }
+
+  /**
+   * Subtracts another quaternion from this quaternion entrywise.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion difference.
+   */
+  public Quaternion minus(Quaternion other) {
+    return new Quaternion(
+        getW() - other.getW(), getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ());
+  }
+
+  /**
+   * Divides by a scalar.
+   *
+   * @param scalar The value to scale each component by.
+   * @return The scaled quaternion.
+   */
+  public Quaternion divide(double scalar) {
+    return new Quaternion(getW() / scalar, getX() / scalar, getY() / scalar, getZ() / scalar);
+  }
+
+  /**
+   * Multiplies with a scalar.
+   *
+   * @param scalar The value to scale each component by.
+   * @return The scaled quaternion.
+   */
+  public Quaternion times(double scalar) {
+    return new Quaternion(getW() * scalar, getX() * scalar, getY() * scalar, getZ() * scalar);
   }
 
   /**
@@ -51,28 +107,29 @@
    */
   public Quaternion times(Quaternion other) {
     // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
-    final var r1 = m_r;
-    final var v1 = m_v;
-    final var r2 = other.m_r;
-    final var v2 = other.m_v;
+    final var r1 = m_w;
+    final var r2 = other.m_w;
+
+    // v₁ ⋅ v₂
+    double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
 
     // v₁ x v₂
-    var cross =
-        VecBuilder.fill(
-            v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0),
-            v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0),
-            v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0));
+    double cross_x = m_y * other.m_z - other.m_y * m_z;
+    double cross_y = other.m_x * m_z - m_x * other.m_z;
+    double cross_z = m_x * other.m_y - other.m_x * m_y;
 
-    // v = r₁v₂ + r₂v₁ + v₁ x v₂
-    final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
-
-    return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0));
+    return new Quaternion(
+        // r = r₁r₂ − v₁ ⋅ v₂
+        r1 * r2 - dot,
+        // v = r₁v₂ + r₂v₁ + v₁ x v₂
+        r1 * other.m_x + r2 * m_x + cross_x,
+        r1 * other.m_y + r2 * m_y + cross_y,
+        r1 * other.m_z + r2 * m_z + cross_z);
   }
 
   @Override
   public String toString() {
-    return String.format(
-        "Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
+    return String.format("Quaternion(%s, %s, %s, %s)", getW(), getX(), getY(), getZ());
   }
 
   /**
@@ -86,14 +143,37 @@
     if (obj instanceof Quaternion) {
       var other = (Quaternion) obj;
 
-      return Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+      return Math.abs(dot(other) - norm() * other.norm()) < 1e-9
+          && Math.abs(norm() - other.norm()) < 1e-9;
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(m_r, m_v);
+    return Objects.hash(m_w, m_x, m_y, m_z);
+  }
+
+  /**
+   * Returns the conjugate of the quaternion.
+   *
+   * @return The conjugate quaternion.
+   */
+  public Quaternion conjugate() {
+    return new Quaternion(getW(), -getX(), -getY(), -getZ());
+  }
+
+  /**
+   * Returns the elementwise product of two quaternions.
+   *
+   * @param other The other quaternion.
+   * @return The dot product of two quaternions.
+   */
+  public double dot(final Quaternion other) {
+    return getW() * other.getW()
+        + getX() * other.getX()
+        + getY() * other.getY()
+        + getZ() * other.getZ();
   }
 
   /**
@@ -102,7 +182,17 @@
    * @return The inverse quaternion.
    */
   public Quaternion inverse() {
-    return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
+    var norm = norm();
+    return conjugate().divide(norm * norm);
+  }
+
+  /**
+   * Calculates the L2 norm of the quaternion.
+   *
+   * @return The L2 norm.
+   */
+  public double norm() {
+    return Math.sqrt(dot(this));
   }
 
   /**
@@ -111,7 +201,7 @@
    * @return The normalized quaternion.
    */
   public Quaternion normalize() {
-    double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
+    double norm = norm();
     if (norm == 0.0) {
       return new Quaternion();
     } else {
@@ -120,13 +210,111 @@
   }
 
   /**
+   * Rational power of a quaternion.
+   *
+   * @param t the power to raise this quaternion to.
+   * @return The quaternion power
+   */
+  public Quaternion pow(double t) {
+    // q^t = e^(ln(q^t)) = e^(t * ln(q))
+    return this.log().times(t).exp();
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * @param adjustment the "Twist" that will be applied to this quaternion.
+   * @return The quaternion product of exp(adjustment) * this
+   */
+  public Quaternion exp(Quaternion adjustment) {
+    return adjustment.exp().times(this);
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * <p>If this quaternion is in 𝖘𝖔(3) and you are looking for an element of SO(3), use {@link
+   * fromRotationVector}
+   *
+   * @return The Matrix exponential of this quaternion.
+   */
+  public Quaternion exp() {
+    var scalar = Math.exp(getW());
+
+    var axial_magnitude = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+    var cosine = Math.cos(axial_magnitude);
+
+    double axial_scalar;
+
+    if (axial_magnitude < 1e-9) {
+      // Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(n⁶)
+      var axial_magnitude_sq = axial_magnitude * axial_magnitude;
+      var axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+      axial_scalar = 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+    } else {
+      axial_scalar = Math.sin(axial_magnitude) / axial_magnitude;
+    }
+
+    return new Quaternion(
+        cosine * scalar,
+        getX() * axial_scalar * scalar,
+        getY() * axial_scalar * scalar,
+        getZ() * axial_scalar * scalar);
+  }
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * @param end The quaternion to map this quaternion onto.
+   * @return The "Twist" that maps this quaternion to the argument.
+   */
+  public Quaternion log(Quaternion end) {
+    return end.times(this.inverse()).log();
+  }
+
+  /**
+   * The Log operator of a general quaternion.
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * <p>If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), use {@link
+   * toRotationVector}
+   *
+   * @return The logarithm of this quaternion.
+   */
+  public Quaternion log() {
+    var scalar = Math.log(norm());
+
+    var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+
+    var s_norm = getW() / norm();
+
+    if (Math.abs(s_norm + 1) < 1e-9) {
+      return new Quaternion(scalar, -Math.PI, 0, 0);
+    }
+
+    double v_scalar;
+
+    if (v_norm < 1e-9) {
+      // Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴)
+      v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
+    } else {
+      v_scalar = Math.atan2(v_norm, getW()) / v_norm;
+    }
+
+    return new Quaternion(scalar, v_scalar * getX(), v_scalar * getY(), v_scalar * getZ());
+  }
+
+  /**
    * Returns W component of the quaternion.
    *
    * @return W component of the quaternion.
    */
   @JsonProperty(value = "W")
   public double getW() {
-    return m_r;
+    return m_w;
   }
 
   /**
@@ -136,7 +324,7 @@
    */
   @JsonProperty(value = "X")
   public double getX() {
-    return m_v.get(0, 0);
+    return m_x;
   }
 
   /**
@@ -146,7 +334,7 @@
    */
   @JsonProperty(value = "Y")
   public double getY() {
-    return m_v.get(1, 0);
+    return m_y;
   }
 
   /**
@@ -156,7 +344,38 @@
    */
   @JsonProperty(value = "Z")
   public double getZ() {
-    return m_v.get(2, 0);
+    return m_z;
+  }
+
+  /**
+   * Returns the quaternion representation of this rotation vector.
+   *
+   * <p>This is also the exp operator of 𝖘𝖔(3).
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * @param rvec The rotation vector.
+   * @return The quaternion representation of this rotation vector.
+   */
+  public static Quaternion fromRotationVector(Vector<N3> rvec) {
+    double theta = rvec.norm();
+
+    double cos = Math.cos(theta / 2);
+
+    double axial_scalar;
+
+    if (theta < 1e-9) {
+      // taylor series expansion of sin(θ/2) / θ = 1/2 - θ²/48 + O(θ⁴)
+      axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+    } else {
+      axial_scalar = Math.sin(theta / 2) / theta;
+    }
+
+    return new Quaternion(
+        cos,
+        axial_scalar * rvec.get(0, 0),
+        axial_scalar * rvec.get(1, 0),
+        axial_scalar * rvec.get(2, 0));
   }
 
   /**
@@ -171,16 +390,89 @@
     // Sound State Representation through Encapsulation of Manifolds"
     //
     // https://arxiv.org/pdf/1107.1119.pdf
-    double norm = m_v.norm();
+    double norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
 
+    double coeff;
     if (norm < 1e-9) {
-      return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
+      coeff = 2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW());
     } else {
       if (getW() < 0.0) {
-        return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
+        coeff = 2.0 * Math.atan2(-norm, -getW()) / norm;
       } else {
-        return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
+        coeff = 2.0 * Math.atan2(norm, getW()) / norm;
       }
     }
+
+    return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ());
   }
+
+  public static final class AStruct implements Struct<Quaternion> {
+    @Override
+    public Class<Quaternion> getTypeClass() {
+      return Quaternion.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Quaternion";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 4;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double w;double x;double y;double z";
+    }
+
+    @Override
+    public Quaternion unpack(ByteBuffer bb) {
+      double w = bb.getDouble();
+      double x = bb.getDouble();
+      double y = bb.getDouble();
+      double z = bb.getDouble();
+      return new Quaternion(w, x, y, z);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Quaternion value) {
+      bb.putDouble(value.getW());
+      bb.putDouble(value.getX());
+      bb.putDouble(value.getY());
+      bb.putDouble(value.getZ());
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Quaternion, ProtobufQuaternion> {
+    @Override
+    public Class<Quaternion> getTypeClass() {
+      return Quaternion.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufQuaternion.getDescriptor();
+    }
+
+    @Override
+    public ProtobufQuaternion createMessage() {
+      return ProtobufQuaternion.newInstance();
+    }
+
+    @Override
+    public Quaternion unpack(ProtobufQuaternion msg) {
+      return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ());
+    }
+
+    @Override
+    public void pack(ProtobufQuaternion msg, Quaternion value) {
+      msg.setW(value.getW()).setX(value.getX()).setY(value.getY()).setZ(value.getZ());
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 5be6156..0556669 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -10,8 +10,13 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
@@ -256,4 +261,67 @@
   public Rotation2d interpolate(Rotation2d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
+
+  public static final class AStruct implements Struct<Rotation2d> {
+    @Override
+    public Class<Rotation2d> getTypeClass() {
+      return Rotation2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Rotation2d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double value";
+    }
+
+    @Override
+    public Rotation2d unpack(ByteBuffer bb) {
+      return new Rotation2d(bb.getDouble());
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Rotation2d value) {
+      bb.putDouble(value.m_value);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Rotation2d, ProtobufRotation2d> {
+    @Override
+    public Class<Rotation2d> getTypeClass() {
+      return Rotation2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufRotation2d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufRotation2d createMessage() {
+      return ProtobufRotation2d.newInstance();
+    }
+
+    @Override
+    public Rotation2d unpack(ProtobufRotation2d msg) {
+      return new Rotation2d(msg.getValue());
+    }
+
+    @Override
+    public void pack(ProtobufRotation2d msg, Rotation2d value) {
+      msg.setValue(value.m_value);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
index 3226e31..b434523 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -17,17 +17,24 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
 import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** A rotation in a 3D coordinate frame represented by a quaternion. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d implements Interpolatable<Rotation3d> {
-  private Quaternion m_q = new Quaternion();
+  private final Quaternion m_q;
 
   /** Constructs a Rotation3d with a default angle of 0 degrees. */
-  public Rotation3d() {}
+  public Rotation3d() {
+    m_q = new Quaternion();
+  }
 
   /**
    * Constructs a Rotation3d from a quaternion.
@@ -73,6 +80,17 @@
   }
 
   /**
+   * Constructs a Rotation3d with the given rotation vector representation. This representation is
+   * equivalent to axis-angle, where the normalized axis is multiplied by the rotation around the
+   * axis in radians.
+   *
+   * @param rvec The rotation vector.
+   */
+  public Rotation3d(Vector<N3> rvec) {
+    this(rvec, rvec.norm());
+  }
+
+  /**
    * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
    * normalized.
    *
@@ -82,6 +100,7 @@
   public Rotation3d(Vector<N3> axis, double angleRadians) {
     double norm = axis.norm();
     if (norm == 0.0) {
+      m_q = new Quaternion();
       return;
     }
 
@@ -175,6 +194,7 @@
     if (dotNorm > 1.0 - 1E-9) {
       // If the dot product is 1, the two vectors point in the same direction so
       // there's no rotation. The default initialization of m_q will work.
+      m_q = new Quaternion();
       return;
     } else if (dotNorm < -1.0 + 1E-9) {
       // If the dot product is -1, the two vectors point in opposite directions
@@ -267,9 +287,14 @@
   }
 
   /**
-   * Adds the new rotation to the current rotation.
+   * Adds the new rotation to the current rotation. The other rotation is applied extrinsically,
+   * which means that it rotates around the global axes. For example, {@code new
+   * Rotation3d(Units.degreesToRadians(90), 0, 0).rotateBy(new Rotation3d(0,
+   * Units.degreesToRadians(45), 0))} rotates by 90 degrees around the +X axis and then by 45
+   * degrees around the global +Y axis. (This is equivalent to {@code new
+   * Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(45), 0)})
    *
-   * @param other The rotation to rotate by.
+   * @param other The extrinsic rotation to rotate by.
    * @return The new rotated Rotation3d.
    */
   public Rotation3d rotateBy(Rotation3d other) {
@@ -297,8 +322,15 @@
     final var y = m_q.getY();
     final var z = m_q.getZ();
 
-    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
-    return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
+    // wpimath/algorithms.md
+    final var cxcy = 1.0 - 2.0 * (x * x + y * y);
+    final var sxcy = 2.0 * (w * x + y * z);
+    final var cy_sq = cxcy * cxcy + sxcy * sxcy;
+    if (cy_sq > 1e-20) {
+      return Math.atan2(sxcy, cxcy);
+    } else {
+      return 0.0;
+    }
   }
 
   /**
@@ -312,7 +344,7 @@
     final var y = m_q.getY();
     final var z = m_q.getZ();
 
-    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
     double ratio = 2.0 * (w * y - z * x);
     if (Math.abs(ratio) >= 1.0) {
       return Math.copySign(Math.PI / 2.0, ratio);
@@ -332,8 +364,15 @@
     final var y = m_q.getY();
     final var z = m_q.getZ();
 
-    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
-    return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
+    // wpimath/algorithms.md
+    final var cycz = 1.0 - 2.0 * (y * y + z * z);
+    final var cysz = 2.0 * (w * z + x * y);
+    final var cy_sq = cycz * cycz + cysz * cysz;
+    if (cy_sq > 1e-20) {
+      return Math.atan2(cysz, cycz);
+    } else {
+      return Math.atan2(2.0 * w * z, w * w - z * z);
+    }
   }
 
   /**
@@ -386,7 +425,7 @@
   public boolean equals(Object obj) {
     if (obj instanceof Rotation3d) {
       var other = (Rotation3d) obj;
-      return m_q.equals(other.m_q);
+      return Math.abs(Math.abs(m_q.dot(other.m_q)) - m_q.norm() * other.m_q.norm()) < 1e-9;
     }
     return false;
   }
@@ -400,4 +439,77 @@
   public Rotation3d interpolate(Rotation3d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
+
+  public static final class AStruct implements Struct<Rotation3d> {
+    @Override
+    public Class<Rotation3d> getTypeClass() {
+      return Rotation3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Rotation3d";
+    }
+
+    @Override
+    public int getSize() {
+      return Quaternion.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Quaternion q";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Quaternion.struct};
+    }
+
+    @Override
+    public Rotation3d unpack(ByteBuffer bb) {
+      return new Rotation3d(Quaternion.struct.unpack(bb));
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Rotation3d value) {
+      Quaternion.struct.pack(bb, value.m_q);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Rotation3d, ProtobufRotation3d> {
+    @Override
+    public Class<Rotation3d> getTypeClass() {
+      return Rotation3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufRotation3d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Quaternion.proto};
+    }
+
+    @Override
+    public ProtobufRotation3d createMessage() {
+      return ProtobufRotation3d.newInstance();
+    }
+
+    @Override
+    public Rotation3d unpack(ProtobufRotation3d msg) {
+      return new Rotation3d(Quaternion.proto.unpack(msg.getQ()));
+    }
+
+    @Override
+    public void pack(ProtobufRotation3d msg, Rotation3d value) {
+      Quaternion.proto.pack(msg.getMutableQ(), value.m_q);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
index c3c6b0c..c7959ba 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java

@@ -4,9 +4,14 @@
 
 package edu.wpi.first.math.geometry;
 
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
-/** Represents a transformation for a Pose2d. */
+/** Represents a transformation for a Pose2d in the pose's frame. */
 public class Transform2d {
   private final Translation2d m_translation;
   private final Rotation2d m_rotation;
@@ -40,6 +45,18 @@
     m_rotation = rotation;
   }
 
+  /**
+   * Constructs a transform with x and y translations instead of a separate Translation2d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
   /** Constructs the identity transform -- maps an initial pose to itself. */
   public Transform2d() {
     m_translation = new Translation2d();
@@ -67,7 +84,8 @@
   }
 
   /**
-   * Composes two transformations.
+   * Composes two transformations. The second transform is applied relative to the orientation of
+   * the first.
    *
    * @param other The transform to compose with this one.
    * @return The composition of the two transformations.
@@ -150,4 +168,83 @@
   public int hashCode() {
     return Objects.hash(m_translation, m_rotation);
   }
+
+  public static final class AStruct implements Struct<Transform2d> {
+    @Override
+    public Class<Transform2d> getTypeClass() {
+      return Transform2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Transform2d";
+    }
+
+    @Override
+    public int getSize() {
+      return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Translation2d translation;Rotation2d rotation";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+    }
+
+    @Override
+    public Transform2d unpack(ByteBuffer bb) {
+      Translation2d translation = Translation2d.struct.unpack(bb);
+      Rotation2d rotation = Rotation2d.struct.unpack(bb);
+      return new Transform2d(translation, rotation);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Transform2d value) {
+      Translation2d.struct.pack(bb, value.m_translation);
+      Rotation2d.struct.pack(bb, value.m_rotation);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Transform2d, ProtobufTransform2d> {
+    @Override
+    public Class<Transform2d> getTypeClass() {
+      return Transform2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTransform2d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
+    }
+
+    @Override
+    public ProtobufTransform2d createMessage() {
+      return ProtobufTransform2d.newInstance();
+    }
+
+    @Override
+    public Transform2d unpack(ProtobufTransform2d msg) {
+      return new Transform2d(
+          Translation2d.proto.unpack(msg.getTranslation()),
+          Rotation2d.proto.unpack(msg.getRotation()));
+    }
+
+    @Override
+    public void pack(ProtobufTransform2d msg, Transform2d value) {
+      Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+      Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
index 4920ef6..223a14b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java

@@ -4,9 +4,14 @@
 
 package edu.wpi.first.math.geometry;
 
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
-/** Represents a transformation for a Pose3d. */
+/** Represents a transformation for a Pose3d in the pose's frame. */
 public class Transform3d {
   private final Translation3d m_translation;
   private final Rotation3d m_rotation;
@@ -40,6 +45,19 @@
     m_rotation = rotation;
   }
 
+  /**
+   * Constructs a transform with x, y, and z translations instead of a separate Translation3d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param z The z component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform3d(double x, double y, double z, Rotation3d rotation) {
+    m_translation = new Translation3d(x, y, z);
+    m_rotation = rotation;
+  }
+
   /** Constructs the identity transform -- maps an initial pose to itself. */
   public Transform3d() {
     m_translation = new Translation3d();
@@ -67,7 +85,8 @@
   }
 
   /**
-   * Composes two transformations.
+   * Composes two transformations. The second transform is applied relative to the orientation of
+   * the first.
    *
    * @param other The transform to compose with this one.
    * @return The composition of the two transformations.
@@ -159,4 +178,83 @@
   public int hashCode() {
     return Objects.hash(m_translation, m_rotation);
   }
+
+  public static final class AStruct implements Struct<Transform3d> {
+    @Override
+    public Class<Transform3d> getTypeClass() {
+      return Transform3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Transform3d";
+    }
+
+    @Override
+    public int getSize() {
+      return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Translation3d translation;Rotation3d rotation";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+    }
+
+    @Override
+    public Transform3d unpack(ByteBuffer bb) {
+      Translation3d translation = Translation3d.struct.unpack(bb);
+      Rotation3d rotation = Rotation3d.struct.unpack(bb);
+      return new Transform3d(translation, rotation);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Transform3d value) {
+      Translation3d.struct.pack(bb, value.m_translation);
+      Rotation3d.struct.pack(bb, value.m_rotation);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Transform3d, ProtobufTransform3d> {
+    @Override
+    public Class<Transform3d> getTypeClass() {
+      return Transform3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTransform3d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
+    }
+
+    @Override
+    public ProtobufTransform3d createMessage() {
+      return ProtobufTransform3d.newInstance();
+    }
+
+    @Override
+    public Transform3d unpack(ProtobufTransform3d msg) {
+      return new Transform3d(
+          Translation3d.proto.unpack(msg.getTranslation()),
+          Rotation3d.proto.unpack(msg.getRotation()));
+    }
+
+    @Override
+    public void pack(ProtobufTransform3d msg, Transform3d value) {
+      Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+      Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 2d57edc..96e0001 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java

@@ -10,7 +10,15 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * Represents a translation in 2D space. This object can be used to represent a point or a vector.
@@ -185,6 +193,16 @@
     return new Translation2d(m_x / scalar, m_y / scalar);
   }
 
+  /**
+   * Returns the nearest Translation2d from a list of translations.
+   *
+   * @param translations The list of translations.
+   * @return The nearest Translation2d from the list.
+   */
+  public Translation2d nearest(List<Translation2d> translations) {
+    return Collections.min(translations, Comparator.comparing(this::getDistance));
+  }
+
   @Override
   public String toString() {
     return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
@@ -216,4 +234,70 @@
         MathUtil.interpolate(this.getX(), endValue.getX(), t),
         MathUtil.interpolate(this.getY(), endValue.getY(), t));
   }
+
+  public static final class AStruct implements Struct<Translation2d> {
+    @Override
+    public Class<Translation2d> getTypeClass() {
+      return Translation2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Translation2d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 2;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double x;double y";
+    }
+
+    @Override
+    public Translation2d unpack(ByteBuffer bb) {
+      double x = bb.getDouble();
+      double y = bb.getDouble();
+      return new Translation2d(x, y);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Translation2d value) {
+      bb.putDouble(value.m_x);
+      bb.putDouble(value.m_y);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Translation2d, ProtobufTranslation2d> {
+    @Override
+    public Class<Translation2d> getTypeClass() {
+      return Translation2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTranslation2d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTranslation2d createMessage() {
+      return ProtobufTranslation2d.newInstance();
+    }
+
+    @Override
+    public Translation2d unpack(ProtobufTranslation2d msg) {
+      return new Translation2d(msg.getX(), msg.getY());
+    }
+
+    @Override
+    public void pack(ProtobufTranslation2d msg, Translation2d value) {
+      msg.setX(value.m_x).setY(value.m_y);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index 810f56c..bc55f65 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -10,7 +10,12 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * Represents a translation in 3D space. This object can be used to represent a point or a vector.
@@ -231,4 +236,72 @@
         MathUtil.interpolate(this.getY(), endValue.getY(), t),
         MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
   }
+
+  public static final class AStruct implements Struct<Translation3d> {
+    @Override
+    public Class<Translation3d> getTypeClass() {
+      return Translation3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Translation3d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 3;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double x;double y;double z";
+    }
+
+    @Override
+    public Translation3d unpack(ByteBuffer bb) {
+      double x = bb.getDouble();
+      double y = bb.getDouble();
+      double z = bb.getDouble();
+      return new Translation3d(x, y, z);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Translation3d value) {
+      bb.putDouble(value.m_x);
+      bb.putDouble(value.m_y);
+      bb.putDouble(value.m_z);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Translation3d, ProtobufTranslation3d> {
+    @Override
+    public Class<Translation3d> getTypeClass() {
+      return Translation3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTranslation3d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTranslation3d createMessage() {
+      return ProtobufTranslation3d.newInstance();
+    }
+
+    @Override
+    public Translation3d unpack(ProtobufTranslation3d msg) {
+      return new Translation3d(msg.getX(), msg.getY(), msg.getZ());
+    }
+
+    @Override
+    public void pack(ProtobufTranslation3d msg, Translation3d value) {
+      msg.setX(value.m_x).setY(value.m_y).setZ(value.m_z);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
index be6831e..a4ae9f8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java

@@ -4,11 +4,16 @@
 
 package edu.wpi.first.math.geometry;
 
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A change in distance along a 2D arc since the last pose update. We can use ideas from
- * differential calculus to create new Pose2d objects from a Twist2d and vise versa.
+ * differential calculus to create new Pose2d objects from a Twist2d and vice versa.
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */
@@ -62,4 +67,72 @@
   public int hashCode() {
     return Objects.hash(dx, dy, dtheta);
   }
+
+  public static final class AStruct implements Struct<Twist2d> {
+    @Override
+    public Class<Twist2d> getTypeClass() {
+      return Twist2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Twist2d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 3;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double dx;double dy;double dtheta";
+    }
+
+    @Override
+    public Twist2d unpack(ByteBuffer bb) {
+      double dx = bb.getDouble();
+      double dy = bb.getDouble();
+      double dtheta = bb.getDouble();
+      return new Twist2d(dx, dy, dtheta);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Twist2d value) {
+      bb.putDouble(value.dx);
+      bb.putDouble(value.dy);
+      bb.putDouble(value.dtheta);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Twist2d, ProtobufTwist2d> {
+    @Override
+    public Class<Twist2d> getTypeClass() {
+      return Twist2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTwist2d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTwist2d createMessage() {
+      return ProtobufTwist2d.newInstance();
+    }
+
+    @Override
+    public Twist2d unpack(ProtobufTwist2d msg) {
+      return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta());
+    }
+
+    @Override
+    public void pack(ProtobufTwist2d msg, Twist2d value) {
+      msg.setDx(value.dx).setDy(value.dy).setDtheta(value.dtheta);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
index b78505e..d08d5cf 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java

@@ -4,11 +4,16 @@
 
 package edu.wpi.first.math.geometry;
 
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A change in distance along a 3D arc since the last pose update. We can use ideas from
- * differential calculus to create new Pose3d objects from a Twist3d and vise versa.
+ * differential calculus to create new Pose3d objects from a Twist3d and vice versa.
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */
@@ -82,4 +87,84 @@
   public int hashCode() {
     return Objects.hash(dx, dy, dz, rx, ry, rz);
   }
+
+  public static final class AStruct implements Struct<Twist3d> {
+    @Override
+    public Class<Twist3d> getTypeClass() {
+      return Twist3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Twist3d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 6;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double dx;double dy;double dz;double rx;double ry;double rz";
+    }
+
+    @Override
+    public Twist3d unpack(ByteBuffer bb) {
+      double dx = bb.getDouble();
+      double dy = bb.getDouble();
+      double dz = bb.getDouble();
+      double rx = bb.getDouble();
+      double ry = bb.getDouble();
+      double rz = bb.getDouble();
+      return new Twist3d(dx, dy, dz, rx, ry, rz);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Twist3d value) {
+      bb.putDouble(value.dx);
+      bb.putDouble(value.dy);
+      bb.putDouble(value.dz);
+      bb.putDouble(value.rx);
+      bb.putDouble(value.ry);
+      bb.putDouble(value.rz);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Twist3d, ProtobufTwist3d> {
+    @Override
+    public Class<Twist3d> getTypeClass() {
+      return Twist3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTwist3d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTwist3d createMessage() {
+      return ProtobufTwist3d.newInstance();
+    }
+
+    @Override
+    public Twist3d unpack(ProtobufTwist3d msg) {
+      return new Twist3d(
+          msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz());
+    }
+
+    @Override
+    public void pack(ProtobufTwist3d msg, Twist3d value) {
+      msg.setDx(value.dx)
+          .setDy(value.dy)
+          .setDz(value.dz)
+          .setRx(value.rx)
+          .setRy(value.ry)
+          .setRz(value.rz);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
new file mode 100644
index 0000000..bcd57a3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ */
+public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
+  public InterpolatingDoubleTreeMap() {
+    super(InverseInterpolator.forDouble(), Interpolator.forDouble());
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java
new file mode 100644
index 0000000..22eb232
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java

@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import java.util.Comparator;
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ *
+ * <p>{@code K} must implement {@link Comparable}, or a {@link Comparator} on {@code K} can be
+ * provided.
+ *
+ * @param <K> The type of keys held in this map.
+ * @param <V> The type of values held in this map.
+ */
+public class InterpolatingTreeMap<K, V> {
+  private final TreeMap<K, V> m_map;
+
+  private final InverseInterpolator<K> m_inverseInterpolator;
+  private final Interpolator<V> m_interpolator;
+
+  /**
+   * Constructs an InterpolatingTreeMap.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator, Interpolator<V> interpolator) {
+    m_map = new TreeMap<>();
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+  }
+
+  /**
+   * Constructs an InterpolatingTreeMap using {@code comparator}.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   * @param comparator Comparator to use on keys.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator,
+      Interpolator<V> interpolator,
+      Comparator<K> comparator) {
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+    m_map = new TreeMap<>(comparator);
+  }
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, V value) {
+    m_map.put(key, value);
+  }
+
+  /**
+   * Returns the value associated with a given key.
+   *
+   * <p>If there's no matching key, the value returned will be an interpolation between the keys
+   * before and after the provided one, using the {@link Interpolator} and {@link
+   * InverseInterpolator} provided.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public V get(K key) {
+    V val = m_map.get(key);
+    if (val == null) {
+      K ceilingKey = m_map.ceilingKey(key);
+      K floorKey = m_map.floorKey(key);
+
+      if (ceilingKey == null && floorKey == null) {
+        return null;
+      }
+      if (ceilingKey == null) {
+        return m_map.get(floorKey);
+      }
+      if (floorKey == null) {
+        return m_map.get(ceilingKey);
+      }
+      V floor = m_map.get(floorKey);
+      V ceiling = m_map.get(ceilingKey);
+
+      return m_interpolator.interpolate(
+          floor, ceiling, m_inverseInterpolator.inverseInterpolate(floorKey, ceilingKey, key));
+    } else {
+      return val;
+    }
+  }
+
+  /** Clears the contents. */
+  public void clear() {
+    m_map.clear();
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
new file mode 100644
index 0000000..be6d8a2
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An interpolation function that returns a value interpolated between an upper and lower bound.
+ * This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link Interpolator} will operate on.
+ */
+@FunctionalInterface
+public interface Interpolator<T> {
+  /**
+   * Perform interpolation between two values.
+   *
+   * @param startValue The value to start at.
+   * @param endValue The value to end at.
+   * @param t How far between the two values to interpolate. This should be bounded to [0, 1].
+   * @return The interpolated value.
+   */
+  T interpolate(T startValue, T endValue, double t);
+
+  static Interpolator<Double> forDouble() {
+    return MathUtil::interpolate;
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
new file mode 100644
index 0000000..8278af3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An inverse interpolation function which determines where within an interpolation range an object
+ * lies. This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link InverseInterpolator} will operate on.
+ */
+@FunctionalInterface
+public interface InverseInterpolator<T> {
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  double inverseInterpolate(T startValue, T endValue, T q);
+
+  static InverseInterpolator<Double> forDouble() {
+    return MathUtil::inverseInterpolate;
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
index 7e0712d..f9f20c3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java

@@ -19,11 +19,10 @@
  */
 public final class TimeInterpolatableBuffer<T> {
   private final double m_historySize;
-  private final InterpolateFunction<T> m_interpolatingFunc;
+  private final Interpolator<T> m_interpolatingFunc;
   private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
 
-  private TimeInterpolatableBuffer(
-      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+  private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySizeSeconds) {
     this.m_historySize = historySizeSeconds;
     this.m_interpolatingFunc = interpolateFunction;
   }
@@ -37,7 +36,7 @@
    * @return The new TimeInterpolatableBuffer.
    */
   public static <T> TimeInterpolatableBuffer<T> createBuffer(
-      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+      Interpolator<T> interpolateFunction, double historySizeSeconds) {
     return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
   }
 
@@ -143,17 +142,4 @@
   public NavigableMap<Double, T> getInternalBuffer() {
     return m_pastSnapshots;
   }
-
-  public interface InterpolateFunction<T> {
-    /**
-     * Return the interpolated value. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param start The lower bound, or start.
-     * @param end The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    T interpolate(T start, T end, double t);
-  }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
index 6c98337..ffbce71 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java

@@ -4,23 +4,24 @@
 
 package edu.wpi.first.math.kinematics;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 
 /**
- * Represents the speed of a robot chassis. Although this struct contains similar members compared
- * to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
- * w.r.t to the robot frame of reference, this ChassisSpeeds struct represents a velocity w.r.t to
- * the robot frame of reference.
+ * Represents the speed of a robot chassis. Although this class contains similar members compared to
+ * a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
+ * w.r.t to the robot frame of reference, a ChassisSpeeds object represents a robot's velocity.
  *
  * <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
  * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
  * will often have all three components.
  */
 public class ChassisSpeeds {
-  /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
+  /** Velocity along the x-axis. (Fwd is +) */
   public double vxMetersPerSecond;
 
-  /** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
+  /** Velocity along the y-axis. (Left is +) */
   public double vyMetersPerSecond;
 
   /** Represents the angular velocity of the robot frame. (CCW is +) */
@@ -44,6 +45,60 @@
   }
 
   /**
+   * Discretizes a continuous-time chassis speed.
+   *
+   * <p>This function converts a continuous-time chassis speed into a discrete-time one such that
+   * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
+   * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
+   * along the y-axis, and omega * dt around the z-axis).
+   *
+   * <p>This is useful for compensating for translational skew when translating and rotating a
+   * swerve drivetrain.
+   *
+   * @param vxMetersPerSecond Forward velocity.
+   * @param vyMetersPerSecond Sideways velocity.
+   * @param omegaRadiansPerSecond Angular velocity.
+   * @param dtSeconds The duration of the timestep the speeds should be applied for.
+   * @return Discretized ChassisSpeeds.
+   */
+  public static ChassisSpeeds discretize(
+      double vxMetersPerSecond,
+      double vyMetersPerSecond,
+      double omegaRadiansPerSecond,
+      double dtSeconds) {
+    var desiredDeltaPose =
+        new Pose2d(
+            vxMetersPerSecond * dtSeconds,
+            vyMetersPerSecond * dtSeconds,
+            new Rotation2d(omegaRadiansPerSecond * dtSeconds));
+    var twist = new Pose2d().log(desiredDeltaPose);
+    return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
+  }
+
+  /**
+   * Discretizes a continuous-time chassis speed.
+   *
+   * <p>This function converts a continuous-time chassis speed into a discrete-time one such that
+   * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
+   * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
+   * along the y-axis, and omega * dt around the z-axis).
+   *
+   * <p>This is useful for compensating for translational skew when translating and rotating a
+   * swerve drivetrain.
+   *
+   * @param continuousSpeeds The continuous speeds.
+   * @param dtSeconds The duration of the timestep the speeds should be applied for.
+   * @return Discretized ChassisSpeeds.
+   */
+  public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
+    return discretize(
+        continuousSpeeds.vxMetersPerSecond,
+        continuousSpeeds.vyMetersPerSecond,
+        continuousSpeeds.omegaRadiansPerSecond,
+        dtSeconds);
+  }
+
+  /**
    * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
    * object.
    *
@@ -62,10 +117,10 @@
       double vyMetersPerSecond,
       double omegaRadiansPerSecond,
       Rotation2d robotAngle) {
-    return new ChassisSpeeds(
-        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
-        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
-        omegaRadiansPerSecond);
+    // CW rotation into chassis frame
+    var rotated =
+        new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
+    return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
   }
 
   /**
@@ -89,6 +144,119 @@
         robotAngle);
   }
 
+  /**
+   * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
+   * object.
+   *
+   * @param vxMetersPerSecond The component of speed in the x direction relative to the robot.
+   *     Positive x is towards the robot's front.
+   * @param vyMetersPerSecond The component of speed in the y direction relative to the robot.
+   *     Positive y is towards the robot's left.
+   * @param omegaRadiansPerSecond The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
+   *     considered to be zero when it is facing directly away from your alliance station wall.
+   *     Remember that this should be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
+   */
+  public static ChassisSpeeds fromRobotRelativeSpeeds(
+      double vxMetersPerSecond,
+      double vyMetersPerSecond,
+      double omegaRadiansPerSecond,
+      Rotation2d robotAngle) {
+    // CCW rotation out of chassis frame
+    var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
+    return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
+  }
+
+  /**
+   * Converts a user provided robot-relative ChassisSpeeds object into a field-relative
+   * ChassisSpeeds object.
+   *
+   * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds in the robot frame
+   *     of reference. Positive x is towards the robot's front. Positive y is towards the robot's
+   *     left.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
+   *     considered to be zero when it is facing directly away from your alliance station wall.
+   *     Remember that this should be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
+   */
+  public static ChassisSpeeds fromRobotRelativeSpeeds(
+      ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
+    return fromRobotRelativeSpeeds(
+        robotRelativeSpeeds.vxMetersPerSecond,
+        robotRelativeSpeeds.vyMetersPerSecond,
+        robotRelativeSpeeds.omegaRadiansPerSecond,
+        robotAngle);
+  }
+
+  /**
+   * Adds two ChassisSpeeds and returns the sum.
+   *
+   * <p>For example, ChassisSpeeds{1.0, 0.5, 0.75} + ChassisSpeeds{2.0, 1.5, 0.25} =
+   * ChassisSpeeds{3.0, 2.0, 1.0}
+   *
+   * @param other The ChassisSpeeds to add.
+   * @return The sum of the ChassisSpeeds.
+   */
+  public ChassisSpeeds plus(ChassisSpeeds other) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond + other.vxMetersPerSecond,
+        vyMetersPerSecond + other.vyMetersPerSecond,
+        omegaRadiansPerSecond + other.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and returns the difference.
+   *
+   * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0} =
+   * ChassisSpeeds{4.0, 2.0, 1.0}
+   *
+   * @param other The ChassisSpeeds to subtract.
+   * @return The difference between the two ChassisSpeeds.
+   */
+  public ChassisSpeeds minus(ChassisSpeeds other) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond - other.vxMetersPerSecond,
+        vyMetersPerSecond - other.vyMetersPerSecond,
+        omegaRadiansPerSecond - other.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current ChassisSpeeds. This is equivalent to negating all components
+   * of the ChassisSpeeds.
+   *
+   * @return The inverse of the current ChassisSpeeds.
+   */
+  public ChassisSpeeds unaryMinus() {
+    return new ChassisSpeeds(-vxMetersPerSecond, -vyMetersPerSecond, -omegaRadiansPerSecond);
+  }
+
+  /**
+   * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2 = ChassisSpeeds{4.0, 5.0, 1.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled ChassisSpeeds.
+   */
+  public ChassisSpeeds times(double scalar) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond * scalar, vyMetersPerSecond * scalar, omegaRadiansPerSecond * scalar);
+  }
+
+  /**
+   * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2 = ChassisSpeeds{1.0, 1.25, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled ChassisSpeeds.
+   */
+  public ChassisSpeeds div(double scalar) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond / scalar, vyMetersPerSecond / scalar, omegaRadiansPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
index 0dfb016..a1fb2db 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java

@@ -16,7 +16,8 @@
  * whereas forward kinematics converts left and right component velocities into a linear and angular
  * chassis speed.
  */
-public class DifferentialDriveKinematics {
+public class DifferentialDriveKinematics
+    implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> {
   public final double trackWidthMeters;
 
   /**
@@ -37,6 +38,7 @@
    * @param wheelSpeeds The left and right velocities.
    * @return The chassis speed.
    */
+  @Override
   public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
     return new ChassisSpeeds(
         (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
@@ -51,6 +53,7 @@
    *     chassis' speed.
    * @return The left and right velocities.
    */
+  @Override
   public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
     return new DifferentialDriveWheelSpeeds(
         chassisSpeeds.vxMetersPerSecond
@@ -59,6 +62,12 @@
             + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
   }
 
+  @Override
+  public Twist2d toTwist2d(
+      DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {
+    return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters);
+  }
+
   /**
    * Performs forward kinematics to return the resulting Twist2d from the given left and right side
    * distance deltas. This method is often used for odometry -- determining the robot's position on

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
index e8f97f5..db5e8a3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java

@@ -8,7 +8,6 @@
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
 
 /**
  * Class for differential drive odometry. Odometry allows you to track the robot's position on the
@@ -20,15 +19,7 @@
  * <p>It is important that you reset your encoders to zero before using this class. Any subsequent
  * pose resets also require the encoders to be reset to zero.
  */
-public class DifferentialDriveOdometry {
-  private Pose2d m_poseMeters;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  private double m_prevLeftDistance;
-  private double m_prevRightDistance;
-
+public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPositions> {
   /**
    * Constructs a DifferentialDriveOdometry object.
    *
@@ -42,13 +33,11 @@
       double leftDistanceMeters,
       double rightDistanceMeters,
       Pose2d initialPoseMeters) {
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-
+    super(
+        new DifferentialDriveKinematics(1),
+        gyroAngle,
+        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
+        initialPoseMeters);
     MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
   }
 
@@ -80,21 +69,10 @@
       double leftDistanceMeters,
       double rightDistanceMeters,
       Pose2d poseMeters) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
+    super.resetPosition(
+        gyroAngle,
+        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
+        poseMeters);
   }
 
   /**
@@ -109,22 +87,7 @@
    */
   public Pose2d update(
       Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
-    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
-    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-
-    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var newPose =
-        m_poseMeters.exp(
-            new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    return m_poseMeters;
+    return super.update(
+        gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
new file mode 100644
index 0000000..18866af
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.Objects;
+
+public class DifferentialDriveWheelPositions
+    implements WheelPositions<DifferentialDriveWheelPositions> {
+  /** Distance measured by the left side. */
+  public double leftMeters;
+
+  /** Distance measured by the right side. */
+  public double rightMeters;
+
+  /**
+   * Constructs a DifferentialDriveWheelPositions.
+   *
+   * @param leftMeters Distance measured by the left side.
+   * @param rightMeters Distance measured by the right side.
+   */
+  public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) {
+    this.leftMeters = leftMeters;
+    this.rightMeters = rightMeters;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof DifferentialDriveWheelPositions) {
+      DifferentialDriveWheelPositions other = (DifferentialDriveWheelPositions) obj;
+      return Math.abs(other.leftMeters - leftMeters) < 1E-9
+          && Math.abs(other.rightMeters - rightMeters) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(leftMeters, rightMeters);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters);
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions copy() {
+    return new DifferentialDriveWheelPositions(leftMeters, rightMeters);
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions interpolate(
+      DifferentialDriveWheelPositions endValue, double t) {
+    return new DifferentialDriveWheelPositions(
+        MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t),
+        MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t));
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
index d4b235e..ec874b6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java

@@ -46,6 +46,77 @@
     }
   }
 
+  /**
+   * Adds two DifferentialDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} + DifferentialDriveWheelSpeeds{2.0, 1.5}
+   * = DifferentialDriveWheelSpeeds{3.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to add.
+   * @return The sum of the DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond + other.leftMetersPerSecond,
+        rightMetersPerSecond + other.rightMetersPerSecond);
+  }
+
+  /**
+   * Subtracts the other DifferentialDriveWheelSpeeds from the current DifferentialDriveWheelSpeeds
+   * and returns the difference.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} - DifferentialDriveWheelSpeeds{1.0, 2.0}
+   * = DifferentialDriveWheelSpeeds{4.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to subtract.
+   * @return The difference between the two DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond - other.leftMetersPerSecond,
+        rightMetersPerSecond - other.rightMetersPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelSpeeds. This is equivalent to negating
+   * all components of the DifferentialDriveWheelSpeeds.
+   *
+   * @return The inverse of the current DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds unaryMinus() {
+    return new DifferentialDriveWheelSpeeds(-leftMetersPerSecond, -rightMetersPerSecond);
+  }
+
+  /**
+   * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2 = DifferentialDriveWheelSpeeds{4.0,
+   * 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds times(double scalar) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond * scalar, rightMetersPerSecond * scalar);
+  }
+
+  /**
+   * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2 = DifferentialDriveWheelSpeeds{1.0,
+   * 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds div(double scalar) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond / scalar, rightMetersPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java
new file mode 100644
index 0000000..35e2642
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Twist2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot
+ * code should not use this directly- Instead, use the particular type for your drivetrain (e.g.,
+ * {@link DifferentialDriveKinematics}).
+ *
+ * @param <S> The type of the wheel speeds.
+ * @param <P> The type of the wheel positions.
+ */
+public interface Kinematics<S, P> {
+  /**
+   * Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This
+   * method is often used for odometry -- determining the robot's position on the field using data
+   * from the real-world speed of each wheel on the robot.
+   *
+   * @param wheelSpeeds The speeds of the wheels.
+   * @return The chassis speed.
+   */
+  ChassisSpeeds toChassisSpeeds(S wheelSpeeds);
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
+   * method is often used to convert joystick values into wheel speeds.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
+
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the given change in wheel
+   * positions. This method is often used for odometry -- determining the robot's position on the
+   * field using changes in the distance driven by each wheel on the robot.
+   *
+   * @param start The starting distances driven by the wheels.
+   * @param end The ending distances driven by the wheels.
+   * @return The resulting Twist2d in the robot's movement.
+   */
+  Twist2d toTwist2d(P start, P end);
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
index 23204d4..76c857a 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java

@@ -30,7 +30,8 @@
  * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
  * field using encoders and a gyro.
  */
-public class MecanumDriveKinematics {
+public class MecanumDriveKinematics
+    implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
@@ -125,6 +126,7 @@
    * @param chassisSpeeds The desired chassis speed.
    * @return The wheel speeds.
    */
+  @Override
   public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
     return toWheelSpeeds(chassisSpeeds, new Translation2d());
   }
@@ -137,6 +139,7 @@
    * @param wheelSpeeds The current mecanum drive wheel speeds.
    * @return The resulting chassis speed.
    */
+  @Override
   public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
     var wheelSpeedsVector = new SimpleMatrix(4, 1);
     wheelSpeedsVector.setColumn(
@@ -154,6 +157,20 @@
         chassisSpeedsVector.get(2, 0));
   }
 
+  @Override
+  public Twist2d toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPositions end) {
+    var wheelDeltasVector = new SimpleMatrix(4, 1);
+    wheelDeltasVector.setColumn(
+        0,
+        0,
+        end.frontLeftMeters - start.frontLeftMeters,
+        end.frontRightMeters - start.frontRightMeters,
+        end.rearLeftMeters - start.rearLeftMeters,
+        end.rearRightMeters - start.rearRightMeters);
+    var twist = m_forwardKinematics.mult(wheelDeltasVector);
+    return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
+  }
+
   /**
    * Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This
    * method is often used for odometry -- determining the robot's position on the field using
@@ -172,7 +189,6 @@
         wheelDeltas.rearLeftMeters,
         wheelDeltas.rearRightMeters);
     var twist = m_forwardKinematics.mult(wheelDeltasVector);
-
     return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
   }
 

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
index 40a77e2..32bc9bf 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java

@@ -16,14 +16,7 @@
  * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
  * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
  */
-public class MecanumDriveOdometry {
-  private final MecanumDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-  private MecanumDriveWheelPositions m_previousWheelPositions;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
+public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
   /**
    * Constructs a MecanumDriveOdometry object.
    *
@@ -37,16 +30,7 @@
       Rotation2d gyroAngle,
       MecanumDriveWheelPositions wheelPositions,
       Pose2d initialPoseMeters) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
+    super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
     MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
   }
 
@@ -63,72 +47,4 @@
       MecanumDriveWheelPositions wheelPositions) {
     this(kinematics, gyroAngle, wheelPositions, new Pose2d());
   }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
-   * automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances driven by each wheel.
-   * @param poseMeters The position on the field that your robot is at.
-   */
-  public void resetPosition(
-      Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and integration of the pose
-   * over time. This method takes in an angle parameter which is used instead of the angular rate
-   * that is calculated from forward kinematics, in addition to the current distance measurement at
-   * each wheel.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances driven by each wheel.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var wheelDeltas =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters);
-
-    var twist = m_kinematics.toTwist2d(wheelDeltas);
-    twist.dtheta = angle.minus(m_previousAngle).getRadians();
-    var newPose = m_poseMeters.exp(twist);
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
-
-    return m_poseMeters;
-  }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
index 9ff3341..2b284cb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java

@@ -4,7 +4,10 @@
 
 package edu.wpi.first.math.kinematics;
 
-public class MecanumDriveWheelPositions {
+import edu.wpi.first.math.MathUtil;
+import java.util.Objects;
+
+public class MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWheelPositions> {
   /** Distance measured by the front left wheel. */
   public double frontLeftMeters;
 
@@ -40,10 +43,42 @@
   }
 
   @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof MecanumDriveWheelPositions) {
+      MecanumDriveWheelPositions other = (MecanumDriveWheelPositions) obj;
+      return Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9
+          && Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9
+          && Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9
+          && Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+  }
+
+  @Override
   public String toString() {
     return String.format(
         "MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
             + "Rear Left: %.2f m, Rear Right: %.2f m)",
         frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
   }
+
+  @Override
+  public MecanumDriveWheelPositions copy() {
+    return new MecanumDriveWheelPositions(
+        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+  }
+
+  @Override
+  public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
+    return new MecanumDriveWheelPositions(
+        MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t),
+        MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t),
+        MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t),
+        MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t));
+  }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
index 1dcfc85..63cef18 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java

@@ -73,6 +73,89 @@
     }
   }
 
+  /**
+   * Adds two MecanumDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelSpeeds{2.0, 1.5,
+   * 0.5, 1.0} = MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
+   *
+   * @param other The MecanumDriveWheelSpeeds to add.
+   * @return The sum of the MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond + other.frontLeftMetersPerSecond,
+        frontRightMetersPerSecond + other.frontRightMetersPerSecond,
+        rearLeftMetersPerSecond + other.rearLeftMetersPerSecond,
+        rearRightMetersPerSecond + other.rearRightMetersPerSecond);
+  }
+
+  /**
+   * Subtracts the other MecanumDriveWheelSpeeds from the current MecanumDriveWheelSpeeds and
+   * returns the difference.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} - MecanumDriveWheelSpeeds{1.0, 2.0,
+   * 3.0, 0.5} = MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
+   *
+   * @param other The MecanumDriveWheelSpeeds to subtract.
+   * @return The difference between the two MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond - other.frontLeftMetersPerSecond,
+        frontRightMetersPerSecond - other.frontRightMetersPerSecond,
+        rearLeftMetersPerSecond - other.rearLeftMetersPerSecond,
+        rearRightMetersPerSecond - other.rearRightMetersPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current MecanumDriveWheelSpeeds. This is equivalent to negating all
+   * components of the MecanumDriveWheelSpeeds.
+   *
+   * @return The inverse of the current MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds unaryMinus() {
+    return new MecanumDriveWheelSpeeds(
+        -frontLeftMetersPerSecond,
+        -frontRightMetersPerSecond,
+        -rearLeftMetersPerSecond,
+        -rearRightMetersPerSecond);
+  }
+
+  /**
+   * Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 = MecanumDriveWheelSpeeds{4.0,
+   * 5.0, 6.0, 7.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds times(double scalar) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond * scalar,
+        frontRightMetersPerSecond * scalar,
+        rearLeftMetersPerSecond * scalar,
+        rearRightMetersPerSecond * scalar);
+  }
+
+  /**
+   * Divides the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 = MecanumDriveWheelSpeeds{1.0,
+   * 1.25, 0.75, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds div(double scalar) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond / scalar,
+        frontRightMetersPerSecond / scalar,
+        rearLeftMetersPerSecond / scalar,
+        rearRightMetersPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
new file mode 100644
index 0000000..b2e5054
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java

@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the particular type for
+ * your drivetrain (e.g., {@link DifferentialDriveOdometry}). Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from encoders and a
+ * gyroscope.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
+ * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
+ */
+public class Odometry<T extends WheelPositions<T>> {
+  private final Kinematics<?, T> m_kinematics;
+  private Pose2d m_poseMeters;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+  private T m_previousWheelPositions;
+
+  /**
+   * Constructs an Odometry object.
+   *
+   * @param kinematics The kinematics of the drivebase.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public Odometry(
+      Kinematics<?, T> kinematics,
+      Rotation2d gyroAngle,
+      T wheelPositions,
+      Pose2d initialPoseMeters) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = m_poseMeters.getRotation();
+    m_previousWheelPositions = wheelPositions.copy();
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param poseMeters The position on the field that your robot is at.
+   */
+  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = m_poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousWheelPositions = wheelPositions.copy();
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and integration of the pose
+   * over time. This method takes in an angle parameter which is used instead of the angular rate
+   * that is calculated from forward kinematics, in addition to the current distance measurement at
+   * each wheel.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
+    twist.dtheta = angle.minus(m_previousAngle).getRadians();
+
+    var newPose = m_poseMeters.exp(twist);
+
+    m_previousWheelPositions = wheelPositions.copy();
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+    return m_poseMeters;
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
index 98df547..aa5338e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java

@@ -10,7 +10,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import java.util.Arrays;
-import java.util.Collections;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -33,32 +32,50 @@
  * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
  * field using encoders and a gyro.
  */
-public class SwerveDriveKinematics {
+public class SwerveDriveKinematics
+    implements Kinematics<SwerveDriveKinematics.SwerveDriveWheelStates, SwerveDriveWheelPositions> {
+  public static class SwerveDriveWheelStates {
+    public SwerveModuleState[] states;
+
+    /**
+     * Creates a new SwerveDriveWheelStates instance.
+     *
+     * @param states The swerve module states. This will be deeply copied.
+     */
+    public SwerveDriveWheelStates(SwerveModuleState[] states) {
+      this.states = new SwerveModuleState[states.length];
+      for (int i = 0; i < states.length; i++) {
+        this.states[i] = new SwerveModuleState(states[i].speedMetersPerSecond, states[i].angle);
+      }
+    }
+  }
+
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
   private final int m_numModules;
   private final Translation2d[] m_modules;
-  private final SwerveModuleState[] m_moduleStates;
+  private Rotation2d[] m_moduleHeadings;
   private Translation2d m_prevCoR = new Translation2d();
 
   /**
-   * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations
-   * as Translation2d objects. The order in which you pass in the wheel locations is the same order
-   * that you will receive the module states when performing inverse kinematics. It is also expected
-   * that you pass in the module states in the same order when calling the forward kinematics
-   * methods.
+   * Constructs a swerve drive kinematics object. This takes in a variable number of module
+   * locations as Translation2d objects. The order in which you pass in the module locations is the
+   * same order that you will receive the module states when performing inverse kinematics. It is
+   * also expected that you pass in the module states in the same order when calling the forward
+   * kinematics methods.
    *
-   * @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
+   * @param moduleTranslationsMeters The locations of the modules relative to the physical center of
+   *     the robot.
    */
-  public SwerveDriveKinematics(Translation2d... wheelsMeters) {
-    if (wheelsMeters.length < 2) {
+  public SwerveDriveKinematics(Translation2d... moduleTranslationsMeters) {
+    if (moduleTranslationsMeters.length < 2) {
       throw new IllegalArgumentException("A swerve drive requires at least two modules");
     }
-    m_numModules = wheelsMeters.length;
-    m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
-    m_moduleStates = new SwerveModuleState[m_numModules];
-    Arrays.fill(m_moduleStates, new SwerveModuleState());
+    m_numModules = moduleTranslationsMeters.length;
+    m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules);
+    m_moduleHeadings = new Rotation2d[m_numModules];
+    Arrays.fill(m_moduleHeadings, new Rotation2d());
     m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
 
     for (int i = 0; i < m_numModules; i++) {
@@ -71,6 +88,21 @@
   }
 
   /**
+   * Reset the internal swerve module headings.
+   *
+   * @param moduleHeadings The swerve module headings. The order of the module headings should be
+   *     same as passed into the constructor of this class.
+   */
+  public void resetHeadings(Rotation2d... moduleHeadings) {
+    if (moduleHeadings.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of headings is not consistent with number of module locations provided in "
+              + "constructor");
+    }
+    m_moduleHeadings = Arrays.copyOf(moduleHeadings, m_numModules);
+  }
+
+  /**
    * Performs inverse kinematics to return the module states from a desired chassis velocity. This
    * method is often used to convert joystick values into module speeds and angles.
    *
@@ -91,17 +123,18 @@
    *     attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
    *     DesaturateWheelSpeeds} function to rectify this issue.
    */
-  @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public SwerveModuleState[] toSwerveModuleStates(
       ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+    var moduleStates = new SwerveModuleState[m_numModules];
+
     if (chassisSpeeds.vxMetersPerSecond == 0.0
         && chassisSpeeds.vyMetersPerSecond == 0.0
         && chassisSpeeds.omegaRadiansPerSecond == 0.0) {
       for (int i = 0; i < m_numModules; i++) {
-        m_moduleStates[i].speedMetersPerSecond = 0.0;
+        moduleStates[i] = new SwerveModuleState(0.0, m_moduleHeadings[i]);
       }
 
-      return m_moduleStates;
+      return moduleStates;
     }
 
     if (!centerOfRotationMeters.equals(m_prevCoR)) {
@@ -139,10 +172,11 @@
       double speed = Math.hypot(x, y);
       Rotation2d angle = new Rotation2d(x, y);
 
-      m_moduleStates[i] = new SwerveModuleState(speed, angle);
+      moduleStates[i] = new SwerveModuleState(speed, angle);
+      m_moduleHeadings[i] = angle;
     }
 
-    return m_moduleStates;
+    return moduleStates;
   }
 
   /**
@@ -156,26 +190,31 @@
     return toSwerveModuleStates(chassisSpeeds, new Translation2d());
   }
 
+  @Override
+  public SwerveDriveWheelStates toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return new SwerveDriveWheelStates(toSwerveModuleStates(chassisSpeeds));
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the given module states.
    * This method is often used for odometry -- determining the robot's position on the field using
    * data from the real-world speed and angle of each module on the robot.
    *
-   * @param wheelStates The state of the modules (as a SwerveModuleState type) as measured from
+   * @param moduleStates The state of the modules (as a SwerveModuleState type) as measured from
    *     respective encoders and gyros. The order of the swerve module states should be same as
    *     passed into the constructor of this class.
    * @return The resulting chassis speed.
    */
-  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
-    if (wheelStates.length != m_numModules) {
+  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) {
+    if (moduleStates.length != m_numModules) {
       throw new IllegalArgumentException(
-          "Number of modules is not consistent with number of wheel locations provided in "
+          "Number of modules is not consistent with number of module locations provided in "
               + "constructor");
     }
     var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
 
     for (int i = 0; i < m_numModules; i++) {
-      var module = wheelStates[i];
+      var module = moduleStates[i];
       moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
       moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
     }
@@ -187,26 +226,31 @@
         chassisSpeedsVector.get(2, 0));
   }
 
+  @Override
+  public ChassisSpeeds toChassisSpeeds(SwerveDriveWheelStates wheelStates) {
+    return toChassisSpeeds(wheelStates.states);
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the given module states.
    * This method is often used for odometry -- determining the robot's position on the field using
    * data from the real-world speed and angle of each module on the robot.
    *
-   * @param wheelDeltas The latest change in position of the modules (as a SwerveModulePosition
+   * @param moduleDeltas The latest change in position of the modules (as a SwerveModulePosition
    *     type) as measured from respective encoders and gyros. The order of the swerve module states
    *     should be same as passed into the constructor of this class.
    * @return The resulting Twist2d.
    */
-  public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas) {
-    if (wheelDeltas.length != m_numModules) {
+  public Twist2d toTwist2d(SwerveModulePosition... moduleDeltas) {
+    if (moduleDeltas.length != m_numModules) {
       throw new IllegalArgumentException(
-          "Number of modules is not consistent with number of wheel locations provided in "
+          "Number of modules is not consistent with number of module locations provided in "
               + "constructor");
     }
     var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1);
 
     for (int i = 0; i < m_numModules; i++) {
-      var module = wheelDeltas[i];
+      var module = moduleDeltas[i];
       moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
       moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
     }
@@ -216,6 +260,22 @@
         chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
   }
 
+  @Override
+  public Twist2d toTwist2d(SwerveDriveWheelPositions start, SwerveDriveWheelPositions end) {
+    if (start.positions.length != end.positions.length) {
+      throw new IllegalArgumentException("Inconsistent number of modules!");
+    }
+    var newPositions = new SwerveModulePosition[start.positions.length];
+    for (int i = 0; i < start.positions.length; i++) {
+      var startModule = start.positions[i];
+      var endModule = end.positions[i];
+      newPositions[i] =
+          new SwerveModulePosition(
+              endModule.distanceMeters - startModule.distanceMeters, endModule.angle);
+    }
+    return toTwist2d(newPositions);
+  }
+
   /**
    * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
    *
@@ -230,7 +290,10 @@
    */
   public static void desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
-    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+    double realMaxSpeed = 0;
+    for (SwerveModuleState moduleState : moduleStates) {
+      realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
+    }
     if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
       for (SwerveModuleState moduleState : moduleStates) {
         moduleState.speedMetersPerSecond =
@@ -250,7 +313,7 @@
    *
    * @param moduleStates Reference to array of module states. The array will be mutated with the
    *     normalized speeds!
-   * @param currentChassisSpeed The current speed of the robot
+   * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach
    * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot
    *     can reach while translating
@@ -259,11 +322,14 @@
    */
   public static void desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates,
-      ChassisSpeeds currentChassisSpeed,
+      ChassisSpeeds desiredChassisSpeed,
       double attainableMaxModuleSpeedMetersPerSecond,
       double attainableMaxTranslationalSpeedMetersPerSecond,
       double attainableMaxRotationalVelocityRadiansPerSecond) {
-    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+    double realMaxSpeed = 0;
+    for (SwerveModuleState moduleState : moduleStates) {
+      realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
+    }
 
     if (attainableMaxTranslationalSpeedMetersPerSecond == 0
         || attainableMaxRotationalVelocityRadiansPerSecond == 0
@@ -271,10 +337,10 @@
       return;
     }
     double translationalK =
-        Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond)
+        Math.hypot(desiredChassisSpeed.vxMetersPerSecond, desiredChassisSpeed.vyMetersPerSecond)
             / attainableMaxTranslationalSpeedMetersPerSecond;
     double rotationalK =
-        Math.abs(currentChassisSpeed.omegaRadiansPerSecond)
+        Math.abs(desiredChassisSpeed.omegaRadiansPerSecond)
             / attainableMaxRotationalVelocityRadiansPerSecond;
     double k = Math.max(translationalK, rotationalK);
     double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
index c2e188f..4f954e9 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java

@@ -17,14 +17,8 @@
  * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
  * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
  */
-public class SwerveDriveOdometry {
-  private final SwerveDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
+public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
   private final int m_numModules;
-  private SwerveModulePosition[] m_previousModulePositions;
 
   /**
    * Constructs a SwerveDriveOdometry object.
@@ -39,18 +33,9 @@
       Rotation2d gyroAngle,
       SwerveModulePosition[] modulePositions,
       Pose2d initialPose) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPose;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPose.getRotation();
-    m_numModules = modulePositions.length;
+    super(kinematics, gyroAngle, new SwerveDriveWheelPositions(modulePositions), initialPose);
 
-    m_previousModulePositions = new SwerveModulePosition[m_numModules];
-    for (int index = 0; index < m_numModules; index++) {
-      m_previousModulePositions[index] =
-          new SwerveModulePosition(
-              modulePositions[index].distanceMeters, modulePositions[index].angle);
-    }
+    m_numModules = modulePositions.length;
 
     MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
   }
@@ -83,29 +68,18 @@
    */
   public void resetPosition(
       Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
-    if (modulePositions.length != m_numModules) {
+    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose);
+  }
+
+  @Override
+  public void resetPosition(
+      Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) {
+    if (modulePositions.positions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");
     }
-
-    m_poseMeters = pose;
-    m_previousAngle = pose.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    for (int index = 0; index < m_numModules; index++) {
-      m_previousModulePositions[index] =
-          new SwerveModulePosition(
-              modulePositions[index].distanceMeters, modulePositions[index].angle);
-    }
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
+    super.resetPosition(gyroAngle, modulePositions, pose);
   }
 
   /**
@@ -121,32 +95,16 @@
    * @return The new pose of the robot.
    */
   public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    if (modulePositions.length != m_numModules) {
+    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+  }
+
+  @Override
+  public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) {
+    if (modulePositions.positions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");
     }
-
-    var moduleDeltas = new SwerveModulePosition[m_numModules];
-    for (int index = 0; index < m_numModules; index++) {
-      var current = modulePositions[index];
-      var previous = m_previousModulePositions[index];
-
-      moduleDeltas[index] =
-          new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
-      previous.distanceMeters = current.distanceMeters;
-    }
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var twist = m_kinematics.toTwist2d(moduleDeltas);
-    twist.dtheta = angle.minus(m_previousAngle).getRadians();
-
-    var newPose = m_poseMeters.exp(twist);
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
-    return m_poseMeters;
+    return super.update(gyroAngle, modulePositions);
   }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
new file mode 100644
index 0000000..e88f044
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java

@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import java.util.Arrays;
+import java.util.Objects;
+
+public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
+  public SwerveModulePosition[] positions;
+
+  /**
+   * Creates a new SwerveDriveWheelPositions instance.
+   *
+   * @param positions The swerve module positions. This will be deeply copied.
+   */
+  public SwerveDriveWheelPositions(SwerveModulePosition[] positions) {
+    this.positions = new SwerveModulePosition[positions.length];
+    for (int i = 0; i < positions.length; i++) {
+      this.positions[i] = positions[i].copy();
+    }
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof SwerveDriveWheelPositions) {
+      SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj;
+      return Arrays.equals(this.positions, other.positions);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    // Cast to interpret positions as single argument, not array of the arguments
+    return Objects.hash((Object) positions);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions));
+  }
+
+  @Override
+  public SwerveDriveWheelPositions copy() {
+    return new SwerveDriveWheelPositions(positions);
+  }
+
+  @Override
+  public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
+    if (endValue.positions.length != positions.length) {
+      throw new IllegalArgumentException("Inconsistent number of modules!");
+    }
+    var newPositions = new SwerveModulePosition[positions.length];
+    for (int i = 0; i < positions.length; i++) {
+      newPositions[i] = positions[i].interpolate(endValue.positions[i], t);
+    }
+    return new SwerveDriveWheelPositions(newPositions);
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
index cdd7834..d058764 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java

@@ -4,11 +4,14 @@
 
 package edu.wpi.first.math.kinematics;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /** Represents the state of one swerve module. */
-public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
+public class SwerveModulePosition
+    implements Comparable<SwerveModulePosition>, Interpolatable<SwerveModulePosition> {
   /** Distance measured by the wheel of the module. */
   public double distanceMeters;
 
@@ -32,14 +35,15 @@
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModulePosition) {
-      return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+      SwerveModulePosition other = (SwerveModulePosition) obj;
+      return Math.abs(other.distanceMeters - distanceMeters) < 1E-9 && angle.equals(other.angle);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(distanceMeters);
+    return Objects.hash(distanceMeters, angle);
   }
 
   /**
@@ -59,4 +63,20 @@
     return String.format(
         "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
   }
+
+  /**
+   * Returns a copy of this swerve module position.
+   *
+   * @return A copy.
+   */
+  public SwerveModulePosition copy() {
+    return new SwerveModulePosition(distanceMeters, angle);
+  }
+
+  @Override
+  public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
+    return new SwerveModulePosition(
+        MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t),
+        this.angle.interpolate(endValue.angle, t));
+  }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
index ec7fd9f..10dee4f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java

@@ -32,15 +32,16 @@
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModuleState) {
-      return Double.compare(speedMetersPerSecond, ((SwerveModuleState) obj).speedMetersPerSecond)
-          == 0;
+      SwerveModuleState other = (SwerveModuleState) obj;
+      return Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9
+          && angle.equals(other.angle);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(speedMetersPerSecond);
+    return Objects.hash(speedMetersPerSecond, angle);
   }
 
   /**

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
new file mode 100644
index 0000000..029a0ac
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.interpolation.Interpolatable;
+
+public interface WheelPositions<T extends WheelPositions<T>> extends Interpolatable<T> {
+  /**
+   * Returns a copy of this instance.
+   *
+   * @return A copy.
+   */
+  T copy();
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
index c1a87f2..b2a34a5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java

@@ -71,8 +71,8 @@
   private SplineParameterizer() {}
 
   /**
-   * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
-   * dy, and dtheta are within specific tolerances.
+   * Parametrizes the spline. This method breaks up the spline into various arcs until their dx, dy,
+   * and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.
    * @return A list of poses and curvatures that represents various points on the spline.
@@ -84,8 +84,8 @@
   }
 
   /**
-   * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
-   * dy, and dtheta are within specific tolerances.
+   * Parametrizes the spline. This method breaks up the spline into various arcs until their dx, dy,
+   * and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.
    * @param t0 Starting internal spline parameter. It is recommended to use 0.0.

diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
index 1871803..b2fe9c8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java

@@ -109,93 +109,6 @@
   }
 
   /**
-   * Discretizes the given continuous A and Q matrices.
-   *
-   * <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive),
-   * we take advantage of the structure of the block matrix of A and Q.
-   *
-   * <ul>
-   *   <li>eᴬᵀ, which is only N x N, is relatively cheap.
-   *   <li>The upper-right quarter of the 2N x 2N matrix, which we can approximate using a taylor
-   *       series to several terms and still be substantially cheaper than taking the big
-   *       exponential.
-   * </ul>
-   *
-   * @param <States> Nat representing the number of states.
-   * @param contA Continuous system matrix.
-   * @param contQ Continuous process noise covariance matrix.
-   * @param dtSeconds Discretization timestep.
-   * @return a pair representing the discrete system matrix and process noise covariance matrix.
-   */
-  public static <States extends Num>
-      Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
-          Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
-    //       T
-    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-    //       0
-    //
-    // M = [−A  Q ]
-    //     [ 0  Aᵀ]
-    // ϕ = eᴹᵀ
-    // ϕ₁₂ = A_d⁻¹Q_d
-    //
-    // Taylor series of ϕ:
-    //
-    //   ϕ = eᴹᵀ = I + MT + 1/2 M²T² + 1/6 M³T³ + …
-    //   ϕ = eᴹᵀ = I + MT + 1/2 T²M² + 1/6 T³M³ + …
-    //
-    // Taylor series of ϕ expanded for ϕ₁₂:
-    //
-    //   ϕ₁₂ = 0 + QT + 1/2 T² (−AQ + QAᵀ) + 1/6 T³ (−A lastTerm + Q Aᵀ²) + …
-    //
-    // ```
-    // lastTerm = Q
-    // lastCoeff = T
-    // ATn = Aᵀ
-    // ϕ₁₂ = lastTerm lastCoeff = QT
-    //
-    // for i in range(2, 6):
-    //   // i = 2
-    //   lastTerm = −A lastTerm + Q ATn = −AQ + QAᵀ
-    //   lastCoeff *= T/i → lastCoeff *= T/2 = 1/2 T²
-    //   ATn *= Aᵀ = Aᵀ²
-    //
-    //   // i = 3
-    //   lastTerm = −A lastTerm + Q ATn = −A (−AQ + QAᵀ) + QAᵀ² = …
-    //   …
-    // ```
-
-    // Make continuous Q symmetric if it isn't already
-    Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
-
-    Matrix<States, States> lastTerm = Q.copy();
-    double lastCoeff = dtSeconds;
-
-    // Aᵀⁿ
-    Matrix<States, States> ATn = contA.transpose();
-
-    Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
-
-    // i = 6 i.e. 5th order should be enough precision
-    for (int i = 2; i < 6; ++i) {
-      lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(ATn));
-      lastCoeff *= dtSeconds / ((double) i);
-
-      phi12 = phi12.plus(lastTerm.times(lastCoeff));
-
-      ATn = ATn.times(contA.transpose());
-    }
-
-    var discA = discretizeA(contA, dtSeconds);
-    Q = discA.times(phi12);
-
-    // Make Q symmetric if it isn't already
-    var discQ = Q.plus(Q.transpose()).div(2.0);
-
-    return new Pair<>(discA, discQ);
-  }
-
-  /**
    * Returns a discretized version of the provided continuous measurement noise covariance matrix.
    * Note that dt=0.0 divides R by zero.
    *

diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
index 94be369..e9b46fc 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java

@@ -238,7 +238,11 @@
                     .times(h))
                 .normF();
 
-        h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        if (truncationError == 0.0) {
+          h = dtSeconds - dtElapsed;
+        } else {
+          h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        }
       } while (truncationError > maxError);
 
       dtElapsed += h;

diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
index cd431f7..0c6e334 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java

@@ -21,7 +21,7 @@
    * Constructs a DC motor.
    *
    * @param nominalVoltageVolts Voltage at which the motor constants were measured.
-   * @param stallTorqueNewtonMeters Current draw when stalled.
+   * @param stallTorqueNewtonMeters Torque when stalled.
    * @param stallCurrentAmps Current draw when stalled.
    * @param freeCurrentAmps Current draw under no load.
    * @param freeSpeedRadPerSec Angular velocity under no load.
@@ -47,10 +47,10 @@
   }
 
   /**
-   * Estimate the current being drawn by this motor.
+   * Calculate current drawn by motor with given speed and input voltage.
    *
-   * @param speedRadiansPerSec The speed of the motor.
-   * @param voltageInputVolts The input voltage.
+   * @param speedRadiansPerSec The current angular velocity of the motor.
+   * @param voltageInputVolts The voltage being applied to the motor.
    * @return The estimated current.
    */
   public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
@@ -58,20 +58,20 @@
   }
 
   /**
-   * Calculate the torque produced by the motor for a given current.
+   * Calculate torque produced by the motor with a given current.
    *
    * @param currentAmpere The current drawn by the motor.
-   * @return The torque produced.
+   * @return The torque output.
    */
   public double getTorque(double currentAmpere) {
     return currentAmpere * KtNMPerAmp;
   }
 
   /**
-   * Calculate the voltage provided to the motor at a given torque and angular velocity.
+   * Calculate the voltage provided to the motor for a given torque and angular velocity.
    *
    * @param torqueNm The torque produced by the motor.
-   * @param speedRadiansPerSec The speed of the motor.
+   * @param speedRadiansPerSec The current angular velocity of the motor.
    * @return The voltage of the motor.
    */
   public double getVoltage(double torqueNm, double speedRadiansPerSec) {
@@ -79,14 +79,15 @@
   }
 
   /**
-   * Calculate the speed of the motor at a given torque and input voltage.
+   * Calculates the angular speed produced by the motor at a given torque and input voltage.
    *
    * @param torqueNm The torque produced by the motor.
    * @param voltageInputVolts The voltage applied to the motor.
-   * @return The speed of the motor.
+   * @return The angular speed of the motor.
    */
   public double getSpeed(double torqueNm, double voltageInputVolts) {
-    return voltageInputVolts - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
+    return voltageInputVolts * KvRadPerSecPerVolt
+        - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
   }
 
   /**
@@ -227,6 +228,18 @@
   }
 
   /**
+   * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control) enabled.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Falcon 500 FOC enabled motors.
+   */
+  public static DCMotor getFalcon500Foc(int numMotors) {
+    // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/
+    return new DCMotor(
+        12, 5.84, 304, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6080.0), numMotors);
+  }
+
+  /**
    * Return a gearbox of Romi/TI_RSLK MAX motors.
    *
    * @param numMotors Number of motors in the gearbox.
@@ -237,4 +250,40 @@
     return new DCMotor(
         4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
   }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return a gearbox of Kraken X60 motors.
+   */
+  public static DCMotor getKrakenX60(int numMotors) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return new DCMotor(
+        12, 7.09, 366, 2, Units.rotationsPerMinuteToRadiansPerSecond(6000), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented Control) enabled.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Kraken X60 FOC enabled motors.
+   */
+  public static DCMotor getKrakenX60Foc(int numMotors) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return new DCMotor(
+        12, 9.37, 483, 2, Units.rotationsPerMinuteToRadiansPerSecond(5800), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Neo Vortex brushless motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return a gearbox of Neo Vortex motors.
+   */
+  public static DCMotor getNeoVortex(int numMotors) {
+    // From https://www.revrobotics.com/next-generation-spark-neo/
+    return new DCMotor(
+        12, 3.60, 211, 3.6, Units.rotationsPerMinuteToRadiansPerSecond(6784), numMotors);
+  }
 }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
index 332e690..8377022 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java

@@ -123,6 +123,39 @@
   }
 
   /**
+   * Create a state-space model of a DC motor system. The states of the system are [angular
+   * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
+   * velocity].
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
+   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
+   *
+   * <p>The parameters provided by the user are from this feedforward model:
+   *
+   * <p>u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volts/(unit/sec)
+   * @param kA The acceleration gain, in volts/(unit/sec^2)
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
+   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
+   */
+  public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) {
+    if (kV <= 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than zero.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kV / kA),
+        VecBuilder.fill(0, 1 / kA),
+        Matrix.eye(Nat.N2()),
+        new Matrix<>(Nat.N2(), Nat.N1()));
+  }
+
+  /**
    * Create a state-space model of a differential drive drivetrain. In this model, the states are
    * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
    * [left velocity, right velocity]ᵀ.

diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
new file mode 100644
index 0000000..1580e85
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java

@@ -0,0 +1,449 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import java.util.Objects;
+
+/**
+ * A exponential curve-shaped velocity profile.
+ *
+ * <p>While this class can be used for a profiled movement from start to finish, the intended usage
+ * is to filter a reference's dynamics based on state-space model dynamics. To compute the reference
+ * obeying this constraint, do the following.
+ *
+ * <p>Initialization:
+ *
+ * <pre><code>
+ * ExponentialProfile.Constraints constraints =
+ *   ExponentialProfile.Constraints.fromCharacteristics(kMaxV, kV, kA);
+ * ExponentialProfile.State previousProfiledReference =
+ *   new ExponentialProfile.State(initialReference, 0.0);
+ * ExponentialProfile profile = new ExponentialProfile(constraints);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ *
+ * <pre><code>
+ * previousProfiledReference =
+ * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
+ * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to
+ * determine when the profile has completed via `isFinished()`.
+ */
+public class ExponentialProfile {
+  private final Constraints m_constraints;
+
+  public static class ProfileTiming {
+    public final double inflectionTime;
+    public final double totalTime;
+
+    protected ProfileTiming(double inflectionTime, double totalTime) {
+      this.inflectionTime = inflectionTime;
+      this.totalTime = totalTime;
+    }
+
+    /**
+     * Decides if the profile is finished by time t.
+     *
+     * @param t The time since the beginning of the profile.
+     * @return if the profile is finished at time t.
+     */
+    public boolean isFinished(double t) {
+      return t > inflectionTime;
+    }
+  }
+
+  public static class Constraints {
+    public final double maxInput;
+
+    public final double A;
+    public final double B;
+
+    /**
+     * Construct constraints for an ExponentialProfile.
+     *
+     * @param maxInput maximum unsigned input voltage
+     * @param A The State-Space 1x1 system matrix.
+     * @param B The State-Space 1x1 input matrix.
+     */
+    private Constraints(double maxInput, double A, double B) {
+      this.maxInput = maxInput;
+      this.A = A;
+      this.B = B;
+    }
+
+    /**
+     * Computes the max achievable velocity for an Exponential Profile.
+     *
+     * @return The seady-state velocity achieved by this profile.
+     */
+    public double maxVelocity() {
+      return -maxInput * B / A;
+    }
+
+    /**
+     * Construct constraints for an ExponentialProfile from characteristics.
+     *
+     * @param maxInput maximum unsigned input voltage
+     * @param kV The velocity gain.
+     * @param kA The acceleration gain.
+     * @return The Constraints object.
+     */
+    public static Constraints fromCharacteristics(double maxInput, double kV, double kA) {
+      return new Constraints(maxInput, -kV / kA, 1.0 / kA);
+    }
+
+    /**
+     * Construct constraints for an ExponentialProfile from State-Space parameters.
+     *
+     * @param maxInput maximum unsigned input voltage
+     * @param A The State-Space 1x1 system matrix.
+     * @param B The State-Space 1x1 input matrix.
+     * @return The Constraints object.
+     */
+    public static Constraints fromStateSpace(double maxInput, double A, double B) {
+      return new Constraints(maxInput, A, B);
+    }
+  }
+
+  public static class State {
+    public final double position;
+
+    public final double velocity;
+
+    /**
+     * Construct a state within an exponential profile.
+     *
+     * @param position The position at this state.
+     * @param velocity The velocity at this state.
+     */
+    public State(double position, double velocity) {
+      this.position = position;
+      this.velocity = velocity;
+    }
+
+    @Override
+    public boolean equals(Object other) {
+      if (other instanceof State) {
+        State rhs = (State) other;
+        return this.position == rhs.position && this.velocity == rhs.velocity;
+      } else {
+        return false;
+      }
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(position, velocity);
+    }
+  }
+
+  /**
+   * Construct an ExponentialProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum input.
+   */
+  public ExponentialProfile(Constraints constraints) {
+    m_constraints = constraints;
+  }
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t where the current state
+   * is at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @return The position and velocity of the profile at time t.
+   */
+  public State calculate(double t, State current, State goal) {
+    var direction = shouldFlipInput(current, goal) ? -1 : 1;
+    var u = direction * m_constraints.maxInput;
+
+    var inflectionPoint = calculateInflectionPoint(current, goal, u);
+    var timing = calculateProfileTiming(current, inflectionPoint, goal, u);
+
+    if (t < 0) {
+      return current;
+    } else if (t < timing.inflectionTime) {
+      return new State(
+          computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current));
+    } else if (t < timing.totalTime) {
+      return new State(
+          computeDistanceFromTime(t - timing.totalTime, -u, goal),
+          computeVelocityFromTime(t - timing.totalTime, -u, goal));
+    } else {
+      return goal;
+    }
+  }
+
+  /**
+   * Calculate the point after which the fastest way to reach the goal state is to apply input in
+   * the opposite direction.
+   *
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @return The position and velocity of the profile at the inflection point.
+   */
+  public State calculateInflectionPoint(State current, State goal) {
+    var direction = shouldFlipInput(current, goal) ? -1 : 1;
+    var u = direction * m_constraints.maxInput;
+
+    return calculateInflectionPoint(current, goal, u);
+  }
+
+  /**
+   * Calculate the point after which the fastest way to reach the goal state is to apply input in
+   * the opposite direction.
+   *
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @param input The signed input applied to this profile from the current state.
+   * @return The position and velocity of the profile at the inflection point.
+   */
+  private State calculateInflectionPoint(State current, State goal, double input) {
+    var u = input;
+
+    if (current.equals(goal)) {
+      return current;
+    }
+
+    var inflectionVelocity = solveForInflectionVelocity(u, current, goal);
+    var inflectionPosition = computeDistanceFromVelocity(inflectionVelocity, -u, goal);
+
+    return new State(inflectionPosition, inflectionVelocity);
+  }
+
+  /**
+   * Calculate the time it will take for this profile to reach the goal state.
+   *
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @return The total duration of this profile.
+   */
+  public double timeLeftUntil(State current, State goal) {
+    var timing = calculateProfileTiming(current, goal);
+
+    return timing.totalTime;
+  }
+
+  /**
+   * Calculate the time it will take for this profile to reach the inflection point, and the time it
+   * will take for this profile to reach the goal state.
+   *
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @return The timing information for this profile.
+   */
+  public ProfileTiming calculateProfileTiming(State current, State goal) {
+    var direction = shouldFlipInput(current, goal) ? -1 : 1;
+    var u = direction * m_constraints.maxInput;
+
+    var inflectionPoint = calculateInflectionPoint(current, goal, u);
+    return calculateProfileTiming(current, inflectionPoint, goal, u);
+  }
+
+  /**
+   * Calculate the time it will take for this profile to reach the inflection point, and the time it
+   * will take for this profile to reach the goal state.
+   *
+   * @param current The current state.
+   * @param inflectionPoint The inflection point of this profile.
+   * @param goal The desired state when the profile is complete.
+   * @param input The signed input applied to this profile from the current state.
+   * @return The timing information for this profile.
+   */
+  private ProfileTiming calculateProfileTiming(
+      State current, State inflectionPoint, State goal, double input) {
+    var u = input;
+
+    double inflectionT_forward;
+
+    // We need to handle 5 cases here:
+    //
+    // - Approaching -maxVelocity from below
+    // - Approaching -maxVelocity from above
+    // - Approaching maxVelocity from below
+    // - Approaching maxVelocity from above
+    // - At +-maxVelocity
+    //
+    // For cases 1 and 3, we want to subtract epsilon from the inflection point velocity.
+    // For cases 2 and 4, we want to add epsilon to the inflection point velocity.
+    // For case 5, we have reached inflection point velocity.
+    double epsilon = 1e-9;
+    if (Math.abs(Math.signum(input) * m_constraints.maxVelocity() - inflectionPoint.velocity)
+        < epsilon) {
+      double solvableV = inflectionPoint.velocity;
+      double t_to_solvable_v;
+      double x_at_solvable_v;
+      if (Math.abs(current.velocity - inflectionPoint.velocity) < epsilon) {
+        t_to_solvable_v = 0;
+        x_at_solvable_v = current.position;
+      } else {
+        if (Math.abs(current.velocity) > m_constraints.maxVelocity()) {
+          solvableV += Math.signum(u) * epsilon;
+        } else {
+          solvableV -= Math.signum(u) * epsilon;
+        }
+
+        t_to_solvable_v = computeTimeFromVelocity(solvableV, u, current.velocity);
+        x_at_solvable_v = computeDistanceFromVelocity(solvableV, u, current);
+      }
+
+      inflectionT_forward =
+          t_to_solvable_v
+              + Math.signum(input)
+                  * (inflectionPoint.position - x_at_solvable_v)
+                  / m_constraints.maxVelocity();
+    } else {
+      inflectionT_forward = computeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
+    }
+
+    var inflectionT_backward = computeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
+
+    return new ProfileTiming(inflectionT_forward, inflectionT_forward - inflectionT_backward);
+  }
+
+  /**
+   * Calculate the position reached after t seconds when applying an input from the initial state.
+   *
+   * @param t The time since the initial state.
+   * @param input The signed input applied to this profile from the initial state.
+   * @param initial The initial state.
+   * @return The distance travelled by this profile.
+   */
+  private double computeDistanceFromTime(double t, double input, State initial) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    return initial.position
+        + (-B * u * t + (initial.velocity + B * u / A) * (Math.exp(A * t) - 1)) / A;
+  }
+
+  /**
+   * Calculate the velocity reached after t seconds when applying an input from the initial state.
+   *
+   * @param t The time since the initial state.
+   * @param input The signed input applied to this profile from the initial state.
+   * @param initial The initial state.
+   * @return The distance travelled by this profile.
+   */
+  private double computeVelocityFromTime(double t, double input, State initial) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    return (initial.velocity + B * u / A) * Math.exp(A * t) - B * u / A;
+  }
+
+  /**
+   * Calculate the time required to reach a specified velocity given the initial velocity.
+   *
+   * @param velocity The goal velocity.
+   * @param input The signed input applied to this profile from the initial state.
+   * @param initial The initial velocity.
+   * @return The time required to reach the goal velocity.
+   */
+  private double computeTimeFromVelocity(double velocity, double input, double initial) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    return Math.log((A * velocity + B * u) / (A * initial + B * u)) / A;
+  }
+
+  /**
+   * Calculate the distance reached at the same time as the given velocity when applying the given
+   * input from the initial state.
+   *
+   * @param velocity The velocity reached by this profile
+   * @param input The signed input applied to this profile from the initial state.
+   * @param initial The initial state.
+   * @return The distance reached when the given velocity is reached.
+   */
+  private double computeDistanceFromVelocity(double velocity, double input, State initial) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    return initial.position
+        + (velocity - initial.velocity) / A
+        - B * u / (A * A) * Math.log((A * velocity + B * u) / (A * initial.velocity + B * u));
+  }
+
+  /**
+   * Calculate the velocity at which input should be reversed in order to reach the goal state from
+   * the current state.
+   *
+   * @param input The signed input applied to this profile from the current state.
+   * @param current The current state.
+   * @param goal The goal state.
+   * @return The inflection velocity.
+   */
+  private double solveForInflectionVelocity(double input, State current, State goal) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    var U_dir = Math.signum(u);
+
+    var position_delta = goal.position - current.position;
+    var velocity_delta = goal.velocity - current.velocity;
+
+    var scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
+    var power = -A / B / u * (A * position_delta - velocity_delta);
+
+    var a = -A * A;
+    var c = (B * B) * (u * u) + scalar * Math.exp(power);
+
+    if (-1e-9 < c && c < 0) {
+      // Numerical stability issue - the heuristic gets it right but c is around -1e-13
+      return 0;
+    }
+
+    return U_dir * Math.sqrt(-c / a);
+  }
+
+  /**
+   * Returns true if the profile should be inverted.
+   *
+   * <p>The profile is inverted if we should first apply negative input in order to reach the goal
+   * state.
+   *
+   * @param current The initial state (usually the current state).
+   * @param goal The desired state when the profile is complete.
+   */
+  @SuppressWarnings("UnnecessaryParentheses")
+  private boolean shouldFlipInput(State current, State goal) {
+    var u = m_constraints.maxInput;
+
+    var xf = goal.position;
+    var v0 = current.velocity;
+    var vf = goal.velocity;
+
+    var x_forward = computeDistanceFromVelocity(vf, u, current);
+    var x_reverse = computeDistanceFromVelocity(vf, -u, current);
+
+    if (v0 >= m_constraints.maxVelocity()) {
+      return xf < x_reverse;
+    }
+
+    if (v0 <= -m_constraints.maxVelocity()) {
+      return xf < x_forward;
+    }
+
+    var a = v0 >= 0;
+    var b = vf >= 0;
+    var c = xf >= x_forward;
+    var d = xf >= x_reverse;
+
+    return (a && !d) || (b && !c) || (!c && !d);
+  }
+}

diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
index f716dc0..f7120be 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java

@@ -30,9 +30,15 @@
    * Constructs a trajectory from a vector of states.
    *
    * @param states A vector of states.
+   * @throws IllegalArgumentException if the vector of states is empty.
    */
   public Trajectory(final List<State> states) {
     m_states = states;
+
+    if (m_states.isEmpty()) {
+      throw new IllegalArgumentException("Trajectory manually created with no states.");
+    }
+
     m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
   }
 
@@ -92,8 +98,13 @@
    *
    * @param timeSeconds The point in time since the beginning of the trajectory to sample.
    * @return The state at that point in time.
+   * @throws IllegalStateException if the trajectory has no states.
    */
   public State sample(double timeSeconds) {
+    if (m_states.isEmpty()) {
+      throw new IllegalStateException("Trajectory cannot be sampled if it has no states.");
+    }
+
     if (timeSeconds <= m_states.get(0).timeSeconds) {
       return m_states.get(0);
     }

diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
index cf9b6f8..a741aa0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java

@@ -271,11 +271,7 @@
       if (minMaxAccel.minAccelerationMetersPerSecondSq
           > minMaxAccel.maxAccelerationMetersPerSecondSq) {
         throw new TrajectoryGenerationException(
-            "The constraint's min acceleration "
-                + "was greater than its max acceleration.\n Offending Constraint: "
-                + constraint.getClass().getName()
-                + "\n If the offending constraint was packaged with WPILib, please file a bug"
-                + " report.");
+            "Infeasible trajectory constraint: " + constraint.getClass().getName() + "\n");
       }
 
       state.minAccelerationMetersPerSecondSq =

diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
index 3b6559e..fd7494c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -22,14 +22,14 @@
  *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
  * TrapezoidProfile.State previousProfiledReference =
  *   new TrapezoidProfile.State(initialReference, 0.0);
+ * TrapezoidProfile profile = new TrapezoidProfile(constraints);
  * </code></pre>
  *
  * <p>Run on update:
  *
  * <pre><code>
- * TrapezoidProfile profile =
- *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
- * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * previousProfiledReference =
+ * profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
  * </code></pre>
  *
  * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
@@ -42,9 +42,10 @@
   // The direction of the profile, either 1 for forwards or -1 for inverted
   private int m_direction;
 
-  private Constraints m_constraints;
-  private State m_initial;
-  private State m_goal;
+  private final Constraints m_constraints;
+  private State m_current;
+  private State m_goal; // TODO: Remove
+  private final boolean m_newAPI; // TODO: Remove
 
   private double m_endAccel;
   private double m_endFullSpeed;
@@ -100,23 +101,36 @@
    * Construct a TrapezoidProfile.
    *
    * @param constraints The constraints on the profile, like maximum velocity.
+   */
+  public TrapezoidProfile(Constraints constraints) {
+    m_constraints = constraints;
+    m_newAPI = true;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
    * @param goal The desired state when the profile is complete.
    * @param initial The initial state (usually the current state).
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
    */
+  @Deprecated(since = "2024", forRemoval = true)
   public TrapezoidProfile(Constraints constraints, State goal, State initial) {
     m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
     m_constraints = constraints;
-    m_initial = direct(initial);
+    m_current = direct(initial);
     m_goal = direct(goal);
-
-    if (m_initial.velocity > m_constraints.maxVelocity) {
-      m_initial.velocity = m_constraints.maxVelocity;
+    m_newAPI = false;
+    if (m_current.velocity > m_constraints.maxVelocity) {
+      m_current.velocity = m_constraints.maxVelocity;
     }
 
     // Deal with a possibly truncated motion profile (with nonzero initial or
     // final velocity) by calculating the parameters as if the profile began and
     // ended at zero velocity
-    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+    double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
     double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
 
     double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
@@ -126,7 +140,7 @@
     // of a truncated one
 
     double fullTrapezoidDist =
-        cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+        cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
     double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
 
     double fullSpeedDist =
@@ -148,7 +162,10 @@
    *
    * @param constraints The constraints on the profile, like maximum velocity.
    * @param goal The desired state when the profile is complete.
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
    */
+  @Deprecated(since = "2024", forRemoval = true)
   public TrapezoidProfile(Constraints constraints, State goal) {
     this(constraints, goal, new State(0, 0));
   }
@@ -159,17 +176,23 @@
    *
    * @param t The time since the beginning of the profile.
    * @return The position and velocity of the profile at time t.
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
    */
+  @Deprecated(since = "2024", forRemoval = true)
   public State calculate(double t) {
-    State result = new State(m_initial.position, m_initial.velocity);
+    if (m_newAPI) {
+      throw new RuntimeException("Cannot use new constructor with deprecated calculate()");
+    }
+    State result = new State(m_current.position, m_current.velocity);
 
     if (t < m_endAccel) {
       result.velocity += t * m_constraints.maxAcceleration;
-      result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+      result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
     } else if (t < m_endFullSpeed) {
       result.velocity = m_constraints.maxVelocity;
       result.position +=
-          (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+          (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
               + m_constraints.maxVelocity * (t - m_endAccel);
     } else if (t <= m_endDeccel) {
       result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
@@ -185,14 +208,83 @@
   }
 
   /**
+   * Calculate the correct position and velocity for the profile at a time t where the beginning of
+   * the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   * @param goal The desired state when the profile is complete.
+   * @param current The current state.
+   * @return The position and velocity of the profile at time t.
+   */
+  public State calculate(double t, State goal, State current) {
+    m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1;
+    m_current = direct(current);
+    goal = direct(goal);
+
+    if (m_current.velocity > m_constraints.maxVelocity) {
+      m_current.velocity = m_constraints.maxVelocity;
+    }
+
+    // Deal with a possibly truncated motion profile (with nonzero initial or
+    // final velocity) by calculating the parameters as if the profile began and
+    // ended at zero velocity
+    double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
+    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+    double cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
+    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+    // Now we can calculate the parameters as if it was a full trapezoid instead
+    // of a truncated one
+
+    double fullTrapezoidDist =
+        cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
+    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+    double fullSpeedDist =
+        fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+    // Handle the case where the profile never reaches full speed
+    if (fullSpeedDist < 0) {
+      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+      fullSpeedDist = 0;
+    }
+
+    m_endAccel = accelerationTime - cutoffBegin;
+    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+    State result = new State(m_current.position, m_current.velocity);
+
+    if (t < m_endAccel) {
+      result.velocity += t * m_constraints.maxAcceleration;
+      result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+    } else if (t < m_endFullSpeed) {
+      result.velocity = m_constraints.maxVelocity;
+      result.position +=
+          (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+              + m_constraints.maxVelocity * (t - m_endAccel);
+    } else if (t <= m_endDeccel) {
+      result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+      double timeLeft = m_endDeccel - t;
+      result.position =
+          goal.position
+              - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
+    } else {
+      result = goal;
+    }
+
+    return direct(result);
+  }
+
+  /**
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
    * @return The time left until a target distance in the profile is reached.
    */
   public double timeLeftUntil(double target) {
-    double position = m_initial.position * m_direction;
-    double velocity = m_initial.velocity * m_direction;
+    double position = m_current.position * m_direction;
+    double velocity = m_current.velocity * m_direction;
 
     double endAccel = m_endAccel * m_direction;
     double endFullSpeed = m_endFullSpeed * m_direction - endAccel;

diff --git a/wpimath/src/main/native/cpp/MathShared.cpp b/wpimath/src/main/native/cpp/MathShared.cpp
index 5252e87..b53d080 100644
--- a/wpimath/src/main/native/cpp/MathShared.cpp
+++ b/wpimath/src/main/native/cpp/MathShared.cpp

@@ -5,6 +5,9 @@
 #include "wpimath/MathShared.h"
 
 #include <wpi/mutex.h>
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
 
 using namespace wpi::math;
 
@@ -15,6 +18,9 @@
   void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
   }
   void ReportUsage(MathUsageId id, int count) override {}
+  units::second_t GetTimestamp() override {
+    return units::second_t{wpi::Now() * 1.0e-6};
+  }
 };
 }  // namespace
 

diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index fc532db..758d2e7 100644
--- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp

@@ -6,16 +6,16 @@
 
 namespace frc {
 
-Vectord<3> PoseTo3dVector(const Pose2d& pose) {
-  return Vectord<3>{pose.Translation().X().value(),
-                    pose.Translation().Y().value(),
-                    pose.Rotation().Radians().value()};
+Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
+  return Eigen::Vector3d{pose.Translation().X().value(),
+                         pose.Translation().Y().value(),
+                         pose.Rotation().Radians().value()};
 }
 
-Vectord<4> PoseTo4dVector(const Pose2d& pose) {
-  return Vectord<4>{pose.Translation().X().value(),
-                    pose.Translation().Y().value(), pose.Rotation().Cos(),
-                    pose.Rotation().Sin()};
+Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
+  return Eigen::Vector4d{pose.Translation().X().value(),
+                         pose.Translation().Y().value(), pose.Rotation().Cos(),
+                         pose.Rotation().Sin()};
 }
 
 template <>
@@ -28,9 +28,15 @@
   return detail::IsStabilizableImpl<2, 1>(A, B);
 }
 
-Vectord<3> PoseToVector(const Pose2d& pose) {
-  return Vectord<3>{pose.X().value(), pose.Y().value(),
-                    pose.Rotation().Radians().value()};
+template <>
+bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
+                                                    const Eigen::MatrixXd& B) {
+  return detail::IsStabilizableImpl<Eigen::Dynamic, Eigen::Dynamic>(A, B);
+}
+
+Eigen::Vector3d PoseToVector(const Pose2d& pose) {
+  return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
+                         pose.Rotation().Radians().value()};
 }
 
 }  // namespace frc

diff --git a/wpimath/src/main/native/cpp/controller/BangBangController.cpp b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
index a4fda0a..4a093d6 100644
--- a/wpimath/src/main/native/cpp/controller/BangBangController.cpp
+++ b/wpimath/src/main/native/cpp/controller/BangBangController.cpp

@@ -11,10 +11,7 @@
 using namespace frc;
 
 BangBangController::BangBangController(double tolerance)
-    : m_tolerance(tolerance) {
-  static int instances = 0;
-  instances++;
-}
+    : m_tolerance(tolerance) {}
 
 void BangBangController::SetSetpoint(double setpoint) {
   m_setpoint = setpoint;

diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
index 9788023..05be645 100644
--- a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp

@@ -6,7 +6,7 @@
 
 #include <utility>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
 
 using namespace frc;
 

diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
index e1b2732..257d7e1 100644
--- a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp

@@ -4,7 +4,8 @@
 
 #include "frc/controller/DifferentialDriveFeedforward.h"
 
-#include "frc/EigenCore.h"
+#include <Eigen/Core>
+
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/system/plant/LinearSystemId.h"
 
@@ -30,8 +31,8 @@
     units::meters_per_second_t nextRightVelocity, units::second_t dt) {
   frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
 
-  frc::Vectord<2> r{currentLeftVelocity, currentRightVelocity};
-  frc::Vectord<2> nextR{nextLeftVelocity, nextRightVelocity};
+  Eigen::Vector2d r{currentLeftVelocity, currentRightVelocity};
+  Eigen::Vector2d nextR{nextLeftVelocity, nextRightVelocity};
   auto u = feedforward.Calculate(r, nextR);
   return {units::volt_t{u(0)}, units::volt_t{u(1)}};
 }

diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
index def7234..64210a7 100644
--- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp

@@ -11,7 +11,7 @@
 using namespace frc;
 
 HolonomicDriveController::HolonomicDriveController(
-    frc2::PIDController xController, frc2::PIDController yController,
+    PIDController xController, PIDController yController,
     ProfiledPIDController<units::radian> thetaController)
     : m_xController(std::move(xController)),
       m_yController(std::move(yController)),
@@ -79,3 +79,16 @@
 void HolonomicDriveController::SetEnabled(bool enabled) {
   m_enabled = enabled;
 }
+
+ProfiledPIDController<units::radian>&
+HolonomicDriveController::getThetaController() {
+  return m_thetaController;
+}
+
+PIDController& HolonomicDriveController::getXController() {
+  return m_xController;
+}
+
+PIDController& HolonomicDriveController::getYController() {
+  return m_yController;
+}

diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
index 17a0c56..173f2ea 100644
--- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp

@@ -5,10 +5,14 @@
 #include "frc/controller/LTVDifferentialDriveController.h"
 
 #include <cmath>
+#include <stdexcept>
 
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
 #include "frc/MathUtil.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
 
 using namespace frc;
 
@@ -63,18 +67,39 @@
   // Ax = -Bu
   // x = -A⁻¹Bu
   units::meters_per_second_t maxV{
+      // NOLINTNEXTLINE(clang-analyzer-unix.Malloc)
       -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
 
+  if (maxV <= 0_mps) {
+    throw std::domain_error(
+        "Max velocity of plant with 12 V input must be greater than 0 m/s.");
+  }
+  if (maxV >= 15_mps) {
+    throw std::domain_error(
+        "Max velocity of plant with 12 V input must be less than 15 m/s.");
+  }
+
+  auto R_llt = R.llt();
+
   for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.
     if (units::math::abs(velocity) < 1e-4_mps) {
-      m_table.insert(velocity, Matrixd<2, 5>::Zero());
+      A(State::kY, State::kHeading) = 1e-4;
     } else {
       A(State::kY, State::kHeading) = velocity.value();
-      m_table.insert(velocity,
-                     frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
     }
+
+    Matrixd<5, 5> discA;
+    Matrixd<5, 2> discB;
+    DiscretizeAB(A, B, dt, &discA, &discB);
+
+    Matrixd<5, 5> S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_table.insert(velocity, (discB.transpose() * S * discB + R)
+                                 .llt()
+                                 .solve(discB.transpose() * S * discA));
   }
 }
 

diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
index 256b757..aca317a 100644
--- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
+++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp

@@ -4,8 +4,13 @@
 
 #include "frc/controller/LTVUnicycleController.h"
 
+#include <stdexcept>
+
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -37,6 +42,13 @@
 LTVUnicycleController::LTVUnicycleController(
     const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& Relems,
     units::second_t dt, units::meters_per_second_t maxVelocity) {
+  if (maxVelocity <= 0_mps) {
+    throw std::domain_error("Max velocity must be greater than 0 m/s.");
+  }
+  if (maxVelocity >= 15_mps) {
+    throw std::domain_error("Max velocity must be less than 15 m/s.");
+  }
+
   // The change in global pose for a unicycle is defined by the following three
   // equations.
   //
@@ -74,17 +86,28 @@
   Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
   Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
 
+  auto R_llt = R.llt();
+
   for (auto velocity = -maxVelocity; velocity < maxVelocity;
        velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.
     if (units::math::abs(velocity) < 1e-4_mps) {
-      m_table.insert(velocity, Matrixd<2, 3>::Zero());
+      A(State::kY, State::kHeading) = 1e-4;
     } else {
       A(State::kY, State::kHeading) = velocity.value();
-      m_table.insert(velocity,
-                     frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
     }
+
+    Matrixd<3, 3> discA;
+    Matrixd<3, 2> discB;
+    DiscretizeAB(A, B, dt, &discA, &discB);
+
+    Matrixd<3, 3> S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_table.insert(velocity, (discB.transpose() * S * discB + R)
+                                 .llt()
+                                 .solve(discB.transpose() * S * discA));
   }
 }
 

diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
index 6c89d25..2638f9e 100644
--- a/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp

@@ -13,7 +13,7 @@
 #include "frc/MathUtil.h"
 #include "wpimath/MathShared.h"
 
-using namespace frc2;
+using namespace frc;
 
 PIDController::PIDController(double Kp, double Ki, double Kd,
                              units::second_t period)
@@ -52,6 +52,14 @@
   m_Kd = Kd;
 }
 
+void PIDController::SetIZone(double iZone) {
+  if (iZone < 0) {
+    wpi::math::MathSharedStore::ReportError(
+        "IZone must be a non-negative number, got {}!", iZone);
+  }
+  m_iZone = iZone;
+}
+
 double PIDController::GetP() const {
   return m_Kp;
 }
@@ -64,6 +72,10 @@
   return m_Kd;
 }
 
+double PIDController::GetIZone() const {
+  return m_iZone;
+}
+
 units::second_t PIDController::GetPeriod() const {
   return m_period;
 }
@@ -78,11 +90,12 @@
 
 void PIDController::SetSetpoint(double setpoint) {
   m_setpoint = setpoint;
+  m_haveSetpoint = true;
 
   if (m_continuous) {
     double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
     m_positionError =
-        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+        InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
   } else {
     m_positionError = m_setpoint - m_measurement;
   }
@@ -95,7 +108,8 @@
 }
 
 bool PIDController::AtSetpoint() const {
-  return std::abs(m_positionError) < m_positionTolerance &&
+  return m_haveMeasurement && m_haveSetpoint &&
+         std::abs(m_positionError) < m_positionTolerance &&
          std::abs(m_velocityError) < m_velocityTolerance;
 }
 
@@ -137,18 +151,23 @@
 double PIDController::Calculate(double measurement) {
   m_measurement = measurement;
   m_prevError = m_positionError;
+  m_haveMeasurement = true;
 
   if (m_continuous) {
     double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
     m_positionError =
-        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+        InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
   } else {
     m_positionError = m_setpoint - m_measurement;
   }
 
   m_velocityError = (m_positionError - m_prevError) / m_period.value();
 
-  if (m_Ki != 0) {
+  // If the absolute value of the position error is outside of IZone, reset the
+  // total error
+  if (std::abs(m_positionError) > m_iZone) {
+    m_totalError = 0;
+  } else if (m_Ki != 0) {
     m_totalError =
         std::clamp(m_totalError + m_positionError * m_period.value(),
                    m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
@@ -159,6 +178,7 @@
 
 double PIDController::Calculate(double measurement, double setpoint) {
   m_setpoint = setpoint;
+  m_haveSetpoint = true;
   return Calculate(measurement);
 }
 
@@ -167,6 +187,7 @@
   m_prevError = 0;
   m_totalError = 0;
   m_velocityError = 0;
+  m_haveMeasurement = false;
 }
 
 void PIDController::InitSendable(wpi::SendableBuilder& builder) {
@@ -178,6 +199,9 @@
   builder.AddDoubleProperty(
       "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
   builder.AddDoubleProperty(
+      "izone", [this] { return GetIZone(); },
+      [this](double value) { SetIZone(value); });
+  builder.AddDoubleProperty(
       "setpoint", [this] { return GetSetpoint(); },
       [this](double value) { SetSetpoint(value); });
 }

diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index 39e0d8c..213ba99 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -4,40 +4,8 @@
 
 #include "frc/estimator/DifferentialDrivePoseEstimator.h"
 
-#include <wpi/timestamp.h>
-
-#include "frc/StateSpaceUtil.h"
-#include "frc/estimator/AngleStatistics.h"
-
 using namespace frc;
 
-DifferentialDrivePoseEstimator::InterpolationRecord
-DifferentialDrivePoseEstimator::InterpolationRecord::Interpolate(
-    DifferentialDriveKinematics& kinematics, InterpolationRecord endValue,
-    double i) const {
-  if (i < 0) {
-    return *this;
-  } else if (i > 1) {
-    return endValue;
-  } else {
-    // Find the interpolated left distance.
-    auto left = wpi::Lerp(this->leftDistance, endValue.leftDistance, i);
-    // Find the interpolated right distance.
-    auto right = wpi::Lerp(this->rightDistance, endValue.rightDistance, i);
-
-    // Find the new gyro angle.
-    auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
-    // Create a twist to represent this changed based on the interpolated
-    // sensor inputs.
-    auto twist =
-        kinematics.ToTwist2d(left - leftDistance, right - rightDistance);
-    twist.dtheta = (gyro - gyroAngle).Radians();
-
-    return {pose.Exp(twist), gyro, left, right};
-  }
-}
-
 DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
     units::meter_t leftDistance, units::meter_t rightDistance,
@@ -51,114 +19,7 @@
     units::meter_t leftDistance, units::meter_t rightDistance,
     const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs)
-    : m_kinematics{kinematics},
-      m_odometry{gyroAngle, leftDistance, rightDistance, initialPose} {
-  for (size_t i = 0; i < 3; ++i) {
-    m_q[i] = stateStdDevs[i] * stateStdDevs[i];
-  }
-
-  SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-}
-
-void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const wpi::array<double, 3>& visionMeasurementStdDevs) {
-  wpi::array<double, 3> r{wpi::empty_array};
-  for (size_t i = 0; i < 3; ++i) {
-    r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
-  }
-
-  // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-  // and C = I. See wpimath/algorithms.md.
-  for (size_t row = 0; row < 3; ++row) {
-    if (m_q[row] == 0.0) {
-      m_visionK(row, row) = 0.0;
-    } else {
-      m_visionK(row, row) =
-          m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
-    }
-  }
-}
-
-void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
-                                                   units::meter_t leftDistance,
-                                                   units::meter_t rightDistance,
-                                                   const Pose2d& pose) {
-  // Reset state estimate and error covariance
-  m_odometry.ResetPosition(gyroAngle, leftDistance, rightDistance, pose);
-  m_poseBuffer.Clear();
-}
-
-Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
-  return m_odometry.GetPose();
-}
-
-void DifferentialDrivePoseEstimator::AddVisionMeasurement(
-    const Pose2d& visionRobotPose, units::second_t timestamp) {
-  // Step 1: Get the estimated pose from when the vision measurement was made.
-  auto sample = m_poseBuffer.Sample(timestamp);
-
-  if (!sample.has_value()) {
-    return;
-  }
-
-  // Step 2: Measure the twist between the odometry pose and the vision pose.
-  auto twist = sample.value().pose.Log(visionRobotPose);
-
-  // Step 3: We should not trust the twist entirely, so instead we scale this
-  // twist by a Kalman gain matrix representing how much we trust vision
-  // measurements compared to our current pose.
-  frc::Vectord<3> k_times_twist =
-      m_visionK *
-      frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
-
-  // Step 4: Convert back to Twist2d.
-  Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
-                      units::meter_t{k_times_twist(1)},
-                      units::radian_t{k_times_twist(2)}};
-
-  // Step 5: Reset Odometry to state at sample with vision adjustment.
-  m_odometry.ResetPosition(
-      sample.value().gyroAngle, sample.value().leftDistance,
-      sample.value().rightDistance, sample.value().pose.Exp(scaledTwist));
-
-  // Step 6: Replay odometry inputs between sample time and latest recorded
-  // sample to update the pose buffer and correct odometry.
-  auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
-  auto first_newer_record =
-      std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
-                       [](const auto& pair, auto t) { return t > pair.first; });
-
-  for (auto entry = first_newer_record + 1; entry != internal_buf.end();
-       entry++) {
-    UpdateWithTime(entry->first, entry->second.gyroAngle,
-                   entry->second.leftDistance, entry->second.rightDistance);
-  }
-}
-
-Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
-                                              units::meter_t leftDistance,
-                                              units::meter_t rightDistance) {
-  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                        leftDistance, rightDistance);
-}
-
-Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    units::meter_t leftDistance, units::meter_t rightDistance) {
-  m_odometry.Update(gyroAngle, leftDistance, rightDistance);
-
-  // fmt::print("odo, {}, {}, {}, {}, {}, {}\n",
-  //   gyroAngle.Radians(),
-  //   leftDistance,
-  //   rightDistance,
-  //   GetEstimatedPosition().X(),
-  //   GetEstimatedPosition().Y(),
-  //   GetEstimatedPosition().Rotation().Radians()
-  // );
-
-  m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
-                                       leftDistance, rightDistance});
-
-  return GetEstimatedPosition();
-}
+    : PoseEstimator<DifferentialDriveWheelSpeeds,
+                    DifferentialDriveWheelPositions>(
+          kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+      m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}

diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index 9642e08..7c75e71 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -8,49 +8,10 @@
 
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
+#include "wpimath/MathShared.h"
 
 using namespace frc;
 
-frc::MecanumDrivePoseEstimator::InterpolationRecord
-frc::MecanumDrivePoseEstimator::InterpolationRecord::Interpolate(
-    MecanumDriveKinematics& kinematics, InterpolationRecord endValue,
-    double i) const {
-  if (i < 0) {
-    return *this;
-  } else if (i > 1) {
-    return endValue;
-  } else {
-    // Find the new wheel distance measurements.
-    MecanumDriveWheelPositions wheels_lerp{
-        wpi::Lerp(this->wheelPositions.frontLeft,
-                  endValue.wheelPositions.frontLeft, i),
-        wpi::Lerp(this->wheelPositions.frontRight,
-                  endValue.wheelPositions.frontRight, i),
-        wpi::Lerp(this->wheelPositions.rearLeft,
-                  endValue.wheelPositions.rearLeft, i),
-        wpi::Lerp(this->wheelPositions.rearRight,
-                  endValue.wheelPositions.rearRight, i)};
-
-    // Find the distance between this measurement and the
-    // interpolated measurement.
-    MecanumDriveWheelPositions wheels_delta{
-        wheels_lerp.frontLeft - this->wheelPositions.frontLeft,
-        wheels_lerp.frontRight - this->wheelPositions.frontRight,
-        wheels_lerp.rearLeft - this->wheelPositions.rearLeft,
-        wheels_lerp.rearRight - this->wheelPositions.rearRight};
-
-    // Find the new gyro angle.
-    auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
-    // Create a twist to represent this changed based on the interpolated
-    // sensor inputs.
-    auto twist = kinematics.ToTwist2d(wheels_delta);
-    twist.dtheta = (gyro - gyroAngle).Radians();
-
-    return {pose.Exp(twist), gyro, wheels_lerp};
-  }
-}
-
 frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
     const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
@@ -63,107 +24,6 @@
     const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
     const wpi::array<double, 3>& stateStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs)
-    : m_kinematics{kinematics},
-      m_odometry{kinematics, gyroAngle, wheelPositions, initialPose} {
-  for (size_t i = 0; i < 3; ++i) {
-    m_q[i] = stateStdDevs[i] * stateStdDevs[i];
-  }
-
-  SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-}
-
-void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const wpi::array<double, 3>& visionMeasurementStdDevs) {
-  wpi::array<double, 3> r{wpi::empty_array};
-  for (size_t i = 0; i < 3; ++i) {
-    r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
-  }
-
-  // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-  // and C = I. See wpimath/algorithms.md.
-  for (size_t row = 0; row < 3; ++row) {
-    if (m_q[row] == 0.0) {
-      m_visionK(row, row) = 0.0;
-    } else {
-      m_visionK(row, row) =
-          m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
-    }
-  }
-}
-
-void frc::MecanumDrivePoseEstimator::ResetPosition(
-    const Rotation2d& gyroAngle,
-    const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
-  // Reset state estimate and error covariance
-  m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
-  m_poseBuffer.Clear();
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
-  return m_odometry.GetPose();
-}
-
-void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
-    const Pose2d& visionRobotPose, units::second_t timestamp) {
-  // Step 1: Get the estimated pose from when the vision measurement was made.
-  auto sample = m_poseBuffer.Sample(timestamp);
-
-  if (!sample.has_value()) {
-    return;
-  }
-
-  // Step 2: Measure the twist between the odometry pose and the vision pose
-  auto twist = sample.value().pose.Log(visionRobotPose);
-
-  // Step 3: We should not trust the twist entirely, so instead we scale this
-  // twist by a Kalman gain matrix representing how much we trust vision
-  // measurements compared to our current pose.
-  frc::Vectord<3> k_times_twist =
-      m_visionK *
-      frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
-
-  // Step 4: Convert back to Twist2d
-  Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
-                      units::meter_t{k_times_twist(1)},
-                      units::radian_t{k_times_twist(2)}};
-
-  // Step 5: Reset Odometry to state at sample with vision adjustment.
-  m_odometry.ResetPosition(sample.value().gyroAngle,
-                           sample.value().wheelPositions,
-                           sample.value().pose.Exp(scaledTwist));
-
-  // Step 6: Replay odometry inputs between sample time and latest recorded
-  // sample to update the pose buffer and correct odometry.
-  auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
-  auto upper_bound =
-      std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
-                       [](const auto& pair, auto t) { return t > pair.first; });
-
-  for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
-    UpdateWithTime(entry->first, entry->second.gyroAngle,
-                   entry->second.wheelPositions);
-  }
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::Update(
-    const Rotation2d& gyroAngle,
-    const MecanumDriveWheelPositions& wheelPositions) {
-  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                        wheelPositions);
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    const MecanumDriveWheelPositions& wheelPositions) {
-  m_odometry.Update(gyroAngle, wheelPositions);
-
-  MecanumDriveWheelPositions internalWheelPositions{
-      wheelPositions.frontLeft, wheelPositions.frontRight,
-      wheelPositions.rearLeft, wheelPositions.rearRight};
-
-  m_poseBuffer.AddSample(
-      currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
-
-  return GetEstimatedPosition();
-}
+    : PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
+          kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+      m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}

diff --git a/wpimath/src/main/native/cpp/filter/Debouncer.cpp b/wpimath/src/main/native/cpp/filter/Debouncer.cpp
index 4e909a2..841768e 100644
--- a/wpimath/src/main/native/cpp/filter/Debouncer.cpp
+++ b/wpimath/src/main/native/cpp/filter/Debouncer.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/filter/Debouncer.h"
 
+#include "wpimath/MathShared.h"
+
 using namespace frc;
 
 Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
@@ -21,11 +23,12 @@
 }
 
 void Debouncer::ResetTimer() {
-  m_prevTime = units::microsecond_t(wpi::Now());
+  m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
 }
 
 bool Debouncer::HasElapsed() const {
-  return units::microsecond_t(wpi::Now()) - m_prevTime >= m_debounceTime;
+  return wpi::math::MathSharedStore::GetTimestamp() - m_prevTime >=
+         m_debounceTime;
 }
 
 bool Debouncer::Calculate(bool input) {

diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
index d1ee93d..2a8c9db 100644
--- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
+++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp

@@ -8,7 +8,8 @@
 #include <stdexcept>
 #include <utility>
 
-#include "Eigen/QR"
+#include <Eigen/Core>
+#include <Eigen/QR>
 
 using namespace frc;
 
@@ -18,7 +19,7 @@
   // Construct a change of basis matrix from the source coordinate system to the
   // NWU coordinate system. Each column vector in the change of basis matrix is
   // one of the old basis vectors mapped to its representation in the new basis.
-  Matrixd<3, 3> R;
+  Eigen::Matrix3d R;
   R.block<3, 1>(0, 0) = positiveX.m_axis;
   R.block<3, 1>(0, 1) = positiveY.m_axis;
   R.block<3, 1>(0, 2) = positiveZ.m_axis;
@@ -68,6 +69,8 @@
 Transform3d CoordinateSystem::Convert(const Transform3d& transform,
                                       const CoordinateSystem& from,
                                       const CoordinateSystem& to) {
-  return Transform3d{Convert(transform.Translation(), from, to),
-                     Convert(transform.Rotation(), from, to)};
+  const auto coordRot = from.m_rotation - to.m_rotation;
+  return Transform3d{
+      Convert(transform.Translation(), from, to),
+      (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
 }

diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index 2648a90..2e15216 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp

@@ -8,6 +8,9 @@
 
 #include <wpi/json.h>
 
+#include "frc/MathUtil.h"
+#include "geometry2d.pb.h"
+
 using namespace frc;
 
 Transform2d Pose2d::operator-(const Pose2d& other) const {
@@ -67,6 +70,36 @@
   return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
 }
 
+Pose2d Pose2d::Nearest(std::span<const Pose2d> poses) const {
+  return *std::min_element(
+      poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+        auto aDistance = this->Translation().Distance(a.Translation());
+        auto bDistance = this->Translation().Distance(b.Translation());
+
+        // If the distances are equal sort by difference in rotation
+        if (aDistance == bDistance) {
+          return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
+                 std::abs((this->Rotation() - b.Rotation()).Radians().value());
+        }
+        return aDistance < bDistance;
+      });
+}
+
+Pose2d Pose2d::Nearest(std::initializer_list<Pose2d> poses) const {
+  return *std::min_element(
+      poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+        auto aDistance = this->Translation().Distance(a.Translation());
+        auto bDistance = this->Translation().Distance(b.Translation());
+
+        // If the distances are equal sort by difference in rotation
+        if (aDistance == bDistance) {
+          return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
+                 std::abs((this->Rotation() - b.Rotation()).Radians().value());
+        }
+        return aDistance < bDistance;
+      });
+}
+
 void frc::to_json(wpi::json& json, const Pose2d& pose) {
   json = wpi::json{{"translation", pose.Translation()},
                    {"rotation", pose.Rotation()}};
@@ -76,3 +109,23 @@
   pose = Pose2d{json.at("translation").get<Translation2d>(),
                 json.at("rotation").get<Rotation2d>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
+      arena);
+}
+
+frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
+  return Pose2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+                wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
index 4732c4d..ffbaecb 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -6,8 +6,11 @@
 
 #include <cmath>
 
+#include <Eigen/Core>
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
+
 using namespace frc;
 
 namespace {
@@ -21,14 +24,14 @@
  * @param rotation The rotation vector.
  * @return The rotation vector as a 3x3 rotation matrix.
  */
-Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
+Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) {
   // Given a rotation vector <a, b, c>,
   //         [ 0 -c  b]
   // Omega = [ c  0 -a]
   //         [-b  a  0]
-  return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
-                       {rotation(2), 0.0, -rotation(0)},
-                       {-rotation(1), rotation(0), 0.0}};
+  return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
+                         {rotation(2), 0.0, -rotation(0)},
+                         {-rotation(1), rotation(0), 0.0}};
 }
 }  // namespace
 
@@ -60,6 +63,10 @@
   return *this * (1.0 / scalar);
 }
 
+Pose3d Pose3d::RotateBy(const Rotation3d& other) const {
+  return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
 Pose3d Pose3d::TransformBy(const Transform3d& other) const {
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
           other.Rotation() + m_rotation};
@@ -71,73 +78,102 @@
 }
 
 Pose3d Pose3d::Exp(const Twist3d& twist) const {
-  Matrixd<3, 3> Omega = RotationVectorToMatrix(
-      Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
-  Matrixd<3, 3> OmegaSq = Omega * Omega;
+  // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
+  Eigen::Vector3d u{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+  Eigen::Vector3d rvec{twist.rx.value(), twist.ry.value(), twist.rz.value()};
+  Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
+  Eigen::Matrix3d omegaSq = omega * omega;
+  double theta = rvec.norm();
+  double thetaSq = theta * theta;
 
-  double thetaSq =
-      (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
-
-  // Get left Jacobian of SO3. See first line in right column of
-  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
-  Matrixd<3, 3> J;
-  if (thetaSq < 1E-9 * 1E-9) {
-    // V = I + 0.5ω
-    J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
+  double A;
+  double B;
+  double C;
+  if (std::abs(theta) < 1E-7) {
+    // Taylor Expansions around θ = 0
+    // A = 1/1! - θ²/3! + θ⁴/5!
+    // B = 1/2! - θ²/4! + θ⁴/6!
+    // C = 1/3! - θ²/5! + θ⁴/7!
+    // sources:
+    // A:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+    // B:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    // C:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
+    B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
+    C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
   } else {
-    double theta = std::sqrt(thetaSq);
-    // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
-    J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
-        (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
+    // A = std::sin(θ)/θ
+    // B = (1 - std::cos(θ)) / θ²
+    // C = (1 - A) / θ²
+    A = std::sin(theta) / theta;
+    B = (1 - std::cos(theta)) / thetaSq;
+    C = (1 - A) / thetaSq;
   }
 
-  // Get translation component
-  Vectord<3> translation =
-      J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+  Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + A * omega + B * omegaSq;
+  Eigen::Matrix3d V = Eigen::Matrix3d::Identity() + B * omega + C * omegaSq;
 
-  const Transform3d transform{Translation3d{units::meter_t{translation(0)},
-                                            units::meter_t{translation(1)},
-                                            units::meter_t{translation(2)}},
-                              Rotation3d{twist.rx, twist.ry, twist.rz}};
+  auto translation_component = V * u;
+  const Transform3d transform{
+      Translation3d{units::meter_t{translation_component(0)},
+                    units::meter_t{translation_component(1)},
+                    units::meter_t{translation_component(2)}},
+      Rotation3d{R}};
 
   return *this + transform;
 }
 
 Twist3d Pose3d::Log(const Pose3d& end) const {
+  // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
   const auto transform = end.RelativeTo(*this);
 
-  Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
+  Eigen::Vector3d u{transform.X().value(), transform.Y().value(),
+                    transform.Z().value()};
+  Eigen::Vector3d rvec =
+      transform.Rotation().GetQuaternion().ToRotationVector();
 
-  Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
-  Matrixd<3, 3> OmegaSq = Omega * Omega;
+  Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
+  Eigen::Matrix3d omegaSq = omega * omega;
+  double theta = rvec.norm();
+  double thetaSq = theta * theta;
 
-  double thetaSq = rotVec.squaredNorm();
-
-  // Get left Jacobian inverse of SO3. See fourth line in right column of
-  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
-  Matrixd<3, 3> Jinv;
-  if (thetaSq < 1E-9 * 1E-9) {
-    // J⁻¹ = I − 0.5ω + 1/12 ω²
-    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
+  double C;
+  if (std::abs(theta) < 1E-7) {
+    // Taylor Expansions around θ = 0
+    // A = 1/1! - θ²/3! + θ⁴/5!
+    // B = 1/2! - θ²/4! + θ⁴/6!
+    // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
+    // sources:
+    // A:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+    // B:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    // C:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
   } else {
-    double theta = std::sqrt(thetaSq);
-    double halfTheta = 0.5 * theta;
-
-    // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
-    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
-           (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
-               thetaSq * OmegaSq;
+    // A = std::sin(θ)/θ
+    // B = (1 - std::cos(θ)) / θ²
+    // C = (1 - A/(2*B)) / θ²
+    double A = std::sin(theta) / theta;
+    double B = (1 - std::cos(theta)) / thetaSq;
+    C = (1 - A / (2 * B)) / thetaSq;
   }
 
-  // Get dtranslation component
-  Vectord<3> dtranslation =
-      Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
-                        transform.Z().value()};
+  Eigen::Matrix3d V_inv =
+      Eigen::Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
 
-  return Twist3d{
-      units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
-      units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
-      units::radian_t{rotVec(1)},      units::radian_t{rotVec(2)}};
+  Eigen::Vector3d translation_component = V_inv * u;
+
+  return Twist3d{units::meter_t{translation_component(0)},
+                 units::meter_t{translation_component(1)},
+                 units::meter_t{translation_component(2)},
+                 units::radian_t{rvec(0)},
+                 units::radian_t{rvec(1)},
+                 units::radian_t{rvec(2)}};
 }
 
 Pose2d Pose3d::ToPose2d() const {
@@ -153,3 +189,23 @@
   pose = Pose3d{json.at("translation").get<Translation3d>(),
                 json.at("rotation").get<Rotation3d>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
+      arena);
+}
+
+frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
+  return Pose3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+                wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
index 9c2ceda..37afbb8 100644
--- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp

@@ -4,13 +4,53 @@
 
 #include "frc/geometry/Quaternion.h"
 
+#include <numbers>
+
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
+
 using namespace frc;
 
 Quaternion::Quaternion(double w, double x, double y, double z)
     : m_r{w}, m_v{x, y, z} {}
 
+Quaternion Quaternion::operator+(const Quaternion& other) const {
+  return Quaternion{
+      m_r + other.m_r,
+      m_v(0) + other.m_v(0),
+      m_v(1) + other.m_v(1),
+      m_v(2) + other.m_v(2),
+  };
+}
+
+Quaternion Quaternion::operator-(const Quaternion& other) const {
+  return Quaternion{
+      m_r - other.m_r,
+      m_v(0) - other.m_v(0),
+      m_v(1) - other.m_v(1),
+      m_v(2) - other.m_v(2),
+  };
+}
+
+Quaternion Quaternion::operator*(const double other) const {
+  return Quaternion{
+      m_r * other,
+      m_v(0) * other,
+      m_v(1) * other,
+      m_v(2) * other,
+  };
+}
+
+Quaternion Quaternion::operator/(const double other) const {
+  return Quaternion{
+      m_r / other,
+      m_v(0) / other,
+      m_v(1) / other,
+      m_v(2) / other,
+  };
+}
+
 Quaternion Quaternion::operator*(const Quaternion& other) const {
   // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
   const auto& r1 = m_r;
@@ -26,26 +66,102 @@
   // v = r₁v₂ + r₂v₁ + v₁ x v₂
   Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
 
-  return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
+  return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂
+                    r1 * r2 - v1.dot(v2),
+                    // v = r₁v₂ + r₂v₁ + v₁ x v₂
+                    v(0), v(1), v(2)};
 }
 
 bool Quaternion::operator==(const Quaternion& other) const {
-  return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+  return std::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
+         std::abs(Norm() - other.Norm()) < 1e-9;
+}
+
+Quaternion Quaternion::Conjugate() const {
+  return Quaternion{W(), -X(), -Y(), -Z()};
+}
+
+double Quaternion::Dot(const Quaternion& other) const {
+  return W() * other.W() + m_v.dot(other.m_v);
 }
 
 Quaternion Quaternion::Inverse() const {
-  return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
+  double norm = Norm();
+  return Conjugate() / (norm * norm);
+}
+
+double Quaternion::Norm() const {
+  return std::sqrt(Dot(*this));
 }
 
 Quaternion Quaternion::Normalize() const {
-  double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+  double norm = Norm();
   if (norm == 0.0) {
     return Quaternion{};
   } else {
-    return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+    return Quaternion{W(), X(), Y(), Z()} / norm;
   }
 }
 
+Quaternion Quaternion::Pow(const double other) const {
+  return (Log() * other).Exp();
+}
+
+Quaternion Quaternion::Exp(const Quaternion& other) const {
+  return other.Exp() * *this;
+}
+
+Quaternion Quaternion::Exp() const {
+  double scalar = std::exp(m_r);
+
+  double axial_magnitude = m_v.norm();
+  double cosine = std::cos(axial_magnitude);
+
+  double axial_scalar;
+
+  if (axial_magnitude < 1e-9) {
+    // Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶)
+    double axial_magnitude_sq = axial_magnitude * axial_magnitude;
+    double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+    axial_scalar =
+        1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+  } else {
+    axial_scalar = std::sin(axial_magnitude) / axial_magnitude;
+  }
+
+  return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
+                    Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
+}
+
+Quaternion Quaternion::Log(const Quaternion& other) const {
+  return (other * Inverse()).Log();
+}
+
+Quaternion Quaternion::Log() const {
+  double scalar = std::log(Norm());
+
+  double v_norm = m_v.norm();
+
+  double s_norm = W() / Norm();
+
+  if (std::abs(s_norm + 1) < 1e-9) {
+    return Quaternion{scalar, -std::numbers::pi, 0, 0};
+  }
+
+  double v_scalar;
+
+  if (v_norm < 1e-9) {
+    // Taylor series expansion of atan2(y / x) / y around y = 0 = 1/x -
+    // y^2/3*x^3 + O(y^4)
+    v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
+  } else {
+    v_scalar = std::atan2(v_norm, W()) / v_norm;
+  }
+
+  return Quaternion{scalar, v_scalar * m_v(0), v_scalar * m_v(1),
+                    v_scalar * m_v(2)};
+}
+
 double Quaternion::W() const {
   return m_r;
 }
@@ -80,6 +196,30 @@
   }
 }
 
+Quaternion Quaternion::FromRotationVector(const Eigen::Vector3d& rvec) {
+  // 𝑣⃗ = θ * v̂
+  // v̂ = 𝑣⃗ / θ
+
+  // 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
+  // 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
+
+  double theta = rvec.norm();
+  double cos = std::cos(theta / 2);
+
+  double axial_scalar;
+
+  if (theta < 1e-9) {
+    // taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 +
+    // O(θ⁴)
+    axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+  } else {
+    axial_scalar = std::sin(theta / 2) / theta;
+  }
+
+  return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
+                    axial_scalar * rvec(2)};
+}
+
 void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
   json = wpi::json{{"W", quaternion.W()},
                    {"X", quaternion.X()},
@@ -92,3 +232,24 @@
       Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
                  json.at("Y").get<double>(), json.at("Z").get<double>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
+      arena);
+}
+
+frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
+  return frc::Quaternion{m->w(), m->x(), m->y(), m->z()};
+}
+
+void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
+                                          const frc::Quaternion& value) {
+  auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
+  m->set_w(value.W());
+  m->set_x(value.X());
+  m->set_y(value.Y());
+  m->set_z(value.Z());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 921e1f8..05a644e 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp

@@ -8,6 +8,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry2d.pb.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -19,3 +20,21 @@
 void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
   rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
+      arena);
+}
+
+frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
+  return frc::Rotation2d{units::radian_t{m->value()}};
+}
+
+void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
+  m->set_value(value.Radians().value());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
index 298f0e6..3a68870 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -7,12 +7,13 @@
 #include <cmath>
 #include <numbers>
 
+#include <Eigen/Core>
+#include <Eigen/LU>
+#include <Eigen/QR>
 #include <wpi/json.h>
 
-#include "Eigen/Core"
-#include "Eigen/LU"
-#include "Eigen/QR"
 #include "frc/fmt/Eigen.h"
+#include "geometry3d.pb.h"
 #include "units/math.h"
 #include "wpimath/MathShared.h"
 
@@ -38,23 +39,26 @@
                    cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
 }
 
-Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
+Rotation3d::Rotation3d(const Eigen::Vector3d& rvec)
+    : Rotation3d{rvec, units::radian_t{rvec.norm()}} {}
+
+Rotation3d::Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
   double norm = axis.norm();
   if (norm == 0.0) {
     return;
   }
 
   // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
-  Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
+  Eigen::Vector3d v = axis / norm * units::math::sin(angle / 2.0);
   m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
 }
 
-Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
+Rotation3d::Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
   const auto& R = rotationMatrix;
 
   // Require that the rotation matrix is special orthogonal. This is true if the
   // matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
-  if ((R * R.transpose() - Matrixd<3, 3>::Identity()).norm() > 1e-9) {
+  if ((R * R.transpose() - Eigen::Matrix3d::Identity()).norm() > 1e-9) {
     std::string msg =
         fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
 
@@ -109,7 +113,8 @@
   m_q = Quaternion{w, x, y, z};
 }
 
-Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
+Rotation3d::Rotation3d(const Eigen::Vector3d& initial,
+                       const Eigen::Vector3d& final) {
   double dot = initial.dot(final);
   double normProduct = initial.norm() * final.norm();
   double dotNorm = dot / normProduct;
@@ -170,6 +175,11 @@
   return *this * (1.0 / scalar);
 }
 
+bool Rotation3d::operator==(const Rotation3d& other) const {
+  return std::abs(std::abs(m_q.Dot(other.m_q)) -
+                  m_q.Norm() * other.m_q.Norm()) < 1e-9;
+}
+
 Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
   return Rotation3d{other.m_q * m_q};
 }
@@ -184,9 +194,15 @@
   double y = m_q.Y();
   double z = m_q.Z();
 
-  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
-  return units::radian_t{
-      std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
+  // wpimath/algorithms.md
+  double cxcy = 1.0 - 2.0 * (x * x + y * y);
+  double sxcy = 2.0 * (w * x + y * z);
+  double cy_sq = cxcy * cxcy + sxcy * sxcy;
+  if (cy_sq > 1e-20) {
+    return units::radian_t{std::atan2(sxcy, cxcy)};
+  } else {
+    return 0_rad;
+  }
 }
 
 units::radian_t Rotation3d::Y() const {
@@ -195,7 +211,7 @@
   double y = m_q.Y();
   double z = m_q.Z();
 
-  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
   double ratio = 2.0 * (w * y - z * x);
   if (std::abs(ratio) >= 1.0) {
     return units::radian_t{std::copysign(std::numbers::pi / 2.0, ratio)};
@@ -210,12 +226,18 @@
   double y = m_q.Y();
   double z = m_q.Z();
 
-  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
-  return units::radian_t{
-      std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
+  // wpimath/algorithms.md
+  double cycz = 1.0 - 2.0 * (y * y + z * z);
+  double cysz = 2.0 * (w * z + x * y);
+  double cy_sq = cycz * cycz + cysz * cysz;
+  if (cy_sq > 1e-20) {
+    return units::radian_t{std::atan2(cysz, cycz)};
+  } else {
+    return units::radian_t{std::atan2(2.0 * w * z, w * w - z * z)};
+  }
 }
 
-Vectord<3> Rotation3d::Axis() const {
+Eigen::Vector3d Rotation3d::Axis() const {
   double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
   if (norm == 0.0) {
     return {0.0, 0.0, 0.0};
@@ -240,3 +262,21 @@
 void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
   rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
+      arena);
+}
+
+frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
+  return Rotation3d{wpi::UnpackProtobuf<frc::Quaternion>(m->q())};
+}
+
+void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
+  wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index 25b0590..77f3cee 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp

@@ -5,6 +5,7 @@
 #include "frc/geometry/Transform2d.h"
 
 #include "frc/geometry/Pose2d.h"
+#include "geometry2d.pb.h"
 
 using namespace frc;
 
@@ -21,3 +22,23 @@
 Transform2d Transform2d::operator+(const Transform2d& other) const {
   return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform2d>(arena);
+}
+
+frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
+  return Transform2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+                     wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
index 1cfabaa..de6c253 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp

@@ -5,6 +5,7 @@
 #include "frc/geometry/Transform3d.h"
 
 #include "frc/geometry/Pose3d.h"
+#include "geometry3d.pb.h"
 
 using namespace frc;
 
@@ -21,6 +22,10 @@
 Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
     : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
 
+Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
+                         Rotation3d rotation)
+    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+
 Transform3d Transform3d::Inverse() const {
   // We are rotating the difference between the translations
   // using a clockwise rotation matrix. This transforms the global
@@ -31,3 +36,23 @@
 Transform3d Transform3d::operator+(const Transform3d& other) const {
   return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform3d>(arena);
+}
+
+frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
+  return Transform3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+                     wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index d463696..6d5f315 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp

@@ -6,6 +6,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry2d.pb.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -23,6 +24,22 @@
          units::math::abs(m_y - other.m_y) < 1E-9_m;
 }
 
+Translation2d Translation2d::Nearest(
+    std::span<const Translation2d> translations) const {
+  return *std::min_element(translations.begin(), translations.end(),
+                           [this](Translation2d a, Translation2d b) {
+                             return this->Distance(a) < this->Distance(b);
+                           });
+}
+
+Translation2d Translation2d::Nearest(
+    std::initializer_list<Translation2d> translations) const {
+  return *std::min_element(translations.begin(), translations.end(),
+                           [this](Translation2d a, Translation2d b) {
+                             return this->Distance(a) < this->Distance(b);
+                           });
+}
+
 void frc::to_json(wpi::json& json, const Translation2d& translation) {
   json =
       wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
@@ -32,3 +49,22 @@
   translation = Translation2d{units::meter_t{json.at("x").get<double>()},
                               units::meter_t{json.at("y").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation2d>(arena);
+}
+
+frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
+  return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}};
+}
+
+void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index 2c53791..90e94ae 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp

@@ -6,6 +6,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
 #include "units/length.h"
 #include "units/math.h"
 
@@ -52,3 +53,24 @@
                               units::meter_t{json.at("y").get<double>()},
                               units::meter_t{json.at("z").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation3d>(arena);
+}
+
+frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
+  return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()},
+                            units::meter_t{m->z()}};
+}
+
+void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+  m->set_z(value.Z().value());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp b/wpimath/src/main/native/cpp/geometry/Twist2d.cpp
new file mode 100644
index 0000000..6c106eb
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Twist2d.cpp

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Twist2d.h"
+
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
+      arena);
+}
+
+frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&msg);
+  return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
+                      units::radian_t{m->dtheta()}};
+}
+
+void wpi::Protobuf<frc::Twist2d>::Pack(google::protobuf::Message* msg,
+                                       const frc::Twist2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTwist2d*>(msg);
+  m->set_dx(value.dx.value());
+  m->set_dy(value.dy.value());
+  m->set_dtheta(value.dtheta.value());
+}

diff --git a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp b/wpimath/src/main/native/cpp/geometry/Twist3d.cpp
new file mode 100644
index 0000000..4f4ce86
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Twist3d.cpp

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Twist3d.h"
+
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
+      arena);
+}
+
+frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&msg);
+  return frc::Twist3d{units::meter_t{m->dx()},  units::meter_t{m->dy()},
+                      units::meter_t{m->dz()},  units::radian_t{m->rx()},
+                      units::radian_t{m->ry()}, units::radian_t{m->rz()}};
+}
+
+void wpi::Protobuf<frc::Twist3d>::Pack(google::protobuf::Message* msg,
+                                       const frc::Twist3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTwist3d*>(msg);
+  m->set_dx(value.dx.value());
+  m->set_dy(value.dy.value());
+  m->set_dz(value.dz.value());
+  m->set_rx(value.rx.value());
+  m->set_ry(value.ry.value());
+  m->set_rz(value.rz.value());
+}

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
deleted file mode 100644
index fd8a223..0000000
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ /dev/null

@@ -1,367 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include <jni.h>
-
-#include <exception>
-
-#include <wpi/jni_util.h>
-
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "Eigen/Eigenvalues"
-#include "Eigen/QR"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "edu_wpi_first_math_WPIMathJNI.h"
-#include "frc/trajectory/TrajectoryUtil.h"
-#include "unsupported/Eigen/MatrixFunctions"
-
-using namespace wpi::java;
-
-/**
- * Returns true if (A, B) is a stabilizable pair.
- *
- * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
- * any, have absolute values less than one, where an eigenvalue is
- * uncontrollable if rank(λI - A, B) < n where n is the number of states.
- *
- * @param A System matrix.
- * @param B Input matrix.
- */
-bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
-                        const Eigen::Ref<const Eigen::MatrixXd>& B) {
-  int states = B.rows();
-  int inputs = B.cols();
-  Eigen::EigenSolver<Eigen::MatrixXd> es{A};
-  for (int i = 0; i < states; ++i) {
-    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
-            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
-        1) {
-      continue;
-    }
-
-    Eigen::MatrixXcd E{states, states + inputs};
-    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A,
-        B;
-    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
-    if (qr.rank() < states) {
-      return false;
-    }
-  }
-
-  return true;
-}
-
-std::vector<double> GetElementsFromTrajectory(
-    const frc::Trajectory& trajectory) {
-  std::vector<double> elements;
-  elements.reserve(trajectory.States().size() * 7);
-
-  for (auto&& state : trajectory.States()) {
-    elements.push_back(state.t.value());
-    elements.push_back(state.velocity.value());
-    elements.push_back(state.acceleration.value());
-    elements.push_back(state.pose.X().value());
-    elements.push_back(state.pose.Y().value());
-    elements.push_back(state.pose.Rotation().Radians().value());
-    elements.push_back(state.curvature.value());
-  }
-
-  return elements;
-}
-
-frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
-  // Make sure that the elements have the correct length.
-  assert(elements.size() % 7 == 0);
-
-  // Create a vector of states from the elements.
-  std::vector<frc::Trajectory::State> states;
-  states.reserve(elements.size() / 7);
-
-  for (size_t i = 0; i < elements.size(); i += 7) {
-    states.emplace_back(frc::Trajectory::State{
-        units::second_t{elements[i]},
-        units::meters_per_second_t{elements[i + 1]},
-        units::meters_per_second_squared_t{elements[i + 2]},
-        frc::Pose2d{units::meter_t{elements[i + 3]},
-                    units::meter_t{elements[i + 4]},
-                    units::radian_t{elements[i + 5]}},
-        units::curvature_t{elements[i + 6]}});
-  }
-
-  return frc::Trajectory(states);
-}
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    discreteAlgebraicRiccatiEquation
- * Signature: ([D[D[D[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation
-  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
-   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
-{
-  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
-  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
-  jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
-  jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{nativeA, states, states};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Bmat{nativeB, states, inputs};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Qmat{nativeQ, states, states};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Rmat{nativeR, inputs, inputs};
-
-  try {
-    Eigen::MatrixXd result =
-        drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
-
-    env->ReleaseDoubleArrayElements(A, nativeA, 0);
-    env->ReleaseDoubleArrayElements(B, nativeB, 0);
-    env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
-    env->ReleaseDoubleArrayElements(R, nativeR, 0);
-
-    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
-  } catch (const std::runtime_error& e) {
-    jclass cls = env->FindClass("java/lang/RuntimeException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    exp
- * Signature: ([DI[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_exp
-  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
-{
-  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{arrayBody, rows, rows};
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
-      Amat.exp();
-
-  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
-  env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    pow
- * Signature: ([DID[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_pow
-  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
-   jdoubleArray dst)
-{
-  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{arrayBody, rows, rows};  // NOLINT
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
-      Amat.pow(exponent);
-
-  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
-  env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    isStabilizable
- * Signature: (II[D[D)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
-  (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
-   jdoubleArray bSrc)
-{
-  jdouble* nativeA = env->GetDoubleArrayElements(aSrc, nullptr);
-  jdouble* nativeB = env->GetDoubleArrayElements(bSrc, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      A{nativeA, states, states};
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      B{nativeB, states, inputs};
-
-  bool isStabilizable = check_stabilizable(A, B);
-
-  env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
-  env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);
-
-  return isStabilizable;
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    fromPathweaverJson
- * Signature: (Ljava/lang/String;)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
-  (JNIEnv* env, jclass, jstring path)
-{
-  try {
-    auto trajectory =
-        frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
-    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
-    return MakeJDoubleArray(env, elements);
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass("java/io/IOException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    toPathweaverJson
- * Signature: ([DLjava/lang/String;)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
-  (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
-{
-  try {
-    auto trajectory =
-        CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
-    frc::TrajectoryUtil::ToPathweaverJson(trajectory,
-                                          JStringRef{env, path}.c_str());
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass("java/io/IOException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    deserializeTrajectory
- * Signature: (Ljava/lang/String;)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
-  (JNIEnv* env, jclass, jstring json)
-{
-  try {
-    auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
-        JStringRef{env, json}.c_str());
-    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
-    return MakeJDoubleArray(env, elements);
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass(
-        "edu/wpi/first/math/trajectory/TrajectoryUtil$"
-        "TrajectorySerializationException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    serializeTrajectory
- * Signature: ([D)Ljava/lang/String;
- */
-JNIEXPORT jstring JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
-  (JNIEnv* env, jclass, jdoubleArray elements)
-{
-  try {
-    auto trajectory =
-        CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
-    return MakeJString(env,
-                       frc::TrajectoryUtil::SerializeTrajectory(trajectory));
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass(
-        "edu/wpi/first/math/trajectory/TrajectoryUtil$"
-        "TrajectorySerializationException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    rankUpdate
- * Signature: ([DI[DDZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
-  (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
-   jdouble sigma, jboolean lowerTriangular)
-{
-  jdouble* matBody = env->GetDoubleArrayElements(mat, nullptr);
-  jdouble* vecBody = env->GetDoubleArrayElements(vec, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      L{matBody, rows, rows};
-  Eigen::Map<Eigen::Vector<double, Eigen::Dynamic>> v{vecBody, rows};
-
-  if (lowerTriangular == JNI_TRUE) {
-    Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
-  } else {
-    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
-  }
-
-  env->ReleaseDoubleArrayElements(mat, matBody, 0);
-  env->ReleaseDoubleArrayElements(vec, vecBody, 0);
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    solveFullPivHouseholderQr
- * Signature: ([DII[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
-  (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
-   jint Brows, jint Bcols, jdoubleArray dst)
-{
-  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
-  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{nativeA, Arows, Acols};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Bmat{nativeB, Brows, Bcols};
-
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
-      Amat.fullPivHouseholderQr().solve(Bmat);
-
-  env->ReleaseDoubleArrayElements(A, nativeA, 0);
-  env->ReleaseDoubleArrayElements(B, nativeB, 0);
-  env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
-}
-
-}  // extern "C"

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
new file mode 100644
index 0000000..e5c3ac5
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp

@@ -0,0 +1,177 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <stdexcept>
+
+#include <wpi/jni_util.h>
+
+#include "WPIMathJNI_Exceptions.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/DARE.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareDetailABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+
+  Eigen::MatrixXd RmatCopy{Rmat};
+  auto R_llt = RmatCopy.llt();
+
+  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+      Amat, Bmat, Qmat, R_llt);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareDetailABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+  JSpan<const jdouble> nativeN{env, N};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Nmat{nativeN.data(), states, inputs};
+
+  Eigen::MatrixXd Rcopy{Rmat};
+  auto R_llt = Rcopy.llt();
+
+  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+      Amat, Bmat, Qmat, R_llt, Nmat);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareABQR
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+
+  try {
+    Eigen::MatrixXd result =
+        frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
+
+    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+  } catch (const std::invalid_argument& e) {
+    illegalArgEx.Throw(env, e.what());
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+  JSpan<const jdouble> nativeN{env, N};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Nmat{nativeN.data(), states, inputs};
+
+  try {
+    Eigen::MatrixXd result =
+        frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
+
+    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+  } catch (const std::invalid_argument& e) {
+    illegalArgEx.Throw(env, e.what());
+  }
+}
+
+}  // extern "C"

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
new file mode 100644
index 0000000..642a2ba
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp

@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <unsupported/Eigen/MatrixFunctions>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    exp
+ * Signature: ([DI[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_exp
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
+{
+  JSpan<const jdouble> arrayBody{env, src};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{arrayBody.data(), rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
+      Amat.exp();
+
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    pow
+ * Signature: ([DID[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_pow
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
+   jdoubleArray dst)
+{
+  JSpan<const jdouble> arrayBody{env, src};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{arrayBody.data(), rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
+      Amat.pow(exponent);
+
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    rankUpdate
+ * Signature: ([DI[DDZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
+  (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
+   jdouble sigma, jboolean lowerTriangular)
+{
+  JSpan<jdouble> matBody{env, mat};
+  JSpan<const jdouble> vecBody{env, vec};
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      L{matBody.data(), rows, rows};
+  Eigen::Map<const Eigen::Vector<double, Eigen::Dynamic>> v{vecBody.data(),
+                                                            rows};
+
+  if (lowerTriangular == JNI_TRUE) {
+    Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
+  } else {
+    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    solveFullPivHouseholderQr
+ * Signature: ([DII[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
+  (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
+   jint Brows, jint Bcols, jdoubleArray dst)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), Arows, Acols};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), Brows, Bcols};
+
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
+      Amat.fullPivHouseholderQr().solve(Bmat);
+
+  env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
+}
+
+}  // extern "C"

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
new file mode 100644
index 0000000..7f5a03f
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "WPIMathJNI_Exceptions.h"
+
+#include <jni.h>
+
+using namespace wpi::java;
+
+//
+// Globals and load/unload
+//
+
+JException illegalArgEx;
+JException ioEx;
+JException trajectorySerializationEx;
+
+static const JExceptionInit exceptions[] = {
+    {"java/lang/IllegalArgumentException", &illegalArgEx},
+    {"java/io/IOException", &ioEx},
+    {"edu/wpi/first/math/trajectory/"
+     "TrajectoryUtil$TrajectorySerializationException",
+     &trajectorySerializationEx}};
+
+extern "C" {
+
+JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return JNI_ERR;
+  }
+
+  // Cache references to exceptions
+  for (auto& c : exceptions) {
+    *c.cls = JException(env, c.name);
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
+  }
+
+  return JNI_VERSION_1_6;
+}
+
+JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return;
+  }
+  // Delete global references
+  for (auto& c : exceptions) {
+    c.cls->free(env);
+  }
+}
+
+}  // extern "C"

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h
new file mode 100644
index 0000000..cf0e149
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h

@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/jni_util.h>
+
+extern wpi::java::JException illegalArgEx;
+extern wpi::java::JException ioEx;
+extern wpi::java::JException trajectorySerializationEx;

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
new file mode 100644
index 0000000..f985231
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp

@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/geometry/Pose3d.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    expPose3d
+ * Signature: (DDDDDDDDDDDDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_expPose3d
+  (JNIEnv* env, jclass, jdouble poseX, jdouble poseY, jdouble poseZ,
+   jdouble poseQw, jdouble poseQx, jdouble poseQy, jdouble poseQz,
+   jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx,
+   jdouble twistRy, jdouble twistRz)
+{
+  frc::Pose3d pose{
+      units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ},
+      frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}};
+  frc::Twist3d twist{units::meter_t{twistDx},  units::meter_t{twistDy},
+                     units::meter_t{twistDz},  units::radian_t{twistRx},
+                     units::radian_t{twistRy}, units::radian_t{twistRz}};
+
+  frc::Pose3d result = pose.Exp(twist);
+
+  const auto& resultQuaternion = result.Rotation().GetQuaternion();
+  return MakeJDoubleArray(
+      env, {{result.X().value(), result.Y().value(), result.Z().value(),
+             resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(),
+             resultQuaternion.Z()}});
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    logPose3d
+ * Signature: (DDDDDDDDDDDDDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_logPose3d
+  (JNIEnv* env, jclass, jdouble startX, jdouble startY, jdouble startZ,
+   jdouble startQw, jdouble startQx, jdouble startQy, jdouble startQz,
+   jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx,
+   jdouble endQy, jdouble endQz)
+{
+  frc::Pose3d startPose{
+      units::meter_t{startX}, units::meter_t{startY}, units::meter_t{startZ},
+      frc::Rotation3d{frc::Quaternion{startQw, startQx, startQy, startQz}}};
+  frc::Pose3d endPose{
+      units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ},
+      frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}};
+
+  frc::Twist3d result = startPose.Log(endPose);
+
+  return MakeJDoubleArray(
+      env, {{result.dx.value(), result.dy.value(), result.dz.value(),
+             result.rx.value(), result.ry.value(), result.rz.value()}});
+}
+
+}  // extern "C"

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
new file mode 100644
index 0000000..dc3dfdd
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <Eigen/Core>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/StateSpaceUtil.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    isStabilizable
+ * Signature: (II[D[D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
+  (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
+   jdoubleArray bSrc)
+{
+  JSpan<const jdouble> nativeA{env, aSrc};
+  JSpan<const jdouble> nativeB{env, bSrc};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      A{nativeA.data(), states, states};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      B{nativeB.data(), states, inputs};
+
+  bool isStabilizable =
+      frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
+
+  return isStabilizable;
+}
+
+}  // extern "C"

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
new file mode 100644
index 0000000..3359da9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp

@@ -0,0 +1,138 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <exception>
+
+#include <wpi/jni_util.h>
+
+#include "WPIMathJNI_Exceptions.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/trajectory/TrajectoryUtil.h"
+
+using namespace wpi::java;
+
+std::vector<double> GetElementsFromTrajectory(
+    const frc::Trajectory& trajectory) {
+  std::vector<double> elements;
+  elements.reserve(trajectory.States().size() * 7);
+
+  for (auto&& state : trajectory.States()) {
+    elements.push_back(state.t.value());
+    elements.push_back(state.velocity.value());
+    elements.push_back(state.acceleration.value());
+    elements.push_back(state.pose.X().value());
+    elements.push_back(state.pose.Y().value());
+    elements.push_back(state.pose.Rotation().Radians().value());
+    elements.push_back(state.curvature.value());
+  }
+
+  return elements;
+}
+
+frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
+  // Make sure that the elements have the correct length.
+  assert(elements.size() % 7 == 0);
+
+  // Create a vector of states from the elements.
+  std::vector<frc::Trajectory::State> states;
+  states.reserve(elements.size() / 7);
+
+  for (size_t i = 0; i < elements.size(); i += 7) {
+    states.emplace_back(frc::Trajectory::State{
+        units::second_t{elements[i]},
+        units::meters_per_second_t{elements[i + 1]},
+        units::meters_per_second_squared_t{elements[i + 2]},
+        frc::Pose2d{units::meter_t{elements[i + 3]},
+                    units::meter_t{elements[i + 4]},
+                    units::radian_t{elements[i + 5]}},
+        units::curvature_t{elements[i + 6]}});
+  }
+
+  return frc::Trajectory(states);
+}
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    fromPathweaverJson
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
+  (JNIEnv* env, jclass, jstring path)
+{
+  try {
+    auto trajectory =
+        frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    ioEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    toPathweaverJson
+ * Signature: ([DLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
+  (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
+    frc::TrajectoryUtil::ToPathweaverJson(trajectory,
+                                          JStringRef{env, path}.c_str());
+  } catch (std::exception& e) {
+    ioEx.Throw(env, e.what());
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    deserializeTrajectory
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
+  (JNIEnv* env, jclass, jstring json)
+{
+  try {
+    auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
+        JStringRef{env, json}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    trajectorySerializationEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    serializeTrajectory
+ * Signature: ([D)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
+  (JNIEnv* env, jclass, jdoubleArray elements)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
+    return MakeJString(env,
+                       frc::TrajectoryUtil::SerializeTrajectory(trajectory));
+  } catch (std::exception& e) {
+    trajectorySerializationEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+}  // extern "C"

diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 1ff7a8a..346a232 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp

@@ -11,32 +11,9 @@
 DifferentialDriveOdometry::DifferentialDriveOdometry(
     const Rotation2d& gyroAngle, units::meter_t leftDistance,
     units::meter_t rightDistance, const Pose2d& initialPose)
-    : m_pose(initialPose),
-      m_prevLeftDistance(leftDistance),
-      m_prevRightDistance(rightDistance) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    : Odometry<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>(
+          m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
+          initialPose) {
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
 }
-
-const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
-                                                units::meter_t leftDistance,
-                                                units::meter_t rightDistance) {
-  auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
-  auto deltaRightDistance = rightDistance - m_prevRightDistance;
-
-  m_prevLeftDistance = leftDistance;
-  m_prevRightDistance = rightDistance;
-
-  auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto newPose = m_pose.Exp(
-      {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}

diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index 298dd7f..c21a7b2 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp

@@ -25,7 +25,7 @@
                                       chassisSpeeds.vy.value(),
                                       chassisSpeeds.omega.value()};
 
-  Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector;
+  Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector;
 
   MecanumDriveWheelSpeeds wheelSpeeds;
   wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
@@ -37,7 +37,7 @@
 
 ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
     const MecanumDriveWheelSpeeds& wheelSpeeds) const {
-  Vectord<4> wheelSpeedsVector{
+  Eigen::Vector4d wheelSpeedsVector{
       wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
       wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
 
@@ -50,15 +50,30 @@
 }
 
 Twist2d MecanumDriveKinematics::ToTwist2d(
+    const MecanumDriveWheelPositions& start,
+    const MecanumDriveWheelPositions& end) const {
+  Eigen::Vector4d wheelDeltasVector{
+      end.frontLeft.value() - start.frontLeft.value(),
+      end.frontRight.value() - start.frontRight.value(),
+      end.rearLeft.value() - start.rearLeft.value(),
+      end.rearRight.value() - start.rearRight.value()};
+
+  Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
+
+  return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
+          units::radian_t{twistVector(2)}};
+}
+
+Twist2d MecanumDriveKinematics::ToTwist2d(
     const MecanumDriveWheelPositions& wheelDeltas) const {
-  Vectord<4> wheelDeltasVector{
+  Eigen::Vector4d wheelDeltasVector{
       wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
       wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()};
 
   Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
 
-  return {units::meter_t{twistVector(0)},  // NOLINT
-          units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
+  return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
+          units::radian_t{twistVector(2)}};
 }
 
 void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,

diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 394adaf..55055be 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp

@@ -11,35 +11,9 @@
 MecanumDriveOdometry::MecanumDriveOdometry(
     MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
     const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
-    : m_kinematics(kinematics),
-      m_pose(initialPose),
-      m_previousWheelPositions(wheelPositions) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    : Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
+          m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
+      m_kinematicsImpl(kinematics) {
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
 }
-
-const Pose2d& MecanumDriveOdometry::Update(
-    const Rotation2d& gyroAngle,
-    const MecanumDriveWheelPositions& wheelPositions) {
-  auto angle = gyroAngle + m_gyroOffset;
-
-  MecanumDriveWheelPositions wheelDeltas{
-      wheelPositions.frontLeft - m_previousWheelPositions.frontLeft,
-      wheelPositions.frontRight - m_previousWheelPositions.frontRight,
-      wheelPositions.rearLeft - m_previousWheelPositions.rearLeft,
-      wheelPositions.rearRight - m_previousWheelPositions.rearRight,
-  };
-
-  auto twist = m_kinematics.ToTwist2d(wheelDeltas);
-  twist.dtheta = (angle - m_previousAngle).Radians();
-
-  auto newPose = m_pose.Exp(twist);
-
-  m_previousAngle = angle;
-  m_previousWheelPositions = wheelPositions;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}

diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp
new file mode 100644
index 0000000..5343de9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+#include "frc/kinematics/SwerveModuleState.h"
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModulePosition::operator==(const SwerveModulePosition& other) const {
+  return units::math::abs(distance - other.distance) < 1E-9_m &&
+         angle == other.angle;
+}

diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp
new file mode 100644
index 0000000..071d53a
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/SwerveModuleState.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModuleState::operator==(const SwerveModuleState& other) const {
+  return units::math::abs(speed - other.speed) < 1E-9_mps &&
+         angle == other.angle;
+}
+
+SwerveModuleState SwerveModuleState::Optimize(
+    const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
+  auto delta = desiredState.angle - currentAngle;
+  if (units::math::abs(delta.Degrees()) > 90_deg) {
+    return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
+  } else {
+    return {desiredState.speed, desiredState.angle};
+  }
+}

diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index de47547..5393f83 100644
--- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp

@@ -5,6 +5,7 @@
 #include "frc/trajectory/Trajectory.h"
 
 #include <algorithm>
+#include <stdexcept>
 
 #include <wpi/MathExtras.h>
 #include <wpi/json.h>
@@ -54,10 +55,20 @@
 }
 
 Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
+  if (m_states.empty()) {
+    throw std::invalid_argument(
+        "Trajectory manually initialized with no states.");
+  }
+
   m_totalTime = states.back().t;
 }
 
 Trajectory::State Trajectory::Sample(units::second_t t) const {
+  if (m_states.empty()) {
+    throw std::runtime_error(
+        "Trajectory cannot be sampled if it has no states.");
+  }
+
   if (t <= m_states.front().t) {
     return m_states.front();
   }

diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
index 92d52ed..4c0a55e 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp

@@ -223,10 +223,9 @@
 
     if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) {
       throw std::runtime_error(
-          "The constraint's min acceleration was greater than its max "
-          "acceleration. To debug this, remove all constraints from the config "
-          "and add each one individually. If the offending constraint was "
-          "packaged with WPILib, please file a bug report.");
+          "There was an infeasible trajectory constraint. To determine which "
+          "one, remove all constraints from the TrajectoryConfig and add them "
+          "back one-by-one.");
     }
 
     state->minAcceleration = units::math::max(

diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
index 169b642..da9c955 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp

@@ -7,9 +7,8 @@
 #include <system_error>
 
 #include <fmt/format.h>
-#include <wpi/SmallString.h>
+#include <wpi/MemoryBuffer.h>
 #include <wpi/json.h>
-#include <wpi/raw_istream.h>
 #include <wpi/raw_ostream.h>
 
 using namespace frc;
@@ -29,15 +28,14 @@
 }
 
 Trajectory TrajectoryUtil::FromPathweaverJson(std::string_view path) {
-  std::error_code error_code;
-
-  wpi::raw_fd_istream input{path, error_code};
-  if (error_code) {
+  std::error_code ec;
+  std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+      wpi::MemoryBuffer::GetFile(path, ec);
+  if (fileBuffer == nullptr || ec) {
     throw std::runtime_error(fmt::format("Cannot open file: {}", path));
   }
 
-  wpi::json json;
-  input >> json;
+  wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
 
   return Trajectory{json.get<std::vector<Trajectory::State>>()};
 }

diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
index 2a308db..460ff8b 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp

@@ -7,9 +7,8 @@
 using namespace frc;
 
 DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
-    const DifferentialDriveKinematics& kinematics,
-    units::meters_per_second_t maxSpeed)
-    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+    DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+    : m_kinematics(std::move(kinematics)), m_maxSpeed(maxSpeed) {}
 
 units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,

diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
index d60b4b6..46c306e 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp

@@ -15,9 +15,9 @@
 
 DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
     const SimpleMotorFeedforward<units::meter>& feedforward,
-    const DifferentialDriveKinematics& kinematics, units::volt_t maxVoltage)
+    DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
     : m_feedforward(feedforward),
-      m_kinematics(kinematics),
+      m_kinematics(std::move(kinematics)),
       m_maxVoltage(maxVoltage) {}
 
 units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(

diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h
new file mode 100644
index 0000000..6a3104e
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/DARE.h

@@ -0,0 +1,406 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdexcept>
+#include <string>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/fmt/Eigen.h"
+
+// Works cited:
+//
+// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
+//     Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
+//     International Journal of Control, 77:8, 767-788, 2004.
+//     DOI: 10.1080/00207170410001714988
+
+namespace frc {
+
+namespace detail {
+
+/**
+ * Checks the preconditions of A, B, and Q for the DARE solver.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
+ * @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
+ * @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
+ *   detectable.
+ */
+template <int States, int Inputs>
+void CheckDARE_ABQ(const Eigen::Matrix<double, States, States>& A,
+                   const Eigen::Matrix<double, States, Inputs>& B,
+                   const Eigen::Matrix<double, States, States>& Q) {
+  // Require Q be symmetric
+  if ((Q - Q.transpose()).norm() > 1e-10) {
+    std::string msg = fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Q);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require Q be positive semidefinite
+  //
+  // If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
+  // positive, negative, and zero diagonal entries in D equals the number of
+  // positive, negative, and zero eigenvalues respectively in Q (see
+  // https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
+  //
+  // Therefore, D having no negative diagonal entries is sufficient to prove Q
+  // is positive semidefinite.
+  auto Q_ldlt = Q.ldlt();
+  if (Q_ldlt.info() != Eigen::Success ||
+      (Q_ldlt.vectorD().array() < 0.0).any()) {
+    std::string msg =
+        fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n", Q);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require (A, B) pair be stabilizable
+  if (!IsStabilizable<States, Inputs>(A, B)) {
+    std::string msg = fmt::format(
+        "The (A, B) pair isn't stabilizable!\n\nA =\n{}\nB =\n{}\n", A, B);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require (A, C) pair be detectable where Q = CᵀC
+  //
+  // Q = CᵀC = PᵀLDLᵀP
+  // Cᵀ = PᵀL√(D)
+  // C = (PᵀL√(D))ᵀ
+  {
+    Eigen::Matrix<double, States, States> C =
+        (Q_ldlt.transpositionsP().transpose() *
+         Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
+         Q_ldlt.vectorD().cwiseSqrt().asDiagonal())
+            .transpose();
+
+    if (!IsDetectable<States, States>(A, C)) {
+      std::string msg = fmt::format(
+          "The (A, C) pair where Q = CᵀC isn't detectable!\n\nA =\n{}\nQ "
+          "=\n{}\n",
+          A, Q);
+      throw std::invalid_argument(msg);
+    }
+  }
+}
+
+/**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic
+ * Riccati equation:
+ *
+ *   AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * This internal function skips expensive precondition checks for increased
+ * performance. The solver may hang if any of the following occur:
+ * <ul>
+ *   <li>Q isn't symmetric positive semidefinite</li>
+ *   <li>R isn't symmetric positive definite</li>
+ *   <li>The (A, B) pair isn't stabilizable</li>
+ *   <li>The (A, C) pair where Q = CᵀC isn't detectable</li>
+ * </ul>
+ * Only use this function if you're sure the preconditions are met.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @param R_llt The LLT decomposition of the input cost matrix.
+ */
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt) {
+  using StateMatrix = Eigen::Matrix<double, States, States>;
+
+  // Implements the SDA algorithm on page 5 of [1].
+
+  // A₀ = A
+  StateMatrix A_k = A;
+
+  // G₀ = BR⁻¹Bᵀ
+  //
+  // See equation (4) of [1].
+  StateMatrix G_k = B * R_llt.solve(B.transpose());
+
+  // H₀ = Q
+  //
+  // See equation (4) of [1].
+  StateMatrix H_k;
+  StateMatrix H_k1 = Q;
+
+  do {
+    H_k = H_k1;
+
+    // W = I + GₖHₖ
+    StateMatrix W = StateMatrix::Identity(H_k.rows(), H_k.cols()) + G_k * H_k;
+
+    auto W_solver = W.lu();
+
+    // Solve WV₁ = Aₖ for V₁
+    StateMatrix V_1 = W_solver.solve(A_k);
+
+    // Solve V₂Wᵀ = Gₖ for V₂
+    //
+    // We want to put V₂Wᵀ = Gₖ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // V₂Wᵀ = Gₖ
+    // (V₂Wᵀ)ᵀ = Gₖᵀ
+    // WV₂ᵀ = Gₖᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // V₂ᵀ = W.solve(Gₖᵀ)
+    // V₂ = W.solve(Gₖᵀ)ᵀ
+    StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
+
+    // Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
+    G_k += A_k * V_2 * A_k.transpose();
+
+    // Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ
+    H_k1 = H_k + V_1.transpose() * H_k * A_k;
+
+    // Aₖ₊₁ = AₖV₁
+    A_k *= V_1;
+
+    // while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁|
+  } while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());
+
+  return H_k1;
+}
+
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+  AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+
+This is equivalent to solving the original DARE:
+
+  A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+
+where A₂ and Q₂ are a change of variables:
+
+  A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+
+This overload of the DARE is useful for finding the control law uₖ that
+minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q  N][xₖ]
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This is a more general form of the following. The linear-quadratic regulator
+is the feedback control law uₖ that minimizes the following cost function
+subject to xₖ₊₁ = Axₖ + Buₖ:
+
+@verbatim
+    ∞
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   k=0
+@endverbatim
+
+This can be refactored as:
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q 0][xₖ]
+J = Σ [uₖ] [0 R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This internal function skips expensive precondition checks for increased
+performance. The solver may hang if any of the following occur:
+<ul>
+  <li>Q₂ isn't symmetric positive semidefinite</li>
+  <li>R isn't symmetric positive definite</li>
+  <li>The (A₂, B) pair isn't stabilizable</li>
+  <li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable</li>
+</ul>
+Only use this function if you're sure the preconditions are met.
+
+@tparam States Number of states.
+@tparam Inputs Number of inputs.
+@param A The system matrix.
+@param B The input matrix.
+@param Q The state cost matrix.
+@param R_llt The LLT decomposition of the input cost matrix.
+@param N The state-input cross cost matrix.
+*/
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt,
+    const Eigen::Matrix<double, Inputs, Inputs>& N) {
+  // This is a change of variables to make the DARE that includes Q, R, and N
+  // cost matrices fit the form of the DARE that includes only Q and R cost
+  // matrices.
+  //
+  // This is equivalent to solving the original DARE:
+  //
+  //   A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+  //
+  // where A₂ and Q₂ are a change of variables:
+  //
+  //   A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+  return detail::DARE<States, Inputs>(A - B * R_llt.solve(N.transpose()), B,
+                                      Q - N * R_llt.solve(N.transpose()),
+                                      R_llt);
+}
+
+}  // namespace detail
+
+/**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic
+ * Riccati equation:
+ *
+ *   AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @param R The input cost matrix.
+ * @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
+ * @throws std::invalid_argument if R isn't symmetric positive definite.
+ * @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
+ * @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
+ *   detectable.
+ */
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::Matrix<double, Inputs, Inputs>& R) {
+  // Require R be symmetric
+  if ((R - R.transpose()).norm() > 1e-10) {
+    std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require R be positive definite
+  auto R_llt = R.llt();
+  if (R_llt.info() != Eigen::Success) {
+    std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
+    throw std::invalid_argument(msg);
+  }
+
+  detail::CheckDARE_ABQ<States, Inputs>(A, B, Q);
+
+  return detail::DARE<States, Inputs>(A, B, Q, R_llt);
+}
+
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+  AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+
+This is equivalent to solving the original DARE:
+
+  A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+
+where A₂ and Q₂ are a change of variables:
+
+  A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+
+This overload of the DARE is useful for finding the control law uₖ that
+minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q  N][xₖ]
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This is a more general form of the following. The linear-quadratic regulator
+is the feedback control law uₖ that minimizes the following cost function
+subject to xₖ₊₁ = Axₖ + Buₖ:
+
+@verbatim
+    ∞
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   k=0
+@endverbatim
+
+This can be refactored as:
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q 0][xₖ]
+J = Σ [uₖ] [0 R][uₖ] ΔT
+   k=0
+@endverbatim
+
+@tparam States Number of states.
+@tparam Inputs Number of inputs.
+@param A The system matrix.
+@param B The input matrix.
+@param Q The state cost matrix.
+@param R The input cost matrix.
+@param N The state-input cross cost matrix.
+@throws std::invalid_argument if Q₂ isn't symmetric positive semidefinite.
+@throws std::invalid_argument if R isn't symmetric positive definite.
+@throws std::invalid_argument if the (A₂, B) pair isn't stabilizable.
+@throws std::invalid_argument if the (A₂, C) pair where Q₂ = CᵀC isn't
+  detectable.
+*/
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::Matrix<double, Inputs, Inputs>& R,
+    const Eigen::Matrix<double, States, Inputs>& N) {
+  // Require R be symmetric
+  if ((R - R.transpose()).norm() > 1e-10) {
+    std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require R be positive definite
+  auto R_llt = R.llt();
+  if (R_llt.info() != Eigen::Success) {
+    std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
+    throw std::invalid_argument(msg);
+  }
+
+  // This is a change of variables to make the DARE that includes Q, R, and N
+  // cost matrices fit the form of the DARE that includes only Q and R cost
+  // matrices.
+  //
+  // This is equivalent to solving the original DARE:
+  //
+  //   A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+  //
+  // where A₂ and Q₂ are a change of variables:
+  //
+  //   A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+  Eigen::Matrix<double, States, States> A_2 =
+      A - B * R_llt.solve(N.transpose());
+  Eigen::Matrix<double, States, States> Q_2 =
+      Q - N * R_llt.solve(N.transpose());
+
+  detail::CheckDARE_ABQ<States, Inputs>(A_2, B, Q_2);
+
+  return detail::DARE<States, Inputs>(A_2, B, Q_2, R_llt);
+}
+
+}  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/EigenCore.h b/wpimath/src/main/native/include/frc/EigenCore.h
index b33e9e2..1604a0d 100644
--- a/wpimath/src/main/native/include/frc/EigenCore.h
+++ b/wpimath/src/main/native/include/frc/EigenCore.h

@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include "Eigen/Core"
+#include <Eigen/Core>
 
 namespace frc {
 

diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h
index 24bf857..26f106e 100644
--- a/wpimath/src/main/native/include/frc/MathUtil.h
+++ b/wpimath/src/main/native/include/frc/MathUtil.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <numbers>
+#include <type_traits>
 
 #include <wpi/SymbolExports.h>
 
@@ -25,12 +26,11 @@
  * be infinite.
  * @return The value after the deadband is applied.
  */
-template <typename T,
-          typename = std::enable_if_t<std::disjunction_v<
-              std::is_floating_point<T>, units::traits::is_unit_t<T>>>>
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
 T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
   T magnitude;
-  if constexpr (std::is_floating_point_v<T>) {
+  if constexpr (std::is_arithmetic_v<T>) {
     magnitude = std::abs(value);
   } else {
     magnitude = units::math::abs(value);
@@ -106,6 +106,58 @@
 }
 
 /**
+ * Checks if the given value matches an expected value within a certain
+ * tolerance.
+ *
+ * @param expected The expected value
+ * @param actual The actual value
+ * @param tolerance The allowed difference between the actual and the expected
+ * value
+ * @return Whether or not the actual value is within the allowed tolerance
+ */
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr bool IsNear(T expected, T actual, T tolerance) {
+  if constexpr (std::is_arithmetic_v<T>) {
+    return std::abs(expected - actual) < tolerance;
+  } else {
+    return units::math::abs(expected - actual) < tolerance;
+  }
+}
+
+/**
+ * Checks if the given value matches an expected value within a certain
+ * tolerance. Supports continuous input for cases like absolute encoders.
+ *
+ * Continuous input means that the min and max value are considered to be the
+ * same point, and tolerances can be checked across them. A common example
+ * would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns
+ * true because 359 is 1 away from 360 (which is treated as the same as 0) and
+ * 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
+ * given tolerance of 5.
+ *
+ * @param expected The expected value
+ * @param actual The actual value
+ * @param tolerance The allowed difference between the actual and the expected
+ * value
+ * @param min Smallest value before wrapping around to the largest value
+ * @param max Largest value before wrapping around to the smallest value
+ * @return Whether or not the actual value is within the allowed tolerance
+ */
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) {
+  T errorBound = (max - min) / 2.0;
+  T error = frc::InputModulus<T>(expected - actual, -errorBound, errorBound);
+
+  if constexpr (std::is_arithmetic_v<T>) {
+    return std::abs(error) < tolerance;
+  } else {
+    return units::math::abs(error) < tolerance;
+  }
+}
+
+/**
  * Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
  *
  * @param angle Angle to wrap.

diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 0345b46..3aa2e75 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h

@@ -6,31 +6,21 @@
 
 #include <array>
 #include <cmath>
+#include <concepts>
 #include <limits>
 #include <random>
-#include <type_traits>
 
+#include <Eigen/Eigenvalues>
+#include <Eigen/QR>
 #include <wpi/SymbolExports.h>
 #include <wpi/deprecated.h>
 
-#include "Eigen/Eigenvalues"
-#include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "frc/geometry/Pose2d.h"
 
 namespace frc {
 namespace detail {
 
-template <int Rows, int Cols, typename Matrix, typename T, typename... Ts>
-void MatrixImpl(Matrix& result, T elem, Ts... elems) {
-  constexpr int count = Rows * Cols - (sizeof...(Ts) + 1);
-
-  result(count / Cols, count % Cols) = elem;
-  if constexpr (sizeof...(Ts) > 0) {
-    MatrixImpl<Rows, Cols>(result, elems...);
-  }
-}
-
 template <typename Matrix, typename T, typename... Ts>
 void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
   if (elem == std::numeric_limits<double>::infinity()) {
@@ -66,26 +56,37 @@
 template <int States, int Inputs>
 bool IsStabilizableImpl(const Matrixd<States, States>& A,
                         const Matrixd<States, Inputs>& B) {
-  Eigen::EigenSolver<Matrixd<States, States>> es{A};
+  Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
 
-  for (int i = 0; i < States; ++i) {
-    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
-            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
-        1) {
+  for (int i = 0; i < A.rows(); ++i) {
+    if (std::norm(es.eigenvalues()[i]) < 1) {
       continue;
     }
 
-    Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
-    E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
-                                             States>::Identity() -
-             A,
-        B;
+    if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
+      Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
+      E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
+                                               States>::Identity() -
+               A,
+          B;
 
-    Eigen::ColPivHouseholderQR<
-        Eigen::Matrix<std::complex<double>, States, States + Inputs>>
-        qr{E};
-    if (qr.rank() < States) {
-      return false;
+      Eigen::ColPivHouseholderQR<
+          Eigen::Matrix<std::complex<double>, States, States + Inputs>>
+          qr{E};
+      if (qr.rank() < States) {
+        return false;
+      }
+    } else {
+      Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
+      E << es.eigenvalues()[i] *
+                   Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
+               A,
+          B;
+
+      Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
+      if (qr.rank() < A.rows()) {
+        return false;
+      }
     }
   }
   return true;
@@ -106,8 +107,7 @@
  *                   of the control inputs from no actuation.
  * @return State excursion or control effort cost matrix.
  */
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
   detail::CostMatrixImpl(result.diagonal(), tolerances...);
@@ -126,8 +126,7 @@
  *                output measurement.
  * @return Process noise or measurement noise covariance matrix.
  */
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
   detail::CovMatrixImpl(result.diagonal(), stdDevs...);
@@ -138,7 +137,8 @@
  * Creates a cost matrix from the given vector for use with LQR.
  *
  * The cost matrix is constructed using Bryson's rule. The inverse square of
- * each element in the input is taken and placed on the cost matrix diagonal.
+ * each element in the input is placed on the cost matrix diagonal. If a
+ * tolerance is infinity, its cost matrix entry is set to zero.
  *
  * @param costs An array. For a Q matrix, its elements are the maximum allowed
  *              excursions of the states from the reference. For an R matrix,
@@ -151,7 +151,11 @@
   Eigen::DiagonalMatrix<double, N> result;
   auto& diag = result.diagonal();
   for (size_t i = 0; i < N; ++i) {
-    diag(i) = 1.0 / std::pow(costs[i], 2);
+    if (costs[i] == std::numeric_limits<double>::infinity()) {
+      diag(i) = 0.0;
+    } else {
+      diag(i) = 1.0 / std::pow(costs[i], 2);
+    }
   }
   return result;
 }
@@ -178,8 +182,7 @@
   return result;
 }
 
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
   Matrixd<sizeof...(Ts), 1> result;
   detail::WhiteNoiseVectorImpl(result, stdDevs...);
@@ -221,7 +224,7 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Vectord<3> PoseTo3dVector(const Pose2d& pose);
+Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
 
 /**
  * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -231,14 +234,14 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Vectord<4> PoseTo4dVector(const Pose2d& pose);
+Eigen::Vector4d PoseTo4dVector(const Pose2d& pose);
 
 /**
  * Returns true if (A, B) is a stabilizable pair.
  *
  * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
  * any, have absolute values less than one, where an eigenvalue is
- * uncontrollable if rank(λI - A, B) < n where n is the number of states.
+ * uncontrollable if rank([λI - A, B]) < n where n is the number of states.
  *
  * @tparam States The number of states.
  * @tparam Inputs The number of inputs.
@@ -256,7 +259,7 @@
  *
  * (A, C) is detectable if and only if the unobservable eigenvalues of A, if
  * any, have absolute values less than one, where an eigenvalue is unobservable
- * if rank(λI - A; C) < n where n is the number of states.
+ * if rank([λI - A; C]) < n where n is the number of states.
  *
  * @tparam States The number of states.
  * @tparam Outputs The number of outputs.
@@ -282,6 +285,12 @@
 WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
                                            const Matrixd<2, 1>& B);
 
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+WPILIB_DLLEXPORT bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
+    const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
+
 /**
  * Converts a Pose2d into a vector of [x, y, theta].
  *
@@ -290,7 +299,7 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Vectord<3> PoseToVector(const Pose2d& pose);
+Eigen::Vector3d PoseToVector(const Pose2d& pose);
 
 /**
  * Clamps input vector between system's minimum and maximum allowable input.

diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
index 21aad25..6306457 100644
--- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h

@@ -7,7 +7,8 @@
 #include <array>
 #include <functional>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+
 #include "frc/EigenCore.h"
 #include "frc/system/NumericalJacobian.h"
 #include "units/time.h"
@@ -165,6 +166,9 @@
   InputVector Calculate(const StateVector& r, const StateVector& nextR) {
     StateVector rDot = (nextR - r) / m_dt.value();
 
+    // ṙ = f(r) + Bu
+    // Bu = ṙ − f(r)
+    // u = B⁺(ṙ − f(r))
     m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
 
     m_r = nextR;

diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
index 3a5148a..d84e3ce 100644
--- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h

@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 
-#include "Eigen/Core"
 #include "frc/controller/DifferentialDriveWheelVoltages.h"
 #include "frc/system/LinearSystem.h"
 #include "units/acceleration.h"

diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
index bbe7720..62a7bad 100644
--- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h

@@ -6,6 +6,9 @@
 
 #include <wpi/MathExtras.h>
 
+#include "frc/EigenCore.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
 #include "units/length.h"
 #include "units/time.h"
 #include "units/voltage.h"
@@ -54,6 +57,50 @@
     return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param currentVelocity The current velocity setpoint, in distance per
+   *                        second.
+   * @param nextVelocity    The next velocity setpoint, in distance per second.
+   * @param dt              Time between velocity setpoints in seconds.
+   * @return The computed feedforward, in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity,
+                          units::second_t dt) const {
+    // Discretize the affine model.
+    //
+    //   dx/dt = Ax + Bu + c
+    //   dx/dt = Ax + B(u + B⁺c)
+    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
+    //
+    // Solve for uₖ.
+    //
+    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+    //
+    // For an elevator with the model
+    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
+    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
+    // assuming sgn(x) is a constant for the duration of the step.
+    //
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
+    auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
+    LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
+
+    Vectord<1> r{currentVelocity.value()};
+    Vectord<1> nextR{nextVelocity.value()};
+
+    return kG + kS * wpi::sgn(currentVelocity.value()) +
+           units::volt_t{feedforward.Calculate(r, nextR)(0)};
+  }
+
   // Rearranging the main equation from the calculate() method yields the
   // formulas for the methods below:
 

diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
index 9a749c6..6f9568e 100644
--- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h

@@ -42,7 +42,7 @@
    * angle.
    */
   HolonomicDriveController(
-      frc2::PIDController xController, frc2::PIDController yController,
+      PIDController xController, PIDController yController,
       ProfiledPIDController<units::radian> thetaController);
 
   HolonomicDriveController(const HolonomicDriveController&) = default;
@@ -103,14 +103,29 @@
    */
   void SetEnabled(bool enabled);
 
+  /**
+   * Returns the rotation ProfiledPIDController
+   */
+  ProfiledPIDController<units::radian>& getThetaController();
+
+  /**
+   * Returns the X PIDController
+   */
+  PIDController& getXController();
+
+  /**
+   * Returns the Y PIDController
+   */
+  PIDController& getYController();
+
  private:
   Pose2d m_poseError;
   Rotation2d m_rotationError;
   Pose2d m_poseTolerance;
   bool m_enabled = true;
 
-  frc2::PIDController m_xController;
-  frc2::PIDController m_yController;
+  PIDController m_xController;
+  PIDController m_yController;
   ProfiledPIDController<units::radian> m_thetaController;
 
   bool m_firstRun = true;

diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
index eef1fc0..3a1230d 100644
--- a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
+++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h

@@ -6,7 +6,8 @@
 
 #include <frc/system/LinearSystem.h>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+
 #include "frc/EigenCore.h"
 #include "units/time.h"
 

diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
index 0a336aa..94f3fa3 100644
--- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h

@@ -23,9 +23,17 @@
 /**
  * The linear time-varying differential drive controller has a similar form to
  * the LQR, but the model used to compute the controller gain is the nonlinear
- * model linearized around the drivetrain's current state. We precomputed gains
- * for important places in our state-space, then interpolated between them with
- * a LUT to save computational resources.
+ * differential drive model linearized around the drivetrain's current state. We
+ * precompute gains for important places in our state-space, then interpolate
+ * between them with a lookup table to save computational resources.
+ *
+ * This controller has a flat hierarchy with pose and wheel velocity references
+ * and voltage outputs. This is different from a Ramsete controller's nested
+ * hierarchy where the top-level controller has a pose reference and chassis
+ * velocity command outputs, and the low-level controller has wheel velocity
+ * references and voltage outputs. Flat hierarchies are easier to tune in one
+ * shot. Furthermore, this controller is more optimal in the "least-squares
+ * error" sense than a controller based on Ramsete.
  *
  * See section 8.7 in Controls Engineering in FRC for a derivation of the
  * control law we used shown in theorem 8.7.4.
@@ -35,12 +43,18 @@
   /**
    * Constructs a linear time-varying differential drive controller.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param plant      The differential drive velocity plant.
    * @param trackwidth The distance between the differential drive's left and
    *                   right wheels.
    * @param Qelems     The maximum desired error tolerance for each state.
    * @param Relems     The maximum desired control effort for each input.
    * @param dt         Discretization timestep.
+   * @throws std::domain_error if max velocity of plant with 12 V input <= 0 m/s
+   *     or >= 15 m/s.
    */
   LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
                                  units::meter_t trackwidth,

diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
index 83cfe4b..38c4287 100644
--- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
+++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h

@@ -20,8 +20,11 @@
 
 /**
  * The linear time-varying unicycle controller has a similar form to the LQR,
- * but the model used to compute the controller gain is the nonlinear model
- * linearized around the drivetrain's current state.
+ * but the model used to compute the controller gain is the nonlinear unicycle
+ * model linearized around the drivetrain's current state.
+ *
+ * This controller is a roughly drop-in replacement for RamseteController with
+ * more optimal feedback gains in the "least-squares error" sense.
  *
  * See section 8.9 in Controls Engineering in FRC for a derivation of the
  * control law we used shown in theorem 8.9.1.
@@ -36,6 +39,7 @@
    * @param dt Discretization timestep.
    * @param maxVelocity The maximum velocity for the controller gain lookup
    *                    table.
+   * @throws std::domain_error if maxVelocity &lt;= 0.
    */
   explicit LTVUnicycleController(
       units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
@@ -43,11 +47,16 @@
   /**
    * Constructs a linear time-varying unicycle controller.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param Qelems The maximum desired error tolerance for each state.
    * @param Relems The maximum desired control effort for each input.
    * @param dt     Discretization timestep.
    * @param maxVelocity The maximum velocity for the controller gain lookup
    *                    table.
+   * @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s.
    */
   LTVUnicycleController(const wpi::array<double, 3>& Qelems,
                         const wpi::array<double, 2>& Relems, units::second_t dt,

diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
index d4cc3c4..1d905e2 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h

@@ -7,7 +7,8 @@
 #include <array>
 #include <functional>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+
 #include "frc/EigenCore.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/LinearSystem.h"
@@ -137,6 +138,9 @@
    * @return The calculated feedforward.
    */
   InputVector Calculate(const StateVector& r, const StateVector& nextR) {
+    // rₖ₊₁ = Arₖ + Buₖ
+    // Buₖ = rₖ₊₁ − Arₖ
+    // uₖ = B⁺(rₖ₊₁ − Arₖ)
     m_uff = m_B.householderQr().solve(nextR - (m_A * r));
     m_r = nextR;
     return m_uff;

diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index 50d6566..979e98a 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h

@@ -36,6 +36,10 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param plant  The plant being controlled.
    * @param Qelems The maximum desired error tolerance for each state.
    * @param Relems The maximum desired control effort for each input.
@@ -50,6 +54,10 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param A      Continuous system matrix of the plant being controlled.
    * @param B      Continuous input matrix of the plant being controlled.
    * @param Qelems The maximum desired error tolerance for each state.

diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
index 87d37ec..1871244 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc

@@ -7,14 +7,14 @@
 #include <stdexcept>
 #include <string>
 
-#include "Eigen/Cholesky"
-#include "Eigen/Eigenvalues"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <Eigen/Cholesky>
+#include <unsupported/Eigen/MatrixFunctions>
+
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "frc/fmt/Eigen.h"
 #include "frc/system/Discretization.h"
-#include "unsupported/Eigen/MatrixFunctions"
 #include "wpimath/MathShared.h"
 
 namespace frc {
@@ -52,8 +52,7 @@
     throw std::invalid_argument(msg);
   }
 
-  Matrixd<States, States> S =
-      drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+  Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R);
 
   // K = (BᵀSB + R)⁻¹BᵀSA
   m_K = (discB.transpose() * S * discB + R)
@@ -72,8 +71,7 @@
   Matrixd<States, Inputs> discB;
   DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
 
-  Matrixd<States, States> S =
-      drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+  Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R, N);
 
   // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
   m_K = (discB.transpose() * S * discB + R)

diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h
index d6a41d1..0d5b0a3 100644
--- a/wpimath/src/main/native/include/frc/controller/PIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/PIDController.h

@@ -13,7 +13,7 @@
 
 #include "units/time.h"
 
-namespace frc2 {
+namespace frc {
 
 /**
  * Implements a PID control loop.
@@ -74,6 +74,18 @@
   void SetD(double Kd);
 
   /**
+   * Sets the IZone range. When the absolute value of the position error is
+   * greater than IZone, the total accumulated error will reset to zero,
+   * disabling integral gain until the absolute value of the position error is
+   * less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral
+   * gain. Passing a value of infinity disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   */
+  void SetIZone(double iZone);
+
+  /**
    * Gets the proportional coefficient.
    *
    * @return proportional coefficient
@@ -95,6 +107,13 @@
   double GetD() const;
 
   /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  double GetIZone() const;
+
+  /**
    * Gets the period of this controller.
    *
    * @return The period of the controller.
@@ -221,6 +240,9 @@
   // Factor for "derivative" control
   double m_Kd;
 
+  // The error range where "integral" control applies
+  double m_iZone = std::numeric_limits<double>::infinity();
+
   // The period (in seconds) of the control loop running this controller
   units::second_t m_period;
 
@@ -252,12 +274,9 @@
 
   double m_setpoint = 0;
   double m_measurement = 0;
+
+  bool m_haveSetpoint = false;
+  bool m_haveMeasurement = false;
 };
 
-}  // namespace frc2
-
-namespace frc {
-
-using frc2::PIDController;
-
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index 8491118..8f211c6 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h

@@ -58,7 +58,9 @@
    */
   ProfiledPIDController(double Kp, double Ki, double Kd,
                         Constraints constraints, units::second_t period = 20_ms)
-      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+      : m_controller{Kp, Ki, Kd, period},
+        m_constraints{constraints},
+        m_profile{m_constraints} {
     int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
     wpi::math::MathSharedStore::ReportUsage(
         wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
@@ -107,6 +109,18 @@
   void SetD(double Kd) { m_controller.SetD(Kd); }
 
   /**
+   * Sets the IZone range. When the absolute value of the position error is
+   * greater than IZone, the total accumulated error will reset to zero,
+   * disabling integral gain until the absolute value of the position error is
+   * less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral
+   * gain. Passing a value of infinity disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   */
+  void SetIZone(double iZone) { m_controller.SetIZone(iZone); }
+
+  /**
    * Gets the proportional coefficient.
    *
    * @return proportional coefficient
@@ -128,6 +142,13 @@
   double GetD() const { return m_controller.GetD(); }
 
   /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  double GetIZone() const { return m_controller.GetIZone(); }
+
+  /**
    * Gets the period of this controller.
    *
    * @return The period of the controller.
@@ -183,7 +204,16 @@
    *
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
+  void SetConstraints(Constraints constraints) {
+    m_constraints = constraints;
+    m_profile = TrapezoidProfile<Distance>{m_constraints};
+  }
+
+  /**
+   * Get the velocity and acceleration constraints for this controller.
+   * @return Velocity and acceleration constraints.
+   */
+  Constraints GetConstraints() { return m_constraints; }
 
   /**
    * Returns the current setpoint of the ProfiledPIDController.
@@ -292,8 +322,7 @@
       m_setpoint.position = setpointMinDistance + measurement;
     }
 
-    frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
-    m_setpoint = profile.Calculate(GetPeriod());
+    m_setpoint = m_profile.Calculate(GetPeriod(), m_goal, m_setpoint);
     return m_controller.Calculate(measurement.value(),
                                   m_setpoint.position.value());
   }
@@ -372,17 +401,22 @@
     builder.AddDoubleProperty(
         "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
     builder.AddDoubleProperty(
+        "izone", [this] { return GetIZone(); },
+        [this](double value) { SetIZone(value); });
+    builder.AddDoubleProperty(
         "goal", [this] { return GetGoal().position.value(); },
         [this](double value) { SetGoal(Distance_t{value}); });
   }
 
  private:
-  frc2::PIDController m_controller;
+  PIDController m_controller;
   Distance_t m_minimumInput{0};
   Distance_t m_maximumInput{0};
+
+  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
+  TrapezoidProfile<Distance> m_profile;
   typename frc::TrapezoidProfile<Distance>::State m_goal;
   typename frc::TrapezoidProfile<Distance>::State m_setpoint;
-  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
 };
 
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
index 0512c68c..026cc67 100644
--- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <functional>
 #include <numbers>
 
 #include "frc/EigenCore.h"

diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index 339ccc9..89dcd35 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h

@@ -7,13 +7,11 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
-#include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/kinematics/DifferentialDriveOdometry.h"
-#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
 #include "units/time.h"
 
 namespace frc {
@@ -32,7 +30,9 @@
  * AddVisionMeasurement() can be called as infrequently as you want; if you
  * never call it, then this class will behave like regular encoder odometry.
  */
-class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
+class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
+    : public PoseEstimator<DifferentialDriveWheelSpeeds,
+                           DifferentialDriveWheelPositions> {
  public:
   /**
    * Constructs a DifferentialDrivePoseEstimator with default standard
@@ -80,19 +80,6 @@
       const wpi::array<double, 3>& visionMeasurementStdDevs);
 
   /**
-   * Sets the pose estimator's trust in vision measurements. This might be used
-   * to change trust in vision measurements after the autonomous period, or to
-   * change trust as distance to a vision target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void SetVisionMeasurementStdDevs(
-      const wpi::array<double, 3>& visionMeasurementStdDevs);
-
-  /**
    * Resets the robot's position on the field.
    *
    * @param gyroAngle The current gyro angle.
@@ -101,71 +88,10 @@
    * @param pose The estimated pose of the robot on the field.
    */
   void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                     units::meter_t rightDistance, const Pose2d& pose);
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose.
-   */
-  Pose2d GetEstimatedPosition() const;
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime(),
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp(). This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
-   */
-  void AddVisionMeasurement(const Pose2d& visionRobotPose,
-                            units::second_t timestamp);
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * Note that the vision measurement standard deviations passed into this
-   * method will continue to apply to future measurements until a subsequent
-   * call to SetVisionMeasurementStdDevs() or this method.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime(),
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp(). This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void AddVisionMeasurement(
-      const Pose2d& visionRobotPose, units::second_t timestamp,
-      const wpi::array<double, 3>& visionMeasurementStdDevs) {
-    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    AddVisionMeasurement(visionRobotPose, timestamp);
+                     units::meter_t rightDistance, const Pose2d& pose) {
+    PoseEstimator<DifferentialDriveWheelSpeeds,
+                  DifferentialDriveWheelPositions>::
+        ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
   }
 
   /**
@@ -179,7 +105,12 @@
    * @return The estimated pose of the robot.
    */
   Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                units::meter_t rightDistance);
+                units::meter_t rightDistance) {
+    return PoseEstimator<
+        DifferentialDriveWheelSpeeds,
+        DifferentialDriveWheelPositions>::Update(gyroAngle,
+                                                 {leftDistance, rightDistance});
+  }
 
   /**
    * Updates the pose estimator with wheel encoder and gyro information. This
@@ -195,61 +126,16 @@
   Pose2d UpdateWithTime(units::second_t currentTime,
                         const Rotation2d& gyroAngle,
                         units::meter_t leftDistance,
-                        units::meter_t rightDistance);
+                        units::meter_t rightDistance) {
+    return PoseEstimator<
+        DifferentialDriveWheelSpeeds,
+        DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle,
+                                                         {leftDistance,
+                                                          rightDistance});
+  }
 
  private:
-  struct InterpolationRecord {
-    // The pose observed given the current sensor inputs and the previous pose.
-    Pose2d pose;
-
-    // The current gyro angle.
-    Rotation2d gyroAngle;
-
-    // The distance traveled by the left encoder.
-    units::meter_t leftDistance;
-
-    // The distance traveled by the right encoder.
-    units::meter_t rightDistance;
-
-    /**
-     * Checks equality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
-     */
-    bool operator==(const InterpolationRecord& other) const = default;
-
-    /**
-     * Checks inequality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const InterpolationRecord& other) const = default;
-
-    /**
-     * Interpolates between two InterpolationRecords.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    InterpolationRecord Interpolate(DifferentialDriveKinematics& kinematics,
-                                    InterpolationRecord endValue,
-                                    double i) const;
-  };
-
-  DifferentialDriveKinematics& m_kinematics;
-  DifferentialDriveOdometry m_odometry;
-  wpi::array<double, 3> m_q{wpi::empty_array};
-  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
-  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
-      1.5_s, [this](const InterpolationRecord& start,
-                    const InterpolationRecord& end, double t) {
-        return start.Interpolate(this->m_kinematics, end, t);
-      }};
+  DifferentialDriveOdometry m_odometryImpl;
 };
 
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index b09e8d9..32ed558 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h

@@ -52,6 +52,10 @@
   /**
    * Constructs an extended Kalman filter.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.
    * @param h                  A vector-valued function of x and u that returns
@@ -69,6 +73,10 @@
   /**
    * Constructs an extended Kalman filter.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.
    * @param h                  A vector-valued function of x and u that returns
@@ -163,6 +171,33 @@
     Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
   }
 
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  void Correct(const InputVector& u, const OutputVector& y,
+               const Matrixd<Outputs, Outputs>& R) {
+    Correct<Outputs>(u, y, m_h, R, m_residualFuncY, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement
+   *          vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
   template <int Rows>
   void Correct(
       const InputVector& u, const Vectord<Rows>& y,
@@ -180,7 +215,7 @@
    * @param y             Measurement vector.
    * @param h             A vector-valued function of x and u that returns
    *                      the measurement vector.
-   * @param R             Discrete measurement noise covariance matrix.
+   * @param R             Continuous measurement noise covariance matrix.
    * @param residualFuncY A function that computes the residual of two
    *                      measurement vectors (i.e. it subtracts them.)
    * @param addFuncX      A function that adds two state vectors.

diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
index a56b8b5..0b0e9de 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc

@@ -4,8 +4,11 @@
 
 #pragma once
 
-#include "Eigen/Cholesky"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <functional>
+
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/ExtendedKalmanFilter.h"
 #include "frc/system/Discretization.h"
@@ -36,13 +39,13 @@
 
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
 
   Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
 
   if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
-    m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
-        discA.transpose(), C.transpose(), discQ, discR);
+    m_initP =
+        DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
   } else {
     m_initP = StateMatrix::Zero();
   }
@@ -72,13 +75,13 @@
 
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
 
   Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
 
   if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
-    m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
-        discA.transpose(), C.transpose(), discQ, discR);
+    m_initP =
+        DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
   } else {
     m_initP = StateMatrix::Zero();
   }
@@ -95,7 +98,7 @@
   // Find discrete A and Q
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
 
   m_xHat = RK4(m_f, m_xHat, u, dt);
 

diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
index 2121284..f143493 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h

@@ -46,6 +46,10 @@
   /**
    * Constructs a state-space observer with the given plant.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param plant              The plant used for the prediction step.
    * @param stateStdDevs       Standard deviations of model states.
    * @param measurementStdDevs Standard deviations of measurements.

diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
index ca4f37c..7506c0d 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc

@@ -10,8 +10,9 @@
 #include <stdexcept>
 #include <string>
 
-#include "Eigen/Cholesky"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/KalmanFilter.h"
 #include "frc/system/Discretization.h"
@@ -31,7 +32,7 @@
 
   Matrixd<States, States> discA;
   Matrixd<States, States> discQ;
-  DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
 
   auto discR = DiscretizeR<Outputs>(contR, dt);
 
@@ -47,8 +48,8 @@
     throw std::invalid_argument(msg);
   }
 
-  Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
-      discA.transpose(), C.transpose(), discQ, discR);
+  Matrixd<States, States> P =
+      DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
 
   // S = CPCᵀ + R
   Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;

diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index d1967e9..d748164 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h

@@ -9,7 +9,7 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/interpolation/TimeInterpolatableBuffer.h"
@@ -32,7 +32,9 @@
  * never call it, then this class will behave mostly like regular encoder
  * odometry.
  */
-class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
+class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
+    : public PoseEstimator<MecanumDriveWheelSpeeds,
+                           MecanumDriveWheelPositions> {
  public:
   /**
    * Constructs a MecanumDrivePoseEstimator with default standard deviations
@@ -76,172 +78,8 @@
       const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
       const wpi::array<double, 3>& visionMeasurementStdDevs);
 
-  /**
-   * Sets the pose estimator's trust in vision measurements. This might be used
-   * to change trust in vision measurements after the autonomous period, or to
-   * change trust as distance to a vision target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void SetVisionMeasurementStdDevs(
-      const wpi::array<double, 3>& visionMeasurementStdDevs);
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * The gyroscope angle does not need to be reset in the user's robot code.
-   * The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances measured at each wheel.
-   * @param pose      The position on the field that your robot is at.
-   */
-  void ResetPosition(const Rotation2d& gyroAngle,
-                     const MecanumDriveWheelPositions& wheelPositions,
-                     const Pose2d& pose);
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  Pose2d GetEstimatedPosition() const;
-
-  /**
-   * Add a vision measurement to the Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime()
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp().) This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
-   */
-  void AddVisionMeasurement(const Pose2d& visionRobotPose,
-                            units::second_t timestamp);
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * Note that the vision measurement standard deviations passed into this
-   * method will continue to apply to future measurements until a subsequent
-   * call to SetVisionMeasurementStdDevs() or this method.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime(),
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp(). This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void AddVisionMeasurement(
-      const Pose2d& visionRobotPose, units::second_t timestamp,
-      const wpi::array<double, 3>& visionMeasurementStdDevs) {
-    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    AddVisionMeasurement(visionRobotPose, timestamp);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This
-   * should be called every loop.
-   *
-   * @param gyroAngle   The current gyro angle.
-   * @param wheelPositions The distances measured at each wheel.
-   * @return The estimated pose of the robot in meters.
-   */
-  Pose2d Update(const Rotation2d& gyroAngle,
-                const MecanumDriveWheelPositions& wheelPositions);
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This
-   * should be called every loop.
-   *
-   * @param currentTime Time at which this method was called, in seconds.
-   * @param gyroAngle   The current gyroscope angle.
-   * @param wheelPositions The distances measured at each wheel.
-   * @return The estimated pose of the robot in meters.
-   */
-  Pose2d UpdateWithTime(units::second_t currentTime,
-                        const Rotation2d& gyroAngle,
-                        const MecanumDriveWheelPositions& wheelPositions);
-
  private:
-  struct InterpolationRecord {
-    // The pose observed given the current sensor inputs and the previous pose.
-    Pose2d pose;
-
-    // The current gyroscope angle.
-    Rotation2d gyroAngle;
-
-    // The distances measured at each wheel.
-    MecanumDriveWheelPositions wheelPositions;
-
-    /**
-     * Checks equality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
-     */
-    bool operator==(const InterpolationRecord& other) const = default;
-
-    /**
-     * Checks inequality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const InterpolationRecord& other) const = default;
-
-    /**
-     * Interpolates between two InterpolationRecords.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    InterpolationRecord Interpolate(MecanumDriveKinematics& kinematics,
-                                    InterpolationRecord endValue,
-                                    double i) const;
-  };
-
-  MecanumDriveKinematics& m_kinematics;
-  MecanumDriveOdometry m_odometry;
-  wpi::array<double, 3> m_q{wpi::empty_array};
-  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
-  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
-      1.5_s, [this](const InterpolationRecord& start,
-                    const InterpolationRecord& end, double t) {
-        return start.Interpolate(this->m_kinematics, end, t);
-      }};
+  MecanumDriveOdometry m_odometryImpl;
 };
 
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
new file mode 100644
index 0000000..eb437a4
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h

@@ -0,0 +1,228 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/Odometry.h"
+#include "frc/kinematics/WheelPositions.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * This class wraps odometry to fuse latency-compensated
+ * vision measurements with encoder measurements. Robot code should not use this
+ * directly- Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDrivePoseEstimator). It will correct for noisy vision
+ * measurements and encoder drift. It is intended to be an easy drop-in for
+ * Odometry.
+ *
+ * Update() should be called every robot loop.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave like regular encoder odometry.
+ */
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+class WPILIB_DLLEXPORT PoseEstimator {
+ public:
+  /**
+   * Constructs a PoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param odometry A correctly-configured odometry object for your drivetrain.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, and heading in radians). Increase these
+   *     numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+                Odometry<WheelSpeeds, WheelPositions>& odometry,
+                const wpi::array<double, 3>& stateStdDevs,
+                const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+  /**
+   * Sets the pose estimator's trust in vision measurements. This might be used
+   * to change trust in vision measurements after the autonomous period, or to
+   * change trust as distance to a vision target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  void SetVisionMeasurementStdDevs(
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset in the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   * @param pose The estimated pose of the robot on the field.
+   */
+  void ResetPosition(const Rotation2d& gyroAngle,
+                     const WheelPositions& wheelPositions, const Pose2d& pose);
+
+  /**
+   * Gets the estimated robot pose.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  Pose2d GetEstimatedPosition() const;
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *     camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note
+   *     that if you don't use your own time source by calling UpdateWithTime(),
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
+   *     the epoch of this timestamp is the same epoch as
+   *     frc::Timer::GetFPGATimestamp(). This means that you should use
+   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp);
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
+   * Note that the vision measurement standard deviations passed into this
+   * method will continue to apply to future measurements until a subsequent
+   * call to SetVisionMeasurementStdDevs() or this method.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *     camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note
+   *     that if you don't use your own time source by calling UpdateWithTime(),
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
+   *     the epoch of this timestamp is the same epoch as
+   *     frc::Timer::GetFPGATimestamp(). This means that you should use
+   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  void AddVisionMeasurement(
+      const Pose2d& visionRobotPose, units::second_t timestamp,
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    AddVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param gyroAngle      The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   *
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d Update(const Rotation2d& gyroAngle,
+                const WheelPositions& wheelPositions);
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param currentTime   The time at which this method was called.
+   * @param gyroAngle     The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   *
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        const WheelPositions& wheelPositions);
+
+ private:
+  struct InterpolationRecord {
+    // The pose observed given the current sensor inputs and the previous pose.
+    Pose2d pose;
+
+    // The current gyroscope angle.
+    Rotation2d gyroAngle;
+
+    // The distances traveled by the wheels.
+    WheelPositions wheelPositions;
+
+    /**
+     * Checks equality between this InterpolationRecord and another object.
+     *
+     * @param other The other object.
+     * @return Whether the two objects are equal.
+     */
+    bool operator==(const InterpolationRecord& other) const = default;
+
+    /**
+     * Checks inequality between this InterpolationRecord and another object.
+     *
+     * @param other The other object.
+     * @return Whether the two objects are not equal.
+     */
+    bool operator!=(const InterpolationRecord& other) const = default;
+
+    /**
+     * Interpolates between two InterpolationRecords.
+     *
+     * @param endValue The end value for the interpolation.
+     * @param i The interpolant (fraction).
+     *
+     * @return The interpolated state.
+     */
+    InterpolationRecord Interpolate(
+        Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+        InterpolationRecord endValue, double i) const;
+  };
+
+  static constexpr units::second_t kBufferDuration = 1.5_s;
+
+  Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+  Odometry<WheelSpeeds, WheelPositions>& m_odometry;
+  wpi::array<double, 3> m_q{wpi::empty_array};
+  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
+      kBufferDuration, [this](const InterpolationRecord& start,
+                              const InterpolationRecord& end, double t) {
+        return start.Interpolate(this->m_kinematics, end, t);
+      }};
+};
+}  // namespace frc
+
+#include "frc/estimator/PoseEstimator.inc"

diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
new file mode 100644
index 0000000..79d71bc
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc

@@ -0,0 +1,165 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/estimator/PoseEstimator.h"
+
+namespace frc {
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
+    Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+    Odometry<WheelSpeeds, WheelPositions>& odometry,
+    const wpi::array<double, 3>& stateStdDevs,
+    const wpi::array<double, 3>& visionMeasurementStdDevs)
+    : m_kinematics(kinematics), m_odometry(odometry) {
+  for (size_t i = 0; i < 3; ++i) {
+    m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+  }
+
+  SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
+    const wpi::array<double, 3>& visionMeasurementStdDevs) {
+  wpi::array<double, 3> r{wpi::empty_array};
+  for (size_t i = 0; i < 3; ++i) {
+    r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+  }
+
+  // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+  // and C = I. See wpimath/algorithms.md.
+  for (size_t row = 0; row < 3; ++row) {
+    if (m_q[row] == 0.0) {
+      m_visionK(row, row) = 0.0;
+    } else {
+      m_visionK(row, row) =
+          m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+    }
+  }
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
+    const Pose2d& pose) {
+  // Reset state estimate and error covariance
+  m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
+  m_poseBuffer.Clear();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
+    const {
+  return m_odometry.GetPose();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
+    const Pose2d& visionRobotPose, units::second_t timestamp) {
+  // Step 0: If this measurement is old enough to be outside the pose buffer's
+  // timespan, skip.
+  if (!m_poseBuffer.GetInternalBuffer().empty() &&
+      m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
+          timestamp) {
+    return;
+  }
+
+  // Step 1: Get the estimated pose from when the vision measurement was made.
+  auto sample = m_poseBuffer.Sample(timestamp);
+
+  if (!sample.has_value()) {
+    return;
+  }
+
+  // Step 2: Measure the twist between the odometry pose and the vision pose
+  auto twist = sample.value().pose.Log(visionRobotPose);
+
+  // Step 3: We should not trust the twist entirely, so instead we scale this
+  // twist by a Kalman gain matrix representing how much we trust vision
+  // measurements compared to our current pose.
+  Eigen::Vector3d k_times_twist =
+      m_visionK *
+      Eigen::Vector3d{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
+
+  // Step 4: Convert back to Twist2d
+  Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
+                      units::meter_t{k_times_twist(1)},
+                      units::radian_t{k_times_twist(2)}};
+
+  // Step 5: Reset Odometry to state at sample with vision adjustment.
+  m_odometry.ResetPosition(sample.value().gyroAngle,
+                           sample.value().wheelPositions,
+                           sample.value().pose.Exp(scaledTwist));
+
+  // Step 6: Record the current pose to allow multiple measurements from the
+  // same timestamp
+  m_poseBuffer.AddSample(timestamp,
+                         {GetEstimatedPosition(), sample.value().gyroAngle,
+                          sample.value().wheelPositions});
+
+  // Step 7: Replay odometry inputs between sample time and latest recorded
+  // sample to update the pose buffer and correct odometry.
+  auto internal_buf = m_poseBuffer.GetInternalBuffer();
+
+  auto upper_bound =
+      std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
+                       [](const auto& pair, auto t) { return t > pair.first; });
+
+  for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
+    UpdateWithTime(entry->first, entry->second.gyroAngle,
+                   entry->second.wheelPositions);
+  }
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
+  return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
+                        wheelPositions);
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& gyroAngle,
+    const WheelPositions& wheelPositions) {
+  m_odometry.Update(gyroAngle, wheelPositions);
+
+  WheelPositions internalWheelPositions = wheelPositions;
+
+  m_poseBuffer.AddSample(
+      currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
+
+  return GetEstimatedPosition();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+typename PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord
+PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
+    Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+    InterpolationRecord endValue, double i) const {
+  if (i < 0) {
+    return *this;
+  } else if (i > 1) {
+    return endValue;
+  } else {
+    // Find the new wheel distance measurements.
+    WheelPositions wheels_lerp =
+        this->wheelPositions.Interpolate(endValue.wheelPositions, i);
+
+    // Find the new gyro angle.
+    auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+    // Create a twist to represent the change based on the interpolated
+    // sensor inputs.
+    auto twist = kinematics.ToTwist2d(this->wheelPositions, wheels_lerp);
+    twist.dtheta = (gyro - gyroAngle).Radians();
+
+    return {pose.Exp(twist), gyro, wheels_lerp};
+  }
+}
+
+}  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index d07bafe..43831d7 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -6,17 +6,15 @@
 
 #include <cmath>
 
-#include <fmt/format.h>
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
-#include <wpi/timestamp.h>
 
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
-#include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
+#include "frc/kinematics/SwerveDriveWheelPositions.h"
 #include "units/time.h"
 
 namespace frc {
@@ -33,7 +31,9 @@
  * odometry.
  */
 template <size_t NumModules>
-class SwerveDrivePoseEstimator {
+class SwerveDrivePoseEstimator
+    : public PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+                           SwerveDriveWheelPositions<NumModules>> {
  public:
   /**
    * Constructs a SwerveDrivePoseEstimator with default standard deviations
@@ -83,14 +83,10 @@
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
       const wpi::array<double, 3>& visionMeasurementStdDevs)
-      : m_kinematics{kinematics},
-        m_odometry{kinematics, gyroAngle, modulePositions, initialPose} {
-    for (size_t i = 0; i < 3; ++i) {
-      m_q[i] = stateStdDevs[i] * stateStdDevs[i];
-    }
-
-    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-  }
+      : PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+                      SwerveDriveWheelPositions<NumModules>>(
+            kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+        m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
 
   /**
    * Resets the robot's position on the field.
@@ -107,143 +103,11 @@
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& pose) {
-    // Reset state estimate and error covariance
-    m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
-    m_poseBuffer.Clear();
-  }
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
-
-  /**
-   * Sets the pose estimator's trust in vision measurements. This might be used
-   * to change trust in vision measurements after the autonomous period, or to
-   * change trust as distance to a vision target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void SetVisionMeasurementStdDevs(
-      const wpi::array<double, 3>& visionMeasurementStdDevs) {
-    wpi::array<double, 3> r{wpi::empty_array};
-    for (size_t i = 0; i < 3; ++i) {
-      r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
-    }
-
-    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-    // and C = I. See wpimath/algorithms.md.
-    for (size_t row = 0; row < 3; ++row) {
-      if (m_q[row] == 0.0) {
-        m_visionK(row, row) = 0.0;
-      } else {
-        m_visionK(row, row) =
-            m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
-      }
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the
-   * odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *    camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime()
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp().) This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
-   */
-  void AddVisionMeasurement(const Pose2d& visionRobotPose,
-                            units::second_t timestamp) {
-    // Step 1: Get the estimated pose from when the vision measurement was made.
-    auto sample = m_poseBuffer.Sample(timestamp);
-
-    if (!sample.has_value()) {
-      return;
-    }
-
-    // Step 2: Measure the twist between the odometry pose and the vision pose
-    auto twist = sample.value().pose.Log(visionRobotPose);
-
-    // Step 3: We should not trust the twist entirely, so instead we scale this
-    // twist by a Kalman gain matrix representing how much we trust vision
-    // measurements compared to our current pose.
-    frc::Vectord<3> k_times_twist =
-        m_visionK * frc::Vectord<3>{twist.dx.value(), twist.dy.value(),
-                                    twist.dtheta.value()};
-
-    // Step 4: Convert back to Twist2d
-    Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
-                        units::meter_t{k_times_twist(1)},
-                        units::radian_t{k_times_twist(2)}};
-
-    // Step 5: Reset Odometry to state at sample with vision adjustment.
-    m_odometry.ResetPosition(sample.value().gyroAngle,
-                             sample.value().modulePostions,
-                             sample.value().pose.Exp(scaledTwist));
-
-    // Step 6: Replay odometry inputs between sample time and latest recorded
-    // sample to update the pose buffer and correct odometry.
-    auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
-    auto upper_bound = std::lower_bound(
-        internal_buf.begin(), internal_buf.end(), timestamp,
-        [](const auto& pair, auto t) { return t > pair.first; });
-
-    for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
-      UpdateWithTime(entry->first, entry->second.gyroAngle,
-                     entry->second.modulePostions);
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the
-   * odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * Note that the vision measurement standard deviations passed into this
-   * method will continue to apply to future measurements until a subsequent
-   * call to SetVisionMeasurementStdDevs() or this method.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime(),
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp(). This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void AddVisionMeasurement(
-      const Pose2d& visionRobotPose, units::second_t timestamp,
-      const wpi::array<double, 3>& visionMeasurementStdDevs) {
-    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    AddVisionMeasurement(visionRobotPose, timestamp);
+    PoseEstimator<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
+                                                              {modulePositions},
+                                                              pose);
   }
 
   /**
@@ -258,8 +122,10 @@
   Pose2d Update(
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                          modulePositions);
+    return PoseEstimator<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
+                                                       {modulePositions});
   }
 
   /**
@@ -275,109 +141,13 @@
   Pose2d UpdateWithTime(
       units::second_t currentTime, const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    m_odometry.Update(gyroAngle, modulePositions);
-
-    wpi::array<SwerveModulePosition, NumModules> internalModulePositions{
-        wpi::empty_array};
-
-    for (size_t i = 0; i < NumModules; i++) {
-      internalModulePositions[i].distance = modulePositions[i].distance;
-      internalModulePositions[i].angle = modulePositions[i].angle;
-    }
-
-    m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
-                                         internalModulePositions});
-
-    return GetEstimatedPosition();
+    return PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+                         SwerveDriveWheelPositions<NumModules>>::
+        UpdateWithTime(currentTime, gyroAngle, {modulePositions});
   }
 
  private:
-  struct InterpolationRecord {
-    // The pose observed given the current sensor inputs and the previous pose.
-    Pose2d pose;
-
-    // The current gyroscope angle.
-    Rotation2d gyroAngle;
-
-    // The distances traveled and rotations meaured at each module.
-    wpi::array<SwerveModulePosition, NumModules> modulePostions;
-
-    /**
-     * Checks equality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
-     */
-    bool operator==(const InterpolationRecord& other) const = default;
-
-    /**
-     * Checks inequality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const InterpolationRecord& other) const = default;
-
-    /**
-     * Interpolates between two InterpolationRecords.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    InterpolationRecord Interpolate(
-        SwerveDriveKinematics<NumModules>& kinematics,
-        InterpolationRecord endValue, double i) const {
-      if (i < 0) {
-        return *this;
-      } else if (i > 1) {
-        return endValue;
-      } else {
-        // Find the new module distances.
-        wpi::array<SwerveModulePosition, NumModules> modulePositions{
-            wpi::empty_array};
-        // Find the distance between this measurement and the
-        // interpolated measurement.
-        wpi::array<SwerveModulePosition, NumModules> modulesDelta{
-            wpi::empty_array};
-
-        for (size_t i = 0; i < NumModules; i++) {
-          modulePositions[i].distance =
-              wpi::Lerp(this->modulePostions[i].distance,
-                        endValue.modulePostions[i].distance, i);
-          modulePositions[i].angle =
-              wpi::Lerp(this->modulePostions[i].angle,
-                        endValue.modulePostions[i].angle, i);
-
-          modulesDelta[i].distance =
-              modulePositions[i].distance - this->modulePostions[i].distance;
-          modulesDelta[i].angle = modulePositions[i].angle;
-        }
-
-        // Find the new gyro angle.
-        auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
-        // Create a twist to represent this changed based on the interpolated
-        // sensor inputs.
-        auto twist = kinematics.ToTwist2d(modulesDelta);
-        twist.dtheta = (gyro - gyroAngle).Radians();
-
-        return {pose.Exp(twist), gyro, modulePositions};
-      }
-    }
-  };
-
-  SwerveDriveKinematics<NumModules>& m_kinematics;
-  SwerveDriveOdometry<NumModules> m_odometry;
-  wpi::array<double, 3> m_q{wpi::empty_array};
-  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
-  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
-      1.5_s, [this](const InterpolationRecord& start,
-                    const InterpolationRecord& end, double t) {
-        return start.Interpolate(this->m_kinematics, end, t);
-      }};
+  SwerveDriveOdometry<NumModules> m_odometryImpl;
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 39ce615..9526f0c 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h

@@ -58,6 +58,10 @@
   /**
    * Constructs an unscented Kalman filter.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.
    * @param h                  A vector-valued function of x and u that returns
@@ -78,6 +82,10 @@
    * you have angles in the state or measurements, because they allow you to
    * correctly account for the modular nature of angle arithmetic.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.
    * @param h                  A vector-valued function of x and u that returns
@@ -205,6 +213,21 @@
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
+   * This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  void Correct(const InputVector& u, const OutputVector& y,
+               const Matrixd<Outputs, Outputs>& R) {
+    Correct<Outputs>(u, y, m_h, R, m_meanFuncY, m_residualFuncY,
+                     m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
    * This is useful for when the measurements available during a timestep's
    * Correct() call vary. The h(x, u) passed to the constructor is used if one
    * is not provided (the two-argument version of this function).
@@ -213,7 +236,7 @@
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement
    *          vector.
-   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param R Continuous measurement noise covariance matrix.
    */
   template <int Rows>
   void Correct(
@@ -232,7 +255,7 @@
    * @param y             Measurement vector.
    * @param h             A vector-valued function of x and u that returns the
    *                      measurement vector.
-   * @param R             Measurement noise covariance matrix (continuous-time).
+   * @param R             Continuous measurement noise covariance matrix.
    * @param meanFuncY     A function that computes the mean of 2 * States + 1
    *                      measurement vectors using a given set of weights.
    * @param residualFuncY A function that computes the residual of two

diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
index a6744bf..c693197 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc

@@ -4,7 +4,10 @@
 
 #pragma once
 
-#include "Eigen/Cholesky"
+#include <functional>
+
+#include <Eigen/Cholesky>
+
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
 #include "frc/estimator/UnscentedTransform.h"
@@ -76,7 +79,7 @@
       NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
+  DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
   Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
 
   Matrixd<States, 2 * States + 1> sigmas =

diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
index e28f094..bec3bc8 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h

@@ -4,9 +4,11 @@
 
 #pragma once
 
+#include <functional>
 #include <tuple>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+
 #include "frc/EigenCore.h"
 
 namespace frc {

diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index 0fb4f48..fc558df 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h

@@ -11,10 +11,10 @@
 #include <stdexcept>
 #include <vector>
 
+#include <Eigen/QR>
 #include <wpi/array.h>
 #include <wpi/circular_buffer.h>
 
-#include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "units/time.h"
 #include "wpimath/MathShared.h"
@@ -95,7 +95,7 @@
     static int instances = 0;
     instances++;
     wpi::math::MathSharedStore::ReportUsage(
-        wpi::math::MathUsageId::kFilter_Linear, 1);
+        wpi::math::MathUsageId::kFilter_Linear, instances);
   }
 
   /**

diff --git a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
index 542cd94..17d0d21 100644
--- a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
+++ b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h

@@ -10,6 +10,7 @@
 #include <wpi/timestamp.h>
 
 #include "units/time.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -45,7 +46,8 @@
       : m_positiveRateLimit{positiveRateLimit},
         m_negativeRateLimit{negativeRateLimit},
         m_prevVal{initialValue},
-        m_prevTime{units::microsecond_t(wpi::Now())} {}
+        m_prevTime{
+            units::microsecond_t(wpi::math::MathSharedStore::GetTimestamp())} {}
 
   /**
    * Creates a new SlewRateLimiter with the given positive rate limit and
@@ -57,19 +59,6 @@
       : SlewRateLimiter(rateLimit, -rateLimit) {}
 
   /**
-   * Creates a new SlewRateLimiter with the given positive rate limit and
-   * negative rate limit of -rateLimit and initial value.
-   *
-   * @param rateLimit The rate-of-change limit.
-   * @param initialValue The initial value of the input.
-   */
-  WPI_DEPRECATED(
-      "Use SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit, "
-      "Unit_t initalValue) instead")
-  SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue)
-      : SlewRateLimiter(rateLimit, -rateLimit, initialValue) {}
-
-  /**
    * Filters the input to limit its slew rate.
    *
    * @param input The input value whose slew rate is to be limited.
@@ -77,7 +66,7 @@
    * rate.
    */
   Unit_t Calculate(Unit_t input) {
-    units::second_t currentTime = units::microsecond_t(wpi::Now());
+    units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
     units::second_t elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
         std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
@@ -94,7 +83,7 @@
    */
   void Reset(Unit_t value) {
     m_prevVal = value;
-    m_prevTime = units::microsecond_t(wpi::Now());
+    m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
   }
 
  private:

diff --git a/wpimath/src/main/native/include/frc/fmt/Eigen.h b/wpimath/src/main/native/include/frc/fmt/Eigen.h
index c6b2ee6..2438b9f 100644
--- a/wpimath/src/main/native/include/frc/fmt/Eigen.h
+++ b/wpimath/src/main/native/include/frc/fmt/Eigen.h

@@ -4,120 +4,41 @@
 
 #pragma once
 
+#include <concepts>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
 #include <fmt/format.h>
 
-#include "Eigen/Core"
-#include "Eigen/SparseCore"
-
 /**
- * Formatter for Eigen::Matrix<double, Rows, Cols>.
- *
- * @tparam Rows Number of rows.
- * @tparam Cols Number of columns.
- * @tparam Args Defaulted template arguments to Eigen::Matrix<>.
+ * Formatter for classes derived from Eigen::MatrixBase<Derived> or
+ * Eigen::SparseCompressedBase<Derived>.
  */
-template <int Rows, int Cols, int... Args>
-struct fmt::formatter<Eigen::Matrix<double, Rows, Cols, Args...>> {
-  /**
-   * Storage for format specifier.
-   */
-  char presentation = 'f';
-
-  /**
-   * Format string parser.
-   *
-   * @param ctx Format string context.
-   */
+template <typename Derived, typename CharT>
+  requires std::derived_from<Derived, Eigen::MatrixBase<Derived>> ||
+           std::derived_from<Derived, Eigen::SparseCompressedBase<Derived>>
+struct fmt::formatter<Derived, CharT> {
   constexpr auto parse(fmt::format_parse_context& ctx) {
-    auto it = ctx.begin(), end = ctx.end();
-    if (it != end && (*it == 'f' || *it == 'e')) {
-      presentation = *it++;
-    }
-
-    if (it != end && *it != '}') {
-      throw fmt::format_error("invalid format");
-    }
-
-    return it;
+    return m_underlying.parse(ctx);
   }
 
-  /**
-   * Writes out a formatted matrix.
-   *
-   * @tparam FormatContext Format string context type.
-   * @param mat Matrix to format.
-   * @param ctx Format string context.
-   */
-  template <typename FormatContext>
-  auto format(const Eigen::Matrix<double, Rows, Cols, Args...>& mat,
-              FormatContext& ctx) {
+  auto format(const Derived& mat, fmt::format_context& ctx) const {
     auto out = ctx.out();
-    for (int i = 0; i < mat.rows(); ++i) {
-      for (int j = 0; j < mat.cols(); ++j) {
-        out = fmt::format_to(out, "  {:f}", mat(i, j));
+
+    for (int row = 0; row < mat.rows(); ++row) {
+      for (int col = 0; col < mat.cols(); ++col) {
+        out = fmt::format_to(out, "  ");
+        out = m_underlying.format(mat.coeff(row, col), ctx);
       }
 
-      if (i < mat.rows() - 1) {
+      if (row < mat.rows() - 1) {
         out = fmt::format_to(out, "\n");
       }
     }
 
     return out;
   }
-};
 
-/**
- * Formatter for Eigen::SparseMatrix<double>.
- *
- * @tparam Options Union of bit flags controlling the storage scheme.
- * @tparam StorageIndex The type of the indices.
- */
-template <int Options, typename StorageIndex>
-struct fmt::formatter<Eigen::SparseMatrix<double, Options, StorageIndex>> {
-  /**
-   * Storage for format specifier.
-   */
-  char presentation = 'f';
-
-  /**
-   * Format string parser.
-   *
-   * @param ctx Format string context.
-   */
-  constexpr auto parse(fmt::format_parse_context& ctx) {
-    auto it = ctx.begin(), end = ctx.end();
-    if (it != end && (*it == 'f' || *it == 'e')) {
-      presentation = *it++;
-    }
-
-    if (it != end && *it != '}') {
-      throw fmt::format_error("invalid format");
-    }
-
-    return it;
-  }
-
-  /**
-   * Writes out a formatted matrix.
-   *
-   * @tparam FormatContext Format string context type.
-   * @param mat Matrix to format.
-   * @param ctx Format string context.
-   */
-  template <typename FormatContext>
-  auto format(const Eigen::SparseMatrix<double, Options, StorageIndex>& mat,
-              FormatContext& ctx) {
-    auto out = ctx.out();
-    for (int i = 0; i < mat.rows(); ++i) {
-      for (int j = 0; j < mat.cols(); ++j) {
-        out = fmt::format_to(out, "  {:f}", mat.coeff(i, j));
-      }
-
-      if (i < mat.rows() - 1) {
-        out = fmt::format_to(out, "\n");
-      }
-    }
-
-    return out;
-  }
+ private:
+  fmt::formatter<typename Derived::Scalar, CharT> m_underlying;
 };

diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
index 58b966a..2b471b7 100644
--- a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h

@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 
-#include "frc/EigenCore.h"
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Rotation3d.h"
 
@@ -67,7 +67,7 @@
  private:
   friend class CoordinateSystem;
 
-  Vectord<3> m_axis;
+  Eigen::Vector3d m_axis;
 };
 
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
index 232455f..f5e71f2 100644
--- a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h

@@ -6,7 +6,6 @@
 
 #include <wpi/SymbolExports.h>
 
-#include "frc/EigenCore.h"
 #include "frc/geometry/CoordinateAxis.h"
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Rotation3d.h"

diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
index d096e8c..970f792 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h

@@ -4,15 +4,18 @@
 
 #pragma once
 
+#include <initializer_list>
+#include <span>
+
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Transform2d.h"
-#include "Translation2d.h"
-#include "Twist2d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Twist2d.h"
 
 namespace frc {
 
@@ -120,6 +123,15 @@
   constexpr Pose2d operator/(double scalar) const;
 
   /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   *
+   * @return The rotated pose.
+   */
+  constexpr Pose2d RotateBy(const Rotation2d& other) const;
+
+  /**
    * Transforms the pose by the given transformation and returns the new pose.
    * See + operator for the matrix multiplication performed.
    *
@@ -176,6 +188,20 @@
    */
   Twist2d Log(const Pose2d& end) const;
 
+  /**
+   * Returns the nearest Pose2d from a collection of poses
+   * @param poses The collection of poses.
+   * @return The nearest Pose2d from the collection.
+   */
+  Pose2d Nearest(std::span<const Pose2d> poses) const;
+
+  /**
+   * Returns the nearest Pose2d from a collection of poses
+   * @param poses The collection of poses.
+   * @return The nearest Pose2d from the collection.
+   */
+  Pose2d Nearest(std::initializer_list<Pose2d> poses) const;
+
  private:
   Translation2d m_translation;
   Rotation2d m_rotation;
@@ -189,4 +215,38 @@
 
 }  // namespace frc
 
-#include "Pose2d.inc"
+template <>
+struct wpi::Struct<frc::Pose2d> {
+  static constexpr std::string_view kTypeString = "struct:Pose2d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
+                                  wpi::Struct<frc::Rotation2d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation2d translation;Rotation2d rotation";
+  static frc::Pose2d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data, const frc::Pose2d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Pose2d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Pose2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value);
+};
+
+#include "frc/geometry/Pose2d.inc"

diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
index c549f26..2764d16 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc

@@ -31,6 +31,10 @@
   return *this * (1.0 / scalar);
 }
 
+constexpr Pose2d Pose2d::RotateBy(const Rotation2d& other) const {
+  return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
 constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
           other.Rotation() + m_rotation};

diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
index 8c7ace3..b5a0e03 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -5,15 +5,15 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Pose2d.h"
-#include "Transform3d.h"
-#include "Translation3d.h"
-#include "Twist3d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Transform3d.h"
+#include "frc/geometry/Translation3d.h"
+#include "frc/geometry/Twist3d.h"
 
 namespace frc {
 
@@ -56,7 +56,9 @@
 
   /**
    * Transforms the pose by the given transformation and returns the new
-   * transformed pose.
+   * transformed pose. The transform is applied relative to the pose's frame.
+   * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
+   * applied relative to the global frame and around the origin.
    *
    * @param other The transform to transform the pose by.
    *
@@ -131,8 +133,20 @@
   Pose3d operator/(double scalar) const;
 
   /**
-   * Transforms the pose by the given transformation and returns the new pose.
-   * See + operator for the matrix multiplication performed.
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by, which is applied
+   * extrinsically (from the global frame).
+   *
+   * @return The rotated pose.
+   */
+  Pose3d RotateBy(const Rotation3d& other) const;
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose. The transform is applied relative to the pose's frame.
+   * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
+   * applied relative to the global frame and around the origin.
    *
    * @param other The transform to transform the pose by.
    *
@@ -202,3 +216,37 @@
 void from_json(const wpi::json& json, Pose3d& pose);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Pose3d> {
+  static constexpr std::string_view kTypeString = "struct:Pose3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
+                                  wpi::Struct<frc::Rotation3d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation3d translation;Rotation3d rotation";
+  static frc::Pose3d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data, const frc::Pose3d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Pose3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Pose3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value);
+};

diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
index b5a318b..63a3af2 100644
--- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h
+++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h

@@ -4,13 +4,11 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
-
-#include "frc/EigenCore.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 namespace frc {
 
@@ -32,6 +30,34 @@
   Quaternion(double w, double x, double y, double z);
 
   /**
+   * Adds with another quaternion.
+   *
+   * @param other the other quaternion
+   */
+  Quaternion operator+(const Quaternion& other) const;
+
+  /**
+   * Subtracts another quaternion.
+   *
+   * @param other the other quaternion
+   */
+  Quaternion operator-(const Quaternion& other) const;
+
+  /**
+   * Multiples with a scalar value.
+   *
+   * @param other the scalar value
+   */
+  Quaternion operator*(const double other) const;
+
+  /**
+   * Divides by a scalar value.
+   *
+   * @param other the scalar value
+   */
+  Quaternion operator/(const double other) const;
+
+  /**
    * Multiply with another quaternion.
    *
    * @param other The other quaternion.
@@ -47,6 +73,16 @@
   bool operator==(const Quaternion& other) const;
 
   /**
+   * Returns the elementwise product of two quaternions.
+   */
+  double Dot(const Quaternion& other) const;
+
+  /**
+   * Returns the conjugate of the quaternion.
+   */
+  Quaternion Conjugate() const;
+
+  /**
    * Returns the inverse of the quaternion.
    */
   Quaternion Inverse() const;
@@ -57,6 +93,52 @@
   Quaternion Normalize() const;
 
   /**
+   * Calculates the L2 norm of the quaternion.
+   */
+  double Norm() const;
+
+  /**
+   * Calculates this quaternion raised to a power.
+   *
+   * @param t the power to raise this quaternion to.
+   */
+  Quaternion Pow(const double t) const;
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * @param other the "Twist" that will be applied to this quaternion.
+   */
+  Quaternion Exp(const Quaternion& other) const;
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * source: wpimath/algorithms.md
+   *
+   *  If this quaternion is in 𝖘𝖔(3) and you are looking for an element of
+   * SO(3), use FromRotationVector
+   */
+  Quaternion Exp() const;
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * @param other The quaternion to map this quaternion onto
+   */
+  Quaternion Log(const Quaternion& other) const;
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * source:  wpimath/algorithms.md
+   *
+   * If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3),
+   * use ToRotationVector
+   */
+  Quaternion Log() const;
+
+  /**
    * Returns W component of the quaternion.
    */
   double W() const;
@@ -83,8 +165,20 @@
    */
   Eigen::Vector3d ToRotationVector() const;
 
+  /**
+   * Returns the quaternion representation of this rotation vector.
+   *
+   * This is also the exp operator of 𝖘𝖔(3).
+   *
+   * source: wpimath/algorithms.md
+   */
+  static Quaternion FromRotationVector(const Eigen::Vector3d& rvec);
+
  private:
+  // Scalar r in versor form
   double m_r = 1.0;
+
+  // Vector v in versor form
   Eigen::Vector3d m_v{0.0, 0.0, 0.0};
 };
 
@@ -95,3 +189,32 @@
 void from_json(const wpi::json& json, Quaternion& quaternion);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Quaternion> {
+  static constexpr std::string_view kTypeString = "struct:Quaternion";
+  static constexpr size_t kSize = 32;
+  static constexpr std::string_view kSchema =
+      "double w;double x;double y;double z";
+  static frc::Quaternion Unpack(std::span<const uint8_t, 32> data) {
+    return {wpi::UnpackStruct<double, 0>(data),
+            wpi::UnpackStruct<double, 8>(data),
+            wpi::UnpackStruct<double, 16>(data),
+            wpi::UnpackStruct<double, 24>(data)};
+  }
+  static void Pack(std::span<uint8_t, 32> data, const frc::Quaternion& value) {
+    wpi::PackStruct<0>(data, value.W());
+    wpi::PackStruct<8>(data, value.X());
+    wpi::PackStruct<16>(data, value.Y());
+    wpi::PackStruct<24>(data, value.Z());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Quaternion> {
+  static constexpr std::string_view kTypeString = "proto:Quaternion";
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Quaternion Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Quaternion& value);
+};

diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index 406ef3c..96041a4 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h

@@ -5,13 +5,12 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "units/angle.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 /**
@@ -179,4 +178,25 @@
 
 }  // namespace frc
 
-#include "Rotation2d.inc"
+template <>
+struct wpi::Struct<frc::Rotation2d> {
+  static constexpr std::string_view kTypeString = "struct:Rotation2d";
+  static constexpr size_t kSize = 8;
+  static constexpr std::string_view kSchema = "double value";
+  static frc::Rotation2d Unpack(std::span<const uint8_t, 8> data) {
+    return units::radian_t{wpi::UnpackStruct<double>(data)};
+  }
+  static void Pack(std::span<uint8_t, 8> data, const frc::Rotation2d& value) {
+    wpi::PackStruct(data, value.Radians().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rotation2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rotation2d& value);
+};
+
+#include "frc/geometry/Rotation2d.inc"

diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
index eb31ebd..dbe9e35 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc

@@ -23,7 +23,8 @@
     : Rotation2d(units::radian_t{value}) {}
 
 constexpr Rotation2d::Rotation2d(double x, double y) {
-  const auto magnitude = gcem::hypot(x, y);
+  double magnitude =
+      std::is_constant_evaluated() ? gcem::hypot(x, y) : std::hypot(x, y);
   if (magnitude > 1e-6) {
     m_sin = y / magnitude;
     m_cos = x / magnitude;

diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
index 7c1a60d..6d04715 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -4,17 +4,16 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Quaternion.h"
-#include "Rotation2d.h"
-#include "frc/EigenCore.h"
+#include "frc/geometry/Quaternion.h"
+#include "frc/geometry/Rotation2d.h"
 #include "units/angle.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 /**
@@ -57,7 +56,16 @@
    * @param axis The rotation axis.
    * @param angle The rotation around the axis.
    */
-  Rotation3d(const Vectord<3>& axis, units::radian_t angle);
+  Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle);
+
+  /**
+   * Constructs a Rotation3d with the given rotation vector representation. This
+   * representation is equivalent to axis-angle, where the normalized axis is
+   * multiplied by the rotation around the axis in radians.
+   *
+   * @param rvec The rotation vector.
+   */
+  explicit Rotation3d(const Eigen::Vector3d& rvec);
 
   /**
    * Constructs a Rotation3d from a rotation matrix.
@@ -65,7 +73,7 @@
    * @param rotationMatrix The rotation matrix.
    * @throws std::domain_error if the rotation matrix isn't special orthogonal.
    */
-  explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
+  explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix);
 
   /**
    * Constructs a Rotation3d that rotates the initial vector onto the final
@@ -77,7 +85,7 @@
    * @param initial The initial vector.
    * @param final The final vector.
    */
-  Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
+  Rotation3d(const Eigen::Vector3d& initial, const Eigen::Vector3d& final);
 
   /**
    * Adds two rotations together.
@@ -126,12 +134,16 @@
   /**
    * Checks equality between this Rotation3d and another object.
    */
-  bool operator==(const Rotation3d&) const = default;
+  bool operator==(const Rotation3d&) const;
 
   /**
-   * Adds the new rotation to the current rotation.
+   * Adds the new rotation to the current rotation. The other rotation is
+   * applied extrinsically, which means that it rotates around the global axes.
+   * For example, Rotation3d{90_deg, 0, 0}.RotateBy(Rotation3d{0, 45_deg, 0})
+   * rotates by 90 degrees around the +X axis and then by 45 degrees around the
+   * global +Y axis. (This is equivalent to Rotation3d{90_deg, 45_deg, 0})
    *
-   * @param other The rotation to rotate by.
+   * @param other The extrinsic rotation to rotate by.
    *
    * @return The new rotated Rotation3d.
    */
@@ -160,7 +172,7 @@
   /**
    * Returns the axis in the axis-angle representation of this rotation.
    */
-  Vectord<3> Axis() const;
+  Eigen::Vector3d Axis() const;
 
   /**
    * Returns the angle in the axis-angle representation of this rotation.
@@ -184,3 +196,31 @@
 void from_json(const wpi::json& json, Rotation3d& rotation);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Rotation3d> {
+  static constexpr std::string_view kTypeString = "struct:Rotation3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Quaternion>::kSize;
+  static constexpr std::string_view kSchema = "Quaternion q";
+  static frc::Rotation3d Unpack(std::span<const uint8_t, kSize> data) {
+    return frc::Rotation3d{wpi::UnpackStruct<frc::Quaternion, 0>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Rotation3d& value) {
+    wpi::PackStruct<0>(data, value.GetQuaternion());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Quaternion>(fn);
+  }
+};
+
+static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rotation3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rotation3d& value);
+};

diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
index 3c21ec1..593dce0 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h

@@ -5,15 +5,17 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Translation2d.h"
+#include "frc/geometry/Translation2d.h"
 
 namespace frc {
 
 class WPILIB_DLLEXPORT Pose2d;
 
 /**
- * Represents a transformation for a Pose2d.
+ * Represents a transformation for a Pose2d in the pose's frame.
  */
 class WPILIB_DLLEXPORT Transform2d {
  public:
@@ -34,6 +36,17 @@
   constexpr Transform2d(Translation2d translation, Rotation2d rotation);
 
   /**
+   * Constructs a transform with x and y translations instead of a separate
+   * Translation2d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  constexpr Transform2d(units::meter_t x, units::meter_t y,
+                        Rotation2d rotation);
+
+  /**
    * Constructs the identity transform -- maps an initial pose to itself.
    */
   constexpr Transform2d() = default;
@@ -94,7 +107,8 @@
   }
 
   /**
-   * Composes two transformations.
+   * Composes two transformations. The second transform is applied relative to
+   * the orientation of the first.
    *
    * @param other The transform to compose with this one.
    * @return The composition of the two transformations.
@@ -112,4 +126,40 @@
 };
 }  // namespace frc
 
-#include "Transform2d.inc"
+template <>
+struct wpi::Struct<frc::Transform2d> {
+  static constexpr std::string_view kTypeString = "struct:Transform2d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
+                                  wpi::Struct<frc::Rotation2d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation2d translation;Rotation2d rotation";
+  static frc::Transform2d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Transform2d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Transform2d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Transform2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Transform2d& value);
+};
+
+#include "frc/geometry/Transform2d.inc"

diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
index f851a05..862b8d5 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc

@@ -16,6 +16,10 @@
                                    Rotation2d rotation)
     : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
 
+constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y,
+                                   Rotation2d rotation)
+    : m_translation(x, y), m_rotation(std::move(rotation)) {}
+
 constexpr Transform2d Transform2d::Inverse() const {
   // We are rotating the difference between the translations
   // using a clockwise rotation matrix. This transforms the global

diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
index 5f50ec2..d5af97d 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h

@@ -5,15 +5,17 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Translation3d.h"
+#include "frc/geometry/Translation3d.h"
 
 namespace frc {
 
 class WPILIB_DLLEXPORT Pose3d;
 
 /**
- * Represents a transformation for a Pose3d.
+ * Represents a transformation for a Pose3d in the pose's frame.
  */
 class WPILIB_DLLEXPORT Transform3d {
  public:
@@ -34,6 +36,18 @@
   Transform3d(Translation3d translation, Rotation3d rotation);
 
   /**
+   * Constructs a transform with x, y, and z translations instead of a separate
+   * Translation3d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param z The z component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
+              Rotation3d rotation);
+
+  /**
    * Constructs the identity transform -- maps an initial pose to itself.
    */
   constexpr Transform3d() = default;
@@ -99,7 +113,8 @@
   Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); }
 
   /**
-   * Composes two transformations.
+   * Composes two transformations. The second transform is applied relative to
+   * the orientation of the first.
    *
    * @param other The transform to compose with this one.
    * @return The composition of the two transformations.
@@ -116,3 +131,39 @@
   Rotation3d m_rotation;
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Transform3d> {
+  static constexpr std::string_view kTypeString = "struct:Transform3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
+                                  wpi::Struct<frc::Rotation3d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation3d translation;Rotation3d rotation";
+  static frc::Transform3d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Transform3d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Transform3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Transform3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Transform3d& value);
+};

diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index e168510..1568586 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h

@@ -4,15 +4,17 @@
 
 #pragma once
 
+#include <initializer_list>
+#include <span>
+
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Rotation2d.h"
+#include "frc/geometry/Rotation2d.h"
 #include "units/length.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 /**
@@ -170,6 +172,21 @@
    */
   bool operator==(const Translation2d& other) const;
 
+  /**
+   * Returns the nearest Translation2d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation2d from the collection.
+   */
+  Translation2d Nearest(std::span<const Translation2d> translations) const;
+
+  /**
+   * Returns the nearest Translation2d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation2d from the collection.
+   */
+  Translation2d Nearest(
+      std::initializer_list<Translation2d> translations) const;
+
  private:
   units::meter_t m_x = 0_m;
   units::meter_t m_y = 0_m;
@@ -183,4 +200,28 @@
 
 }  // namespace frc
 
-#include "Translation2d.inc"
+template <>
+struct wpi::Struct<frc::Translation2d> {
+  static constexpr std::string_view kTypeString = "struct:Translation2d";
+  static constexpr size_t kSize = 16;
+  static constexpr std::string_view kSchema = "double x;double y";
+  static frc::Translation2d Unpack(std::span<const uint8_t, 16> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 16> data,
+                   const frc::Translation2d& value) {
+    wpi::PackStruct<0>(data, value.X().value());
+    wpi::PackStruct<8>(data, value.Y().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Translation2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Translation2d& value);
+};
+
+#include "frc/geometry/Translation2d.inc"

diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index ab641fa..47ba1f6 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h

@@ -5,15 +5,14 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Rotation3d.h"
-#include "Translation2d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation2d.h"
 #include "units/length.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 /**
@@ -186,4 +185,30 @@
 
 }  // namespace frc
 
-#include "Translation3d.inc"
+template <>
+struct wpi::Struct<frc::Translation3d> {
+  static constexpr std::string_view kTypeString = "struct:Translation3d";
+  static constexpr size_t kSize = 24;
+  static constexpr std::string_view kSchema = "double x;double y;double z";
+  static frc::Translation3d Unpack(std::span<const uint8_t, 24> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 16>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 24> data,
+                   const frc::Translation3d& value) {
+    wpi::PackStruct<0>(data, value.X().value());
+    wpi::PackStruct<8>(data, value.Y().value());
+    wpi::PackStruct<16>(data, value.Z().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Translation3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Translation3d& value);
+};
+
+#include "frc/geometry/Translation3d.inc"

diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
index 620b688..257f1b6 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "units/angle.h"
 #include "units/length.h"
@@ -14,7 +16,7 @@
 /**
  * A change in distance along a 2D arc since the last pose update. We can use
  * ideas from differential calculus to create new Pose2ds from a Twist2d and
- * vise versa.
+ * vice versa.
  *
  * A Twist can be used to represent a difference between two poses.
  */
@@ -57,3 +59,28 @@
   }
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Twist2d> {
+  static constexpr std::string_view kTypeString = "struct:Twist2d";
+  static constexpr size_t kSize = 24;
+  static constexpr std::string_view kSchema =
+      "double dx;double dy;double dtheta";
+  static frc::Twist2d Unpack(std::span<const uint8_t, 24> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 16>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 24> data, const frc::Twist2d& value) {
+    wpi::PackStruct<0>(data, value.dx.value());
+    wpi::PackStruct<8>(data, value.dy.value());
+    wpi::PackStruct<16>(data, value.dtheta.value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Twist2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value);
+};

diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
index 3040ab3..4d902df 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation3d.h"
 #include "units/angle.h"
@@ -15,7 +17,7 @@
 /**
  * A change in distance along a 3D arc since the last pose update. We can use
  * ideas from differential calculus to create new Pose3ds from a Twist3d and
- * vise versa.
+ * vice versa.
  *
  * A Twist can be used to represent a difference between two poses.
  */
@@ -77,3 +79,35 @@
   }
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Twist3d> {
+  static constexpr std::string_view kTypeString = "struct:Twist3d";
+  static constexpr size_t kSize = 48;
+  static constexpr std::string_view kSchema =
+      "double dx;double dy;double dz;double rx;double ry;double rz";
+  static frc::Twist3d Unpack(std::span<const uint8_t, 48> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 16>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 24>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 32>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 40>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 48> data, const frc::Twist3d& value) {
+    wpi::PackStruct<0>(data, value.dx.value());
+    wpi::PackStruct<8>(data, value.dy.value());
+    wpi::PackStruct<16>(data, value.dz.value());
+    wpi::PackStruct<24>(data, value.rx.value());
+    wpi::PackStruct<32>(data, value.ry.value());
+    wpi::PackStruct<40>(data, value.rz.value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist3d> {
+  static constexpr std::string_view kTypeString = "proto:Twist3d";
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Twist3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value);
+};

diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index 771fe84..9dd8e62 100644
--- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -68,11 +68,26 @@
     if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
       m_pastSnapshots.emplace_back(time, sample);
     } else {
-      m_pastSnapshots.insert(
-          std::upper_bound(
-              m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
-              [](auto t, const auto& pair) { return t < pair.first; }),
-          std::pair(time, sample));
+      auto first_after = std::upper_bound(
+          m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+          [](auto t, const auto& pair) { return t < pair.first; });
+
+      // Don't access this before ensuring first_after isn't first.
+      auto last_not_greater_than = first_after - 1;
+
+      if (first_after == m_pastSnapshots.begin() ||
+          last_not_greater_than == m_pastSnapshots.begin() ||
+          last_not_greater_than->first < time) {
+        // Two cases handled together:
+        // 1. All entries come after the sample
+        // 2. Some entries come before the sample, but none are recorded with
+        // the same time
+        m_pastSnapshots.insert(first_after, std::pair(time, sample));
+      } else {
+        // Final case:
+        // 3. An entry exists with the same recorded time.
+        last_not_greater_than->second = sample;
+      }
     }
     while (time - m_pastSnapshots[0].first > m_historySize) {
       m_pastSnapshots.erase(m_pastSnapshots.begin());
@@ -112,6 +127,10 @@
         m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
         [](const auto& pair, auto t) { return t > pair.first; });
 
+    if (upper_bound == m_pastSnapshots.begin()) {
+      return upper_bound->second;
+    }
+
     auto lower_bound = upper_bound - 1;
 
     double t = ((time - lower_bound->first) /

diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
index 37fe768..93c9044 100644
--- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h

@@ -6,6 +6,7 @@
 
 #include <wpi/SymbolExports.h>
 
+#include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "units/angular_velocity.h"
 #include "units/velocity.h"
@@ -15,8 +16,7 @@
  * Represents the speed of a robot chassis. Although this struct contains
  * similar members compared to a Twist2d, they do NOT represent the same thing.
  * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
- * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
- * frame of reference.
+ * reference, a ChassisSpeeds struct represents a robot's velocity.
  *
  * A strictly non-holonomic drivetrain, such as a differential drive, should
  * never have a dy component because it can never move sideways. Holonomic
@@ -24,12 +24,12 @@
  */
 struct WPILIB_DLLEXPORT ChassisSpeeds {
   /**
-   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+   * Velocity along the x-axis. (Fwd is +)
    */
   units::meters_per_second_t vx = 0_mps;
 
   /**
-   * Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
+   * Velocity along the y-axis. (Left is +)
    */
   units::meters_per_second_t vy = 0_mps;
 
@@ -39,6 +39,57 @@
   units::radians_per_second_t omega = 0_rad_per_s;
 
   /**
+   * Disretizes a continuous-time chassis speed.
+   *
+   * This function converts a continuous-time chassis speed into a discrete-time
+   * one such that when the discrete-time chassis speed is applied for one
+   * timestep, the robot moves as if the velocity components are independent
+   * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
+   * y-axis, and omega * dt around the z-axis).
+   *
+   * This is useful for compensating for translational skew when translating and
+   * rotating a swerve drivetrain.
+   *
+   * @param vx Forward velocity.
+   * @param vy Sideways velocity.
+   * @param omega Angular velocity.
+   * @param dt The duration of the timestep the speeds should be applied for.
+   *
+   * @return Discretized ChassisSpeeds.
+   */
+  static ChassisSpeeds Discretize(units::meters_per_second_t vx,
+                                  units::meters_per_second_t vy,
+                                  units::radians_per_second_t omega,
+                                  units::second_t dt) {
+    Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
+    auto twist = Pose2d{}.Log(desiredDeltaPose);
+    return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
+  }
+
+  /**
+   * Disretizes a continuous-time chassis speed.
+   *
+   * This function converts a continuous-time chassis speed into a discrete-time
+   * one such that when the discrete-time chassis speed is applied for one
+   * timestep, the robot moves as if the velocity components are independent
+   * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
+   * y-axis, and omega * dt around the z-axis).
+   *
+   * This is useful for compensating for translational skew when translating and
+   * rotating a swerve drivetrain.
+   *
+   * @param continuousSpeeds The continuous speeds.
+   * @param dt The duration of the timestep the speeds should be applied for.
+   *
+   * @return Discretized ChassisSpeeds.
+   */
+  static ChassisSpeeds Discretize(const ChassisSpeeds& continuousSpeeds,
+                                  units::second_t dt) {
+    return Discretize(continuousSpeeds.vx, continuousSpeeds.vy,
+                      continuousSpeeds.omega, dt);
+  }
+
+  /**
    * Converts a user provided field-relative set of speeds into a robot-relative
    * ChassisSpeeds object.
    *
@@ -57,8 +108,12 @@
   static ChassisSpeeds FromFieldRelativeSpeeds(
       units::meters_per_second_t vx, units::meters_per_second_t vy,
       units::radians_per_second_t omega, const Rotation2d& robotAngle) {
-    return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
-            -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
+    // CW rotation into chassis frame
+    auto rotated =
+        Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
+            .RotateBy(-robotAngle);
+    return {units::meters_per_second_t{rotated.X().value()},
+            units::meters_per_second_t{rotated.Y().value()}, omega};
   }
 
   /**
@@ -81,5 +136,118 @@
                                    fieldRelativeSpeeds.vy,
                                    fieldRelativeSpeeds.omega, robotAngle);
   }
+
+  /**
+   * Converts a user provided robot-relative set of speeds into a field-relative
+   * ChassisSpeeds object.
+   *
+   * @param vx The component of speed in the x direction relative to the robot.
+   * Positive x is towards the robot's front.
+   * @param vy The component of speed in the y direction relative to the robot.
+   * Positive y is towards the robot's left.
+   * @param omega The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   * robot's angle is considered to be zero when it is facing directly away from
+   * your alliance station wall. Remember that this should be CCW positive.
+   *
+   * @return ChassisSpeeds object representing the speeds in the field's frame
+   * of reference.
+   */
+  static ChassisSpeeds FromRobotRelativeSpeeds(
+      units::meters_per_second_t vx, units::meters_per_second_t vy,
+      units::radians_per_second_t omega, const Rotation2d& robotAngle) {
+    // CCW rotation out of chassis frame
+    auto rotated =
+        Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
+            .RotateBy(robotAngle);
+    return {units::meters_per_second_t{rotated.X().value()},
+            units::meters_per_second_t{rotated.Y().value()}, omega};
+  }
+
+  /**
+   * Converts a user provided robot-relative ChassisSpeeds object into a
+   * field-relative ChassisSpeeds object.
+   *
+   * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds
+   *    in the robot frame of reference. Positive x is the towards robot's
+   * front. Positive y is towards the robot's left.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   *    robot's angle is considered to be zero when it is facing directly away
+   *    from your alliance station wall. Remember that this should be CCW
+   *    positive.
+   * @return ChassisSpeeds object representing the speeds in the field's frame
+   *    of reference.
+   */
+  static ChassisSpeeds FromRobotRelativeSpeeds(
+      const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
+    return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
+                                   robotRelativeSpeeds.vy,
+                                   robotRelativeSpeeds.omega, robotAngle);
+  }
+
+  /**
+   * Adds two ChassisSpeeds and returns the sum.
+   *
+   * <p>For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5}
+   * = ChassisSpeeds{3.0, 2.0, 2.0}
+   *
+   * @param other The ChassisSpeeds to add.
+   *
+   * @return The sum of the ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const {
+    return {vx + other.vx, vy + other.vy, omega + other.omega};
+  }
+
+  /**
+   * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and
+   * returns the difference.
+   *
+   * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0}
+   * = ChassisSpeeds{4.0, 2.0, 1.0}
+   *
+   * @param other The ChassisSpeeds to subtract.
+   *
+   * @return The difference between the two ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current ChassisSpeeds.
+   * This is equivalent to negating all components of the ChassisSpeeds.
+   *
+   * @return The inverse of the current ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; }
+
+  /**
+   * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2
+   * = ChassisSpeeds{4.0, 5.0, 1.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator*(double scalar) const {
+    return {scalar * vx, scalar * vy, scalar * omega};
+  }
+
+  /**
+   * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2
+   * = ChassisSpeeds{1.0, 1.25, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
 };
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 930c7a6..95f53fa 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h

@@ -8,7 +8,9 @@
 
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
 #include "units/angle.h"
 #include "units/length.h"
 #include "wpimath/MathShared.h"
@@ -22,7 +24,9 @@
  * velocity components whereas forward kinematics converts left and right
  * component velocities into a linear and angular chassis speed.
  */
-class WPILIB_DLLEXPORT DifferentialDriveKinematics {
+class WPILIB_DLLEXPORT DifferentialDriveKinematics
+    : public Kinematics<DifferentialDriveWheelSpeeds,
+                        DifferentialDriveWheelPositions> {
  public:
   /**
    * Constructs a differential drive kinematics object.
@@ -46,7 +50,7 @@
    * @return The chassis speed.
    */
   constexpr ChassisSpeeds ToChassisSpeeds(
-      const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+      const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
     return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
             (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
   }
@@ -60,7 +64,7 @@
    * @return The left and right velocities.
    */
   constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
-      const ChassisSpeeds& chassisSpeeds) const {
+      const ChassisSpeeds& chassisSpeeds) const override {
     return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
             chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
   }
@@ -79,6 +83,11 @@
             (rightDistance - leftDistance) / trackWidth * 1_rad};
   }
 
+  Twist2d ToTwist2d(const DifferentialDriveWheelPositions& start,
+                    const DifferentialDriveWheelPositions& end) const override {
+    return ToTwist2d(end.left - start.left, end.right - start.right);
+  }
+
   units::meter_t trackWidth;
 };
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index cc198ac..279c1dc 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h

@@ -7,6 +7,10 @@
 #include <wpi/SymbolExports.h>
 
 #include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
 #include "units/length.h"
 
 namespace frc {
@@ -22,7 +26,9 @@
  * It is important that you reset your encoders to zero before using this class.
  * Any subsequent pose resets also require the encoders to be reset to zero.
  */
-class WPILIB_DLLEXPORT DifferentialDriveOdometry {
+class WPILIB_DLLEXPORT DifferentialDriveOdometry
+    : public Odometry<DifferentialDriveWheelSpeeds,
+                      DifferentialDriveWheelPositions> {
  public:
   /**
    * Constructs a DifferentialDriveOdometry object.
@@ -56,21 +62,14 @@
    */
   void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
                      units::meter_t rightDistance, const Pose2d& pose) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-    m_prevLeftDistance = leftDistance;
-    m_prevRightDistance = rightDistance;
+    Odometry<DifferentialDriveWheelSpeeds,
+             DifferentialDriveWheelPositions>::ResetPosition(gyroAngle,
+                                                             {leftDistance,
+                                                              rightDistance},
+                                                             pose);
   }
 
   /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
    * Updates the robot position on the field using distance measurements from
    * encoders. This method is more numerically accurate than using velocities to
    * integrate the pose and is also advantageous for teams that are using lower
@@ -82,15 +81,14 @@
    * @return The new pose of the robot.
    */
   const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                       units::meter_t rightDistance);
+                       units::meter_t rightDistance) {
+    return Odometry<DifferentialDriveWheelSpeeds,
+                    DifferentialDriveWheelPositions>::Update(gyroAngle,
+                                                             {leftDistance,
+                                                              rightDistance});
+  }
 
  private:
-  Pose2d m_pose;
-
-  Rotation2d m_gyroOffset;
-  Rotation2d m_previousAngle;
-
-  units::meter_t m_prevLeftDistance = 0_m;
-  units::meter_t m_prevRightDistance = 0_m;
+  DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
 };
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h
new file mode 100644
index 0000000..4dddbea
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a differential drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
+  /**
+   * Distance driven by the left side.
+   */
+  units::meter_t left = 0_m;
+
+  /**
+   * Distance driven by the right side.
+   */
+  units::meter_t right = 0_m;
+
+  /**
+   * Checks equality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const DifferentialDriveWheelPositions& other) const = default;
+
+  /**
+   * Checks inequality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const DifferentialDriveWheelPositions& other) const = default;
+
+  DifferentialDriveWheelPositions Interpolate(
+      const DifferentialDriveWheelPositions& endValue, double t) const {
+    return {wpi::Lerp(left, endValue.left, t),
+            wpi::Lerp(right, endValue.right, t)};
+  }
+};
+}  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
index fce2b96..9d9e705 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h

@@ -36,5 +36,79 @@
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
    */
   void Desaturate(units::meters_per_second_t attainableMaxSpeed);
+
+  /**
+   * Adds two DifferentialDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} +
+   * DifferentialDriveWheelSpeeds{2.0, 1.5} =
+   * DifferentialDriveWheelSpeeds{3.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to add.
+   *
+   * @return The sum of the DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator+(
+      const DifferentialDriveWheelSpeeds& other) const {
+    return {left + other.left, right + other.right};
+  }
+
+  /**
+   * Subtracts the other DifferentialDriveWheelSpeeds from the current
+   * DifferentialDriveWheelSpeeds and returns the difference.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} -
+   * DifferentialDriveWheelSpeeds{1.0, 2.0} =
+   * DifferentialDriveWheelSpeeds{4.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to subtract.
+   *
+   * @return The difference between the two DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator-(
+      const DifferentialDriveWheelSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelSpeeds.
+   * This is equivalent to negating all components of the
+   * DifferentialDriveWheelSpeeds.
+   *
+   * @return The inverse of the current DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator-() const {
+    return {-left, -right};
+  }
+
+  /**
+   * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2
+   * = DifferentialDriveWheelSpeeds{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator*(double scalar) const {
+    return {scalar * left, scalar * right};
+  }
+
+  /**
+   * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2
+   * = DifferentialDriveWheelSpeeds{1.0, 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
 };
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h
new file mode 100644
index 0000000..e27cfc6
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds. Robot code should not use this directly-
+ * Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDriveKinematics).
+ *
+ * Inverse kinematics converts a desired chassis speed into wheel speeds whereas
+ * forward kinematics converts wheel speeds into chassis speed.
+ */
+template <typename WheelSpeeds, typename WheelPositions>
+class WPILIB_DLLEXPORT Kinematics {
+ public:
+  /**
+   * Performs forward kinematics to return the resulting chassis speed from the
+   * wheel speeds. This method is often used for odometry -- determining the
+   * robot's position on the field using data from the real-world speed of each
+   * wheel on the robot.
+   *
+   * @param wheelSpeeds The speeds of the wheels.
+   * @return The chassis speed.
+   */
+  virtual ChassisSpeeds ToChassisSpeeds(
+      const WheelSpeeds& wheelSpeeds) const = 0;
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired
+   * chassis velocity. This method is often used to convert joystick values into
+   * wheel speeds.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  virtual WheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const = 0;
+
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the given
+   * change in wheel positions. This method is often used for odometry --
+   * determining the robot's position on the field using changes in the distance
+   * driven by each wheel on the robot.
+   *
+   * @param start The starting distances driven by the wheels.
+   * @param end The ending distances driven by the wheels.
+   *
+   * @return The resulting Twist2d in the robot's movement.
+   */
+  virtual Twist2d ToTwist2d(const WheelPositions& start,
+                            const WheelPositions& end) const = 0;
+};
+}  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index 2880cef..fe6eb51 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h

@@ -4,13 +4,14 @@
 
 #pragma once
 
+#include <Eigen/QR>
 #include <wpi/SymbolExports.h>
 
-#include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
 #include "frc/kinematics/MecanumDriveWheelPositions.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
 #include "wpimath/MathShared.h"
@@ -39,7 +40,8 @@
  * Forward kinematics is also used for odometry -- determining the position of
  * the robot on the field using encoders and a gyro.
  */
-class WPILIB_DLLEXPORT MecanumDriveKinematics {
+class WPILIB_DLLEXPORT MecanumDriveKinematics
+    : public Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
  public:
   /**
    * Constructs a mecanum drive kinematics object.
@@ -101,7 +103,12 @@
    */
   MecanumDriveWheelSpeeds ToWheelSpeeds(
       const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d{}) const;
+      const Translation2d& centerOfRotation) const;
+
+  MecanumDriveWheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const override {
+    return ToWheelSpeeds(chassisSpeeds, {});
+  }
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -114,7 +121,10 @@
    * @return The resulting chassis speed.
    */
   ChassisSpeeds ToChassisSpeeds(
-      const MecanumDriveWheelSpeeds& wheelSpeeds) const;
+      const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
+
+  Twist2d ToTwist2d(const MecanumDriveWheelPositions& start,
+                    const MecanumDriveWheelPositions& end) const override;
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the

diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index 5e949ca..0a6f537 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h

@@ -9,7 +9,9 @@
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
 #include "units/time.h"
 
 namespace frc {
@@ -23,7 +25,8 @@
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  */
-class WPILIB_DLLEXPORT MecanumDriveOdometry {
+class WPILIB_DLLEXPORT MecanumDriveOdometry
+    : public Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
  public:
   /**
    * Constructs a MecanumDriveOdometry object.
@@ -38,52 +41,8 @@
       const MecanumDriveWheelPositions& wheelPositions,
       const Pose2d& initialPose = Pose2d{});
 
-  /**
-   * Resets the robot's position on the field.
-   *
-   * The gyroscope angle does not need to be reset here on the user's robot
-   * code. The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The current distances measured by each wheel.
-   * @param pose The position on the field that your robot is at.
-   */
-  void ResetPosition(const Rotation2d& gyroAngle,
-                     const MecanumDriveWheelPositions& wheelPositions,
-                     const Pose2d& pose) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-    m_previousWheelPositions = wheelPositions;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in an angle parameter
-   * which is used instead of the angular rate that is calculated from forward
-   * kinematics, in addition to the current distance measurement at each wheel.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The current distances measured by each wheel.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       const MecanumDriveWheelPositions& wheelPositions);
-
  private:
-  MecanumDriveKinematics m_kinematics;
-  Pose2d m_pose;
-
-  MecanumDriveWheelPositions m_previousWheelPositions;
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
+  MecanumDriveKinematics m_kinematicsImpl;
 };
 
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
index b69aceb..76c8b9d 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h

@@ -4,13 +4,14 @@
 
 #pragma once
 
+#include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 
 #include "units/length.h"
 
 namespace frc {
 /**
- * Represents the wheel speeds for a mecanum drive drivetrain.
+ * Represents the wheel positions for a mecanum drive drivetrain.
  */
 struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
   /**
@@ -49,5 +50,13 @@
    * @return Whether the two objects are not equal.
    */
   bool operator!=(const MecanumDriveWheelPositions& other) const = default;
+
+  MecanumDriveWheelPositions Interpolate(
+      const MecanumDriveWheelPositions& endValue, double t) const {
+    return {wpi::Lerp(frontLeft, endValue.frontLeft, t),
+            wpi::Lerp(frontRight, endValue.frontRight, t),
+            wpi::Lerp(rearLeft, endValue.rearLeft, t),
+            wpi::Lerp(rearRight, endValue.rearRight, t)};
+  }
 };
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
index e698c5f..80e8460 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h

@@ -46,5 +46,77 @@
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
    */
   void Desaturate(units::meters_per_second_t attainableMaxSpeed);
+
+  /**
+   * Adds two MecanumDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} +
+   * MecanumDriveWheelSpeeds{2.0, 1.5, 0.5, 1.0} =
+   * MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
+   *
+   * @param other The MecanumDriveWheelSpeeds to add.
+   * @return The sum of the MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator+(
+      const MecanumDriveWheelSpeeds& other) const {
+    return {frontLeft + other.frontLeft, frontRight + other.frontRight,
+            rearLeft + other.rearLeft, rearRight + other.rearRight};
+  }
+
+  /**
+   * Subtracts the other MecanumDriveWheelSpeeds from the current
+   * MecanumDriveWheelSpeeds and returns the difference.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} -
+   * MecanumDriveWheelSpeeds{1.0, 2.0, 3.0, 0.5} =
+   * MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
+   *
+   * @param other The MecanumDriveWheelSpeeds to subtract.
+   * @return The difference between the two MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator-(
+      const MecanumDriveWheelSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current MecanumDriveWheelSpeeds.
+   * This is equivalent to negating all components of the
+   * MecanumDriveWheelSpeeds.
+   *
+   * @return The inverse of the current MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator-() const {
+    return {-frontLeft, -frontRight, -rearLeft, -rearRight};
+  }
+
+  /**
+   * Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new
+   * MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 =
+   * MecanumDriveWheelSpeeds{4.0, 5.0, 6.0, 7.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator*(double scalar) const {
+    return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
+            scalar * rearRight};
+  }
+
+  /**
+   * Divides the MecanumDriveWheelSpeeds by a scalar and returns the new
+   * MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 =
+   * MecanumDriveWheelSpeeds{1.0, 1.25, 0.75, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
 };
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h
new file mode 100644
index 0000000..55c074c
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h

@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/WheelPositions.h"
+
+namespace frc {
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the
+ * particular type for your drivetrain (e.g., DifferentialDriveOdometry).
+ * Odometry allows you to track the robot's position on the field over a course
+ * of a match using readings from your wheel encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+class WPILIB_DLLEXPORT Odometry {
+ public:
+  /**
+   * Constructs an Odometry object.
+   *
+   * @param kinematics The kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit Odometry(const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+                    const Rotation2d& gyroAngle,
+                    const WheelPositions& wheelPositions,
+                    const Pose2d& initialPose = Pose2d{});
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   * @param pose The position on the field that your robot is at.
+   */
+  void ResetPosition(const Rotation2d& gyroAngle,
+                     const WheelPositions& wheelPositions, const Pose2d& pose) {
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_previousWheelPositions = wheelPositions;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in an angle parameter
+   * which is used instead of the angular rate that is calculated from forward
+   * kinematics, in addition to the current distance measurement at each wheel.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   *
+   * @return The new pose of the robot.
+   */
+  const Pose2d& Update(const Rotation2d& gyroAngle,
+                       const WheelPositions& wheelPositions);
+
+ private:
+  const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+  Pose2d m_pose;
+
+  WheelPositions m_previousWheelPositions;
+  Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
+};
+}  // namespace frc
+
+#include "Odometry.inc"

diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.inc b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc
new file mode 100644
index 0000000..688c1bd
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/kinematics/Odometry.h"
+
+namespace frc {
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Odometry<WheelSpeeds, WheelPositions>::Odometry(
+    const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
+    const Pose2d& initialPose)
+    : m_kinematics(kinematics),
+      m_pose(initialPose),
+      m_previousWheelPositions(wheelPositions) {
+  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+const Pose2d& Odometry<WheelSpeeds, WheelPositions>::Update(
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
+  auto angle = gyroAngle + m_gyroOffset;
+
+  auto twist = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
+  twist.dtheta = (angle - m_previousAngle).Radians();
+
+  auto newPose = m_pose.Exp(twist);
+
+  m_previousAngle = angle;
+  m_previousWheelPositions = wheelPositions;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}
+}  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 97ee233..f6d4e5d 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

@@ -4,23 +4,30 @@
 
 #pragma once
 
+#include <concepts>
 #include <cstddef>
 
+#include <Eigen/QR>
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/SwerveDriveWheelPositions.h"
 #include "frc/kinematics/SwerveModulePosition.h"
 #include "frc/kinematics/SwerveModuleState.h"
 #include "units/velocity.h"
 #include "wpimath/MathShared.h"
 
 namespace frc {
+
+template <size_t NumModules>
+using SwerveDriveWheelSpeeds = wpi::array<SwerveModuleState, NumModules>;
+
 /**
  * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
  * into individual module states (speed and angle).
@@ -44,27 +51,25 @@
  * the robot on the field using encoders and a gyro.
  */
 template <size_t NumModules>
-class SwerveDriveKinematics {
+class SwerveDriveKinematics
+    : public Kinematics<SwerveDriveWheelSpeeds<NumModules>,
+                        SwerveDriveWheelPositions<NumModules>> {
  public:
   /**
    * Constructs a swerve drive kinematics object. This takes in a variable
-   * number of wheel locations as Translation2ds. The order in which you pass in
-   * the wheel locations is the same order that you will receive the module
+   * number of module locations as Translation2ds. The order in which you pass
+   * in the module locations is the same order that you will receive the module
    * states when performing inverse kinematics. It is also expected that you
    * pass in the module states in the same order when calling the forward
    * kinematics methods.
    *
-   * @param wheel  The location of the first wheel relative to the physical
-   *               center of the robot.
-   * @param wheels The locations of the other wheels relative to the physical
-   *               center of the robot.
+   * @param moduleTranslations The locations of the modules relative to the
+   *                           physical center of the robot.
    */
-  template <typename... Wheels>
-  explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
-      : m_modules{wheel, wheels...}, m_moduleStates(wpi::empty_array) {
-    static_assert(sizeof...(wheels) >= 1,
-                  "A swerve drive requires at least two modules");
-
+  template <std::convertible_to<Translation2d>... ModuleTranslations>
+    requires(sizeof...(ModuleTranslations) == NumModules)
+  explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations)
+      : m_modules{moduleTranslations...}, m_moduleHeadings(wpi::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -80,8 +85,8 @@
   }
 
   explicit SwerveDriveKinematics(
-      const wpi::array<Translation2d, NumModules>& wheels)
-      : m_modules{wheels}, m_moduleStates(wpi::empty_array) {
+      const wpi::array<Translation2d, NumModules>& modules)
+      : m_modules{modules}, m_moduleHeadings(wpi::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -99,6 +104,25 @@
   SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
 
   /**
+   * Reset the internal swerve module headings.
+   * @param moduleHeadings The swerve module headings. The order of the module
+   * headings should be same as passed into the constructor of this class.
+   */
+  template <std::convertible_to<Rotation2d>... ModuleHeadings>
+    requires(sizeof...(ModuleHeadings) == NumModules)
+  void ResetHeadings(ModuleHeadings&&... moduleHeadings) {
+    return this->ResetHeadings(
+        wpi::array<Rotation2d, NumModules>{moduleHeadings...});
+  }
+
+  /**
+   * Reset the internal swerve module headings.
+   * @param moduleHeadings The swerve module headings. The order of the module
+   * headings should be same as passed into the constructor of this class.
+   */
+  void ResetHeadings(wpi::array<Rotation2d, NumModules> moduleHeadings);
+
+  /**
    * Performs inverse kinematics to return the module states from a desired
    * chassis velocity. This method is often used to convert joystick values into
    * module speeds and angles.
@@ -133,20 +157,29 @@
       const ChassisSpeeds& chassisSpeeds,
       const Translation2d& centerOfRotation = Translation2d{}) const;
 
+  SwerveDriveWheelSpeeds<NumModules> ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const override {
+    return ToSwerveModuleStates(chassisSpeeds);
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the
    * given module states. This method is often used for odometry -- determining
    * the robot's position on the field using data from the real-world speed and
    * angle of each module on the robot.
    *
-   * @param wheelStates The state of the modules (as a SwerveModuleState type)
+   * @param moduleStates The state of the modules (as a SwerveModuleState type)
    * as measured from respective encoders and gyros. The order of the swerve
    * module states should be same as passed into the constructor of this class.
    *
    * @return The resulting chassis speed.
    */
-  template <typename... ModuleStates>
-  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
+  template <std::convertible_to<SwerveModuleState>... ModuleStates>
+    requires(sizeof...(ModuleStates) == NumModules)
+  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... moduleStates) const {
+    return this->ToChassisSpeeds(
+        wpi::array<SwerveModuleState, NumModules>{moduleStates...});
+  }
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -161,8 +194,8 @@
    *
    * @return The resulting chassis speed.
    */
-  ChassisSpeeds ToChassisSpeeds(
-      wpi::array<SwerveModuleState, NumModules> moduleStates) const;
+  ChassisSpeeds ToChassisSpeeds(const wpi::array<SwerveModuleState, NumModules>&
+                                    moduleStates) const override;
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
@@ -170,15 +203,19 @@
    * determining the robot's position on the field using data from the
    * real-world position delta and angle of each module on the robot.
    *
-   * @param wheelDeltas The latest change in position of the modules (as a
+   * @param moduleDeltas The latest change in position of the modules (as a
    * SwerveModulePosition type) as measured from respective encoders and gyros.
    * The order of the swerve module states should be same as passed into the
    * constructor of this class.
    *
    * @return The resulting Twist2d.
    */
-  template <typename... ModuleDeltas>
-  Twist2d ToTwist2d(ModuleDeltas&&... wheelDeltas) const;
+  template <std::convertible_to<SwerveModulePosition>... ModuleDeltas>
+    requires(sizeof...(ModuleDeltas) == NumModules)
+  Twist2d ToTwist2d(ModuleDeltas&&... moduleDeltas) const {
+    return this->ToTwist2d(
+        wpi::array<SwerveModulePosition, NumModules>{moduleDeltas...});
+  }
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
@@ -186,7 +223,7 @@
    * determining the robot's position on the field using data from the
    * real-world position delta and angle of each module on the robot.
    *
-   * @param wheelDeltas The latest change in position of the modules (as a
+   * @param moduleDeltas The latest change in position of the modules (as a
    * SwerveModulePosition type) as measured from respective encoders and gyros.
    * The order of the swerve module states should be same as passed into the
    * constructor of this class.
@@ -194,7 +231,20 @@
    * @return The resulting Twist2d.
    */
   Twist2d ToTwist2d(
-      wpi::array<SwerveModulePosition, NumModules> wheelDeltas) const;
+      wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
+
+  Twist2d ToTwist2d(
+      const SwerveDriveWheelPositions<NumModules>& start,
+      const SwerveDriveWheelPositions<NumModules>& end) const override {
+    auto result =
+        wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+    for (size_t i = 0; i < NumModules; i++) {
+      auto startModule = start.positions[i];
+      auto endModule = end.positions[i];
+      result[i] = {endModule.distance - startModule.distance, endModule.angle};
+    }
+    return ToTwist2d(result);
+  }
 
   /**
    * Renormalizes the wheel speeds if any individual speed is above the
@@ -229,7 +279,7 @@
    *
    * @param moduleStates Reference to array of module states. The array will be
    * mutated with the normalized speeds!
-   * @param currentChassisSpeed Current speed of the robot
+   * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeed The absolute max speed a module can reach
    * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
    * can reach while translating
@@ -238,7 +288,7 @@
    */
   static void DesaturateWheelSpeeds(
       wpi::array<SwerveModuleState, NumModules>* moduleStates,
-      ChassisSpeeds currentChassisSpeed,
+      ChassisSpeeds desiredChassisSpeed,
       units::meters_per_second_t attainableMaxModuleSpeed,
       units::meters_per_second_t attainableMaxRobotTranslationSpeed,
       units::radians_per_second_t attainableMaxRobotRotationSpeed);
@@ -247,7 +297,7 @@
   mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
   wpi::array<Translation2d, NumModules> m_modules;
-  mutable wpi::array<SwerveModuleState, NumModules> m_moduleStates;
+  mutable wpi::array<Rotation2d, NumModules> m_moduleHeadings;
 
   mutable Translation2d m_previousCoR;
 };

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index c7f26e0..db85d99 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc

@@ -13,22 +13,32 @@
 
 namespace frc {
 
-template <class... Wheels>
-SwerveDriveKinematics(Translation2d, Wheels...)
-    -> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
+template <typename ModuleTranslation, typename... ModuleTranslations>
+SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...)
+    -> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>;
+
+template <size_t NumModules>
+void SwerveDriveKinematics<NumModules>::ResetHeadings(
+    wpi::array<Rotation2d, NumModules> moduleHeadings) {
+  for (size_t i = 0; i < NumModules; i++) {
+    m_moduleHeadings[i] = moduleHeadings[i];
+  }
+}
 
 template <size_t NumModules>
 wpi::array<SwerveModuleState, NumModules>
 SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
     const ChassisSpeeds& chassisSpeeds,
     const Translation2d& centerOfRotation) const {
+  wpi::array<SwerveModuleState, NumModules> moduleStates(wpi::empty_array);
+
   if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
       chassisSpeeds.omega == 0_rad_per_s) {
     for (size_t i = 0; i < NumModules; i++) {
-      m_moduleStates[i].speed = 0_mps;
+      moduleStates[i] = {0_mps, m_moduleHeadings[i]};
     }
 
-    return m_moduleStates;
+    return moduleStates;
   }
 
   // We have a new center of rotation. We need to compute the matrix again.
@@ -58,28 +68,16 @@
     auto speed = units::math::hypot(x, y);
     Rotation2d rotation{x.value(), y.value()};
 
-    m_moduleStates[i] = {speed, rotation};
+    moduleStates[i] = {speed, rotation};
+    m_moduleHeadings[i] = rotation;
   }
 
-  return m_moduleStates;
-}
-
-template <size_t NumModules>
-template <typename... ModuleStates>
-ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    ModuleStates&&... wheelStates) const {
-  static_assert(sizeof...(wheelStates) == NumModules,
-                "Number of modules is not consistent with number of wheel "
-                "locations provided in constructor.");
-
-  wpi::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
-
-  return this->ToChassisSpeeds(moduleStates);
+  return moduleStates;
 }
 
 template <size_t NumModules>
 ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    wpi::array<SwerveModuleState, NumModules> moduleStates) const {
+    const wpi::array<SwerveModuleState, NumModules>& moduleStates) const {
   Matrixd<NumModules * 2, 1> moduleStateMatrix;
 
   for (size_t i = 0; i < NumModules; ++i) {
@@ -97,19 +95,6 @@
 }
 
 template <size_t NumModules>
-template <typename... ModuleDeltas>
-Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
-    ModuleDeltas&&... wheelDeltas) const {
-  static_assert(sizeof...(wheelDeltas) == NumModules,
-                "Number of modules is not consistent with number of wheel "
-                "locations provided in constructor.");
-
-  wpi::array<SwerveModulePosition, NumModules> moduleDeltas{wheelDeltas...};
-
-  return this->ToTwist2d(moduleDeltas);
-}
-
-template <size_t NumModules>
 Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
     wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {
   Matrixd<NumModules * 2, 1> moduleDeltaMatrix;
@@ -134,12 +119,13 @@
     wpi::array<SwerveModuleState, NumModules>* moduleStates,
     units::meters_per_second_t attainableMaxSpeed) {
   auto& states = *moduleStates;
-  auto realMaxSpeed = std::max_element(states.begin(), states.end(),
-                                       [](const auto& a, const auto& b) {
-                                         return units::math::abs(a.speed) <
-                                                units::math::abs(b.speed);
-                                       })
-                          ->speed;
+  auto realMaxSpeed =
+      units::math::abs(std::max_element(states.begin(), states.end(),
+                                        [](const auto& a, const auto& b) {
+                                          return units::math::abs(a.speed) <
+                                                 units::math::abs(b.speed);
+                                        })
+                           ->speed);
 
   if (realMaxSpeed > attainableMaxSpeed) {
     for (auto& module : states) {
@@ -151,18 +137,19 @@
 template <size_t NumModules>
 void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
     wpi::array<SwerveModuleState, NumModules>* moduleStates,
-    ChassisSpeeds currentChassisSpeed,
+    ChassisSpeeds desiredChassisSpeed,
     units::meters_per_second_t attainableMaxModuleSpeed,
     units::meters_per_second_t attainableMaxRobotTranslationSpeed,
     units::radians_per_second_t attainableMaxRobotRotationSpeed) {
   auto& states = *moduleStates;
 
-  auto realMaxSpeed = std::max_element(states.begin(), states.end(),
-                                       [](const auto& a, const auto& b) {
-                                         return units::math::abs(a.speed) <
-                                                units::math::abs(b.speed);
-                                       })
-                          ->speed;
+  auto realMaxSpeed =
+      units::math::abs(std::max_element(states.begin(), states.end(),
+                                        [](const auto& a, const auto& b) {
+                                          return units::math::abs(a.speed) <
+                                                 units::math::abs(b.speed);
+                                        })
+                           ->speed);
 
   if (attainableMaxRobotTranslationSpeed == 0_mps ||
       attainableMaxRobotRotationSpeed == 0_rad_per_s || realMaxSpeed == 0_mps) {
@@ -170,10 +157,10 @@
   }
 
   auto translationalK =
-      units::math::hypot(currentChassisSpeed.vx, currentChassisSpeed.vy) /
+      units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) /
       attainableMaxRobotTranslationSpeed;
 
-  auto rotationalK = units::math::abs(currentChassisSpeed.omega) /
+  auto rotationalK = units::math::abs(desiredChassisSpeed.omega) /
                      attainableMaxRobotRotationSpeed;
 
   auto k = units::math::max(translationalK, rotationalK);

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 015c2c0..2e0e553 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h

@@ -11,8 +11,11 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/timestamp.h>
 
+#include "Odometry.h"
 #include "SwerveDriveKinematics.h"
+#include "SwerveDriveWheelPositions.h"
 #include "SwerveModulePosition.h"
+#include "SwerveModuleState.h"
 #include "frc/geometry/Pose2d.h"
 #include "units/time.h"
 
@@ -28,7 +31,9 @@
  * when using computer-vision systems.
  */
 template <size_t NumModules>
-class SwerveDriveOdometry {
+class SwerveDriveOdometry
+    : public Odometry<SwerveDriveWheelSpeeds<NumModules>,
+                      SwerveDriveWheelPositions<NumModules>> {
  public:
   /**
    * Constructs a SwerveDriveOdometry object.
@@ -56,13 +61,13 @@
   void ResetPosition(
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
-      const Pose2d& pose);
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
+      const Pose2d& pose) {
+    Odometry<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
+                                                              {modulePositions},
+                                                              pose);
+  }
 
   /**
    * Updates the robot's position on the field using forward kinematics and
@@ -79,17 +84,15 @@
    */
   const Pose2d& Update(
       const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
+    return Odometry<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
+                                                       {modulePositions});
+  }
 
  private:
-  SwerveDriveKinematics<NumModules> m_kinematics;
-  Pose2d m_pose;
-
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
-
-  wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
-      wpi::empty_array};
+  SwerveDriveKinematics<NumModules> m_kinematicsImpl;
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 64b46c1..48ddfec 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc

@@ -13,58 +13,11 @@
     SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
     const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
     const Pose2d& initialPose)
-    : m_kinematics(kinematics), m_pose(initialPose) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    m_previousModulePositions[i] = {modulePositions[i].distance,
-                                    modulePositions[i].angle};
-  }
-
+    : Odometry<SwerveDriveWheelSpeeds<NumModules>,
+               SwerveDriveWheelPositions<NumModules>>(
+          m_kinematicsImpl, gyroAngle, {modulePositions}, initialPose),
+      m_kinematicsImpl(kinematics) {
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
 }
-
-template <size_t NumModules>
-void SwerveDriveOdometry<NumModules>::ResetPosition(
-    const Rotation2d& gyroAngle,
-    const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
-    const Pose2d& pose) {
-  m_pose = pose;
-  m_previousAngle = pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    m_previousModulePositions[i].distance = modulePositions[i].distance;
-  }
-}
-
-template <size_t NumModules>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
-    const Rotation2d& gyroAngle,
-    const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-  auto moduleDeltas =
-      wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
-  for (size_t index = 0; index < NumModules; index++) {
-    auto lastPosition = m_previousModulePositions[index];
-    auto currentPosition = modulePositions[index];
-    moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
-                           currentPosition.angle};
-
-    m_previousModulePositions[index].distance = modulePositions[index].distance;
-  }
-
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto twist = m_kinematics.ToTwist2d(moduleDeltas);
-  twist.dtheta = (angle - m_previousAngle).Radians();
-
-  auto newPose = m_pose.Exp(twist);
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h
new file mode 100644
index 0000000..c1686d2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a swerve drive drivetrain.
+ */
+template <size_t NumModules>
+struct WPILIB_DLLEXPORT SwerveDriveWheelPositions {
+  /**
+   * The distances driven by the wheels.
+   */
+  wpi::array<SwerveModulePosition, NumModules> positions;
+
+  /**
+   * Checks equality between this SwerveDriveWheelPositions and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveDriveWheelPositions& other) const = default;
+
+  /**
+   * Checks inequality between this SwerveDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const SwerveDriveWheelPositions& other) const = default;
+
+  SwerveDriveWheelPositions<NumModules> Interpolate(
+      const SwerveDriveWheelPositions<NumModules>& endValue, double t) const {
+    auto result =
+        wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+    for (size_t i = 0; i < NumModules; i++) {
+      result[i] = positions[i].Interpolate(endValue.positions[i], t);
+    }
+    return {result};
+  }
+};
+}  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
index 18ed464..93f7465 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 
 #include "frc/geometry/Rotation2d.h"
@@ -25,5 +26,19 @@
    * Angle of the module.
    */
   Rotation2d angle;
+
+  /**
+   * Checks equality between this SwerveModulePosition and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveModulePosition& other) const;
+
+  SwerveModulePosition Interpolate(const SwerveModulePosition& endValue,
+                                   double t) const {
+    return {wpi::Lerp(distance, endValue.distance, t),
+            wpi::Lerp(angle, endValue.angle, t)};
+  }
 };
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
index cae2d53..2f95d9b 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h

@@ -27,6 +27,14 @@
   Rotation2d angle;
 
   /**
+   * Checks equality between this SwerveModuleState and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveModuleState& other) const;
+
+  /**
    * Minimize the change in heading the desired swerve module state would
    * require by potentially reversing the direction the wheel spins. If this is
    * used with the PIDController class's continuous input functionality, the
@@ -36,13 +44,6 @@
    * @param currentAngle The current module angle.
    */
   static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
-                                    const Rotation2d& currentAngle) {
-    auto delta = desiredState.angle - currentAngle;
-    if (units::math::abs(delta.Degrees()) > 90_deg) {
-      return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
-    } else {
-      return {desiredState.speed, desiredState.angle};
-    }
-  }
+                                    const Rotation2d& currentAngle);
 };
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h
new file mode 100644
index 0000000..8867f66
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+
+namespace frc {
+template <typename T>
+concept WheelPositions =
+    std::copy_constructible<T> && requires(T a, T b, double t) {
+      { a.Interpolate(b, t) } -> std::convertible_to<T>;
+    };
+}  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
index 0636707..1d6aaeb 100644
--- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h

@@ -49,7 +49,7 @@
    * Returns the hermite basis matrix for cubic hermite spline interpolation.
    * @return The hermite basis matrix for cubic hermite spline interpolation.
    */
-  static Matrixd<4, 4> MakeHermiteBasis() {
+  static Eigen::Matrix4d MakeHermiteBasis() {
     // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
     // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
     //
@@ -71,10 +71,10 @@
     // [a₁] = [ 0  1  0  0][P(i+1) ]
     // [a₀] = [ 1  0  0  0][P'(i+1)]
 
-    static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
-                                     {-3.0, -2.0, +3.0, -1.0},
-                                     {+0.0, +1.0, +0.0, +0.0},
-                                     {+1.0, +0.0, +0.0, +0.0}};
+    static const Eigen::Matrix4d basis{{+2.0, +1.0, -2.0, +1.0},
+                                       {-3.0, -2.0, +3.0, -1.0},
+                                       {+0.0, +1.0, +0.0, +0.0},
+                                       {+1.0, +0.0, +0.0, +0.0}};
     return basis;
   }
 

diff --git a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
index 0720cd1..e1d2d52 100644
--- a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
+++ b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h

@@ -56,7 +56,7 @@
   };
 
   /**
-   * Parameterizes the spline. This method breaks up the spline into various
+   * Parametrizes the spline. This method breaks up the spline into various
    * arcs until their dx, dy, and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.

diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h
index 957b875..72a3d43 100644
--- a/wpimath/src/main/native/include/frc/system/Discretization.h
+++ b/wpimath/src/main/native/include/frc/system/Discretization.h

@@ -4,9 +4,10 @@
 
 #pragma once
 
+#include <unsupported/Eigen/MatrixFunctions>
+
 #include "frc/EigenCore.h"
 #include "units/time.h"
-#include "unsupported/Eigen/MatrixFunctions"
 
 namespace frc {
 
@@ -44,9 +45,9 @@
   // M = [A  B]
   //     [0  0]
   Matrixd<States + Inputs, States + Inputs> M;
-  M.setZero();
   M.template block<States, States>(0, 0) = contA;
   M.template block<States, Inputs>(0, States) = contB;
+  M.template block<Inputs, States + Inputs>(States, 0).setZero();
 
   // ϕ = eᴹᵀ = [A_d  B_d]
   //           [ 0    I ]
@@ -101,95 +102,6 @@
 }
 
 /**
- * Discretizes the given continuous A and Q matrices.
- *
- * Rather than solving a 2N x 2N matrix exponential like in DiscretizeAQ()
- * (which is expensive), we take advantage of the structure of the block matrix
- * of A and Q.
- *
- * <ul>
- *   <li>eᴬᵀ, which is only N x N, is relatively cheap.
- *   <li>The upper-right quarter of the 2N x 2N matrix, which we can approximate
- *       using a taylor series to several terms and still be substantially
- *       cheaper than taking the big exponential.
- * </ul>
- *
- * @tparam States Number of states.
- * @param contA Continuous system matrix.
- * @param contQ Continuous process noise covariance matrix.
- * @param dt    Discretization timestep.
- * @param discA Storage for discrete system matrix.
- * @param discQ Storage for discrete process noise covariance matrix.
- */
-template <int States>
-void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
-                        const Matrixd<States, States>& contQ,
-                        units::second_t dt, Matrixd<States, States>* discA,
-                        Matrixd<States, States>* discQ) {
-  //       T
-  // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-  //       0
-  //
-  // M = [−A  Q ]
-  //     [ 0  Aᵀ]
-  // ϕ = eᴹᵀ
-  // ϕ₁₂ = A_d⁻¹Q_d
-  //
-  // Taylor series of ϕ:
-  //
-  //   ϕ = eᴹᵀ = I + MT + 1/2 M²T² + 1/6 M³T³ + …
-  //   ϕ = eᴹᵀ = I + MT + 1/2 T²M² + 1/6 T³M³ + …
-  //
-  // Taylor series of ϕ expanded for ϕ₁₂:
-  //
-  //   ϕ₁₂ = 0 + QT + 1/2 T² (−AQ + QAᵀ) + 1/6 T³ (−A lastTerm + Q Aᵀ²) + …
-  //
-  // ```
-  // lastTerm = Q
-  // lastCoeff = T
-  // ATn = Aᵀ
-  // ϕ₁₂ = lastTerm lastCoeff = QT
-  //
-  // for i in range(2, 6):
-  //   // i = 2
-  //   lastTerm = −A lastTerm + Q ATn = −AQ + QAᵀ
-  //   lastCoeff *= T/i → lastCoeff *= T/2 = 1/2 T²
-  //   ATn *= Aᵀ = Aᵀ²
-  //
-  //   // i = 3
-  //   lastTerm = −A lastTerm + Q ATn = −A (−AQ + QAᵀ) + QAᵀ² = …
-  //   …
-  // ```
-
-  // Make continuous Q symmetric if it isn't already
-  Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
-
-  Matrixd<States, States> lastTerm = Q;
-  double lastCoeff = dt.value();
-
-  // Aᵀⁿ
-  Matrixd<States, States> ATn = contA.transpose();
-
-  Matrixd<States, States> phi12 = lastTerm * lastCoeff;
-
-  // i = 6 i.e. 5th order should be enough precision
-  for (int i = 2; i < 6; ++i) {
-    lastTerm = -contA * lastTerm + Q * ATn;
-    lastCoeff *= dt.value() / static_cast<double>(i);
-
-    phi12 += lastTerm * lastCoeff;
-
-    ATn *= contA.transpose();
-  }
-
-  DiscretizeA<States>(contA, dt, discA);
-  Q = *discA * phi12;
-
-  // Make discrete Q symmetric if it isn't already
-  *discQ = (Q + Q.transpose()) / 2.0;
-}
-
-/**
  * Returns a discretized version of the provided continuous measurement noise
  * covariance matrix.
  *

diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index 1300a82..c61531a 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <functional>
+
 #include <wpi/SymbolExports.h>
 
 #include "frc/EigenCore.h"

diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
index bb856ec..98b6cc3 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h

@@ -4,12 +4,10 @@
 
 #pragma once
 
-#include <frc/StateSpaceUtil.h>
-
 #include <algorithm>
 #include <array>
+#include <cmath>
 
-#include "Eigen/Core"
 #include "units/time.h"
 
 namespace frc {
@@ -122,7 +120,11 @@
                               (b1[6] - b2[6]) * k7))
                             .norm();
 
-      h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+      if (truncationError == 0.0) {
+        h = dt.value() - dtElapsed;
+      } else {
+        h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+      }
     } while (truncationError > maxError);
 
     dtElapsed += h;

diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
index 831f532..ad711ee 100644
--- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
+++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h

@@ -97,7 +97,7 @@
   }
 
   /**
-   * Returns the speed produced by the motor at a given torque and input
+   * Returns the angular speed produced by the motor at a given torque and input
    * voltage.
    *
    * @param torque        The torque produced by the motor.
@@ -119,89 +119,123 @@
   }
 
   /**
-   * Returns instance of CIM.
+   * Returns a gearbox of CIM motors.
    */
   static constexpr DCMotor CIM(int numMotors = 1) {
     return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors);
   }
 
   /**
-   * Returns instance of MiniCIM.
+   * Returns a gearbox of MiniCIM motors.
    */
   static constexpr DCMotor MiniCIM(int numMotors = 1) {
     return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Bag motor.
+   * Returns a gearbox of Bag motor motors.
    */
   static constexpr DCMotor Bag(int numMotors = 1) {
     return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Vex 775 Pro.
+   * Returns a gearbox of Vex 775 Pro motors.
    */
   static constexpr DCMotor Vex775Pro(int numMotors = 1) {
     return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Andymark RS 775-125.
+   * Returns a gearbox of Andymark RS 775-125 motors.
    */
   static constexpr DCMotor RS775_125(int numMotors = 1) {
     return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Banebots RS 775.
+   * Returns a gearbox of Banebots RS 775 motors.
    */
   static constexpr DCMotor BanebotsRS775(int numMotors = 1) {
     return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Andymark 9015.
+   * Returns a gearbox of Andymark 9015 motors.
    */
   static constexpr DCMotor Andymark9015(int numMotors = 1) {
     return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Banebots RS 550.
+   * Returns a gearbox of Banebots RS 550 motors.
    */
   static constexpr DCMotor BanebotsRS550(int numMotors = 1) {
     return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors);
   }
 
   /**
-   * Returns instance of NEO brushless motor.
+   * Returns a gearbox of NEO brushless motors.
    */
   static constexpr DCMotor NEO(int numMotors = 1) {
     return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors);
   }
 
   /**
-   * Returns instance of NEO 550 brushless motor.
+   * Returns a gearbox of NEO 550 brushless motors.
    */
   static constexpr DCMotor NEO550(int numMotors = 1) {
     return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Falcon 500 brushless motor.
+   * Returns a gearbox of Falcon 500 brushless motors.
    */
   static constexpr DCMotor Falcon500(int numMotors = 1) {
     return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
   }
 
   /**
+   * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control)
+   * enabled.
+   */
+  static constexpr DCMotor Falcon500FOC(int numMotors = 1) {
+    // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/
+    return DCMotor(12_V, 5.84_Nm, 304_A, 1.5_A, 6080_rpm, numMotors);
+  }
+
+  /**
    * Return a gearbox of Romi/TI_RSLK MAX motors.
    */
   static constexpr DCMotor RomiBuiltIn(int numMotors = 1) {
     // From https://www.pololu.com/product/1520/specs
     return DCMotor(4.5_V, 0.1765_Nm, 1.25_A, 0.13_A, 150_rpm, numMotors);
   }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors.
+   */
+  static constexpr DCMotor KrakenX60(int numMotors = 1) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return DCMotor(12_V, 7.09_Nm, 366_A, 2_A, 6000_rpm, numMotors);
+  }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented
+   * Control) enabled.
+   */
+  static constexpr DCMotor KrakenX60FOC(int numMotors = 1) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return DCMotor(12_V, 9.37_Nm, 483_A, 2_A, 5800_rpm, numMotors);
+  }
+
+  /**
+   * Return a gearbox of Neo Vortex brushless motors.
+   */
+  static constexpr DCMotor NeoVortex(int numMotors = 1) {
+    // From https://www.revrobotics.com/next-generation-spark-neo/
+    return DCMotor(12_V, 3.60_Nm, 211_A, 3.615_A, 6784_rpm, numMotors);
+  }
 };
 
 }  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
index 3e69545..64f3496 100644
--- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <stdexcept>
 
 #include <wpi/SymbolExports.h>
@@ -76,9 +77,9 @@
    * @param kA The acceleration gain, in volts/(unit/sec²).
    * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
-  template <typename Distance, typename = std::enable_if_t<
-                                   std::is_same_v<units::meter, Distance> ||
-                                   std::is_same_v<units::radian, Distance>>>
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
   static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
@@ -117,9 +118,9 @@
    *
    * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
-  template <typename Distance, typename = std::enable_if_t<
-                                   std::is_same_v<units::meter, Distance> ||
-                                   std::is_same_v<units::radian, Distance>>>
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
   static LinearSystem<2, 1, 1> IdentifyPositionSystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
@@ -217,6 +218,48 @@
   static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
                                              units::kilogram_square_meter_t J,
                                              double G);
+
+  /**
+   * Create a state-space model of a DC motor system from its kV
+   * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
+   * found using SysId. the states of the system are [position, velocity],
+   * inputs are [voltage], and outputs are [position].
+   *
+   * You MUST use an SI unit (i.e. meters or radians) for the Distance template
+   * argument. You may still use non-SI units (such as feet or inches) for the
+   * actual method arguments; they will automatically be converted to SI
+   * internally.
+   *
+   * The parameters provided by the user are from this feedforward model:
+   *
+   * u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volts/(unit/sec).
+   * @param kA The acceleration gain, in volts/(unit/sec²).
+   *
+   * @throws std::domain_error if kV <= 0 or kA <= 0.
+   */
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
+  static LinearSystem<2, 1, 2> DCMotorSystem(
+      decltype(1_V / Velocity_t<Distance>(1)) kV,
+      decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+    if (kV <= decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than zero.");
+    }
+    if (kA <= decltype(kA){0}) {
+      throw std::domain_error("Ka must be greater than zero.");
+    }
+
+    Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
+    Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
+    Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+    Matrixd<2, 1> D{{0.0}, {0.0}};
+
+    return LinearSystem<2, 1, 2>(A, B, C, D);
+  }
+
   /**
    * Create a state-space model of differential drive drivetrain. In this model,
    * the states are [left velocity, right velocity], the inputs are [left

diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
new file mode 100644
index 0000000..f45987b
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h

@@ -0,0 +1,194 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A Exponential-shaped velocity profile.
+ *
+ * While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on
+ * ExponentialProfile velocity constraints. To compute the reference obeying
+ * this constraint, do the following.
+ *
+ * Initialization:
+ * @code{.cpp}
+ * ExponentialProfile::Constraints constraints{kMaxV, kV, kA};
+ * State previousProfiledReference = {initialReference, 0_mps};
+ * @endcode
+ *
+ * Run on update:
+ * @code{.cpp}
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
+ * previousProfiledReference, unprofiledReference);
+ * @endcode
+ *
+ * where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `Calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * Otherwise, a timer can be started to provide monotonic values for
+ * `Calculate()` and to determine when the profile has completed via
+ * `IsFinished()`.
+ */
+template <class Distance, class Input>
+class ExponentialProfile {
+ public:
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Input_t = units::unit_t<Input>;
+  using A_t = units::unit_t<units::inverse<units::seconds>>;
+  using B_t =
+      units::unit_t<units::compound_unit<Acceleration, units::inverse<Input>>>;
+  using KV = units::compound_unit<Input, units::inverse<Velocity>>;
+  using kV_t = units::unit_t<KV>;
+  using KA = units::compound_unit<Input, units::inverse<Acceleration>>;
+  using kA_t = units::unit_t<KA>;
+
+  class Constraints {
+   public:
+    Constraints(Input_t maxInput, A_t A, B_t B)
+        : maxInput{maxInput}, A{A}, B{B} {}
+    Constraints(Input_t maxInput, kV_t kV, kA_t kA)
+        : maxInput{maxInput}, A{-kV / kA}, B{1 / kA} {}
+    Velocity_t MaxVelocity() const { return -maxInput * B / A; }
+
+    Input_t maxInput{0};
+    A_t A{0};
+    B_t B{0};
+  };
+
+  class State {
+   public:
+    Distance_t position{0};
+    Velocity_t velocity{0};
+    bool operator==(const State&) const = default;
+  };
+
+  class ProfileTiming {
+   public:
+    units::second_t inflectionTime;
+    units::second_t totalTime;
+
+    bool IsFinished(const units::second_t& time) const {
+      return time > totalTime;
+    }
+  };
+
+  /**
+   * Construct a ExponentialProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum input.
+   */
+  explicit ExponentialProfile(Constraints constraints);
+
+  ExponentialProfile(const ExponentialProfile&) = default;
+  ExponentialProfile& operator=(const ExponentialProfile&) = default;
+  ExponentialProfile(ExponentialProfile&&) = default;
+  ExponentialProfile& operator=(ExponentialProfile&&) = default;
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the current state is at time t = 0.
+   */
+  State Calculate(const units::second_t& t, const State& current,
+                  const State& goal) const;
+
+  /**
+   * Calculate the point after which the fastest way to reach the goal state is
+   * to apply input in the opposite direction.
+   */
+  State CalculateInflectionPoint(const State& current, const State& goal) const;
+
+  /**
+   * Calculate the time it will take for this profile to reach the goal state.
+   */
+  units::second_t TimeLeftUntil(const State& current, const State& goal) const;
+
+  /**
+   * Calculate the time it will take for this profile to reach the inflection
+   * point, and the time it will take for this profile to reach the goal state.
+   */
+  ProfileTiming CalculateProfileTiming(const State& current,
+                                       const State& goal) const;
+
+ private:
+  /**
+   * Calculate the point after which the fastest way to reach the goal state is
+   * to apply input in the opposite direction.
+   */
+  State CalculateInflectionPoint(const State& current, const State& goal,
+                                 const Input_t& input) const;
+
+  /**
+   * Calculate the time it will take for this profile to reach the inflection
+   * point, and the time it will take for this profile to reach the goal state.
+   */
+  ProfileTiming CalculateProfileTiming(const State& current,
+                                       const State& inflectionPoint,
+                                       const State& goal,
+                                       const Input_t& input) const;
+
+  /**
+   * Calculate the velocity reached after t seconds when applying an input from
+   * the initial state.
+   */
+  Velocity_t ComputeVelocityFromTime(const units::second_t& time,
+                                     const Input_t& input,
+                                     const State& initial) const;
+
+  /**
+   * Calculate the position reached after t seconds when applying an input from
+   * the initial state.
+   */
+  Distance_t ComputeDistanceFromTime(const units::second_t& time,
+                                     const Input_t& input,
+                                     const State& initial) const;
+
+  /**
+   * Calculate the distance reached at the same time as the given velocity when
+   * applying the given input from the initial state.
+   */
+  Distance_t ComputeDistanceFromVelocity(const Velocity_t& velocity,
+                                         const Input_t& input,
+                                         const State& initial) const;
+
+  /**
+   * Calculate the time required to reach a specified velocity given the initial
+   * velocity.
+   */
+  units::second_t ComputeTimeFromVelocity(const Velocity_t& velocity,
+                                          const Input_t& input,
+                                          const Velocity_t& initial) const;
+
+  /**
+   * Calculate the velocity at which input should be reversed in order to reach
+   * the goal state from the current state.
+   */
+  Velocity_t SolveForInflectionVelocity(const Input_t& input,
+                                        const State& current,
+                                        const State& goal) const;
+
+  /**
+   * Returns true if the profile should be inverted.
+   *
+   * <p>The profile is inverted if we should first apply negative input in order
+   * to reach the goal state.
+   */
+  bool ShouldFlipInput(const State& current, const State& goal) const;
+
+  Constraints m_constraints;
+};
+}  // namespace frc
+
+#include "ExponentialProfile.inc"

diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc
new file mode 100644
index 0000000..ded5245
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc

@@ -0,0 +1,253 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+
+#include <fmt/core.h>
+
+#include "frc/trajectory/ExponentialProfile.h"
+#include "units/math.h"
+
+namespace frc {
+template <class Distance, class Input>
+ExponentialProfile<Distance, Input>::ExponentialProfile(Constraints constraints)
+    : m_constraints(constraints) {}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::Calculate(const units::second_t& t,
+                                               const State& current,
+                                               const State& goal) const {
+  auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+  auto u = direction * m_constraints.maxInput;
+
+  auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
+  auto timing = CalculateProfileTiming(current, inflectionPoint, goal, u);
+
+  if (t < 0_s) {
+    return current;
+  } else if (t < timing.inflectionTime) {
+    return {ComputeDistanceFromTime(t, u, current),
+            ComputeVelocityFromTime(t, u, current)};
+  } else if (t < timing.totalTime) {
+    return {ComputeDistanceFromTime(t - timing.totalTime, -u, goal),
+            ComputeVelocityFromTime(t - timing.totalTime, -u, goal)};
+  } else {
+    return goal;
+  }
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
+    const State& current, const State& goal) const {
+  auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+  auto u = direction * m_constraints.maxInput;
+
+  return CalculateInflectionPoint(current, goal, u);
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
+    const State& current, const State& goal, const Input_t& input) const {
+  auto u = input;
+
+  if (current == goal) {
+    return current;
+  }
+
+  auto inflectionVelocity = SolveForInflectionVelocity(u, current, goal);
+  auto inflectionPosition =
+      ComputeDistanceFromVelocity(inflectionVelocity, -u, goal);
+
+  return {inflectionPosition, inflectionVelocity};
+}
+
+template <class Distance, class Input>
+units::second_t ExponentialProfile<Distance, Input>::TimeLeftUntil(
+    const State& current, const State& goal) const {
+  auto timing = CalculateProfileTiming(current, goal);
+
+  return timing.totalTime;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::ProfileTiming
+ExponentialProfile<Distance, Input>::CalculateProfileTiming(
+    const State& current, const State& goal) const {
+  auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+  auto u = direction * m_constraints.maxInput;
+
+  auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
+  return CalculateProfileTiming(current, inflectionPoint, goal, u);
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::ProfileTiming
+ExponentialProfile<Distance, Input>::CalculateProfileTiming(
+    const State& current, const State& inflectionPoint, const State& goal,
+    const Input_t& input) const {
+  auto u = input;
+  auto u_dir = units::math::abs(u) / u;
+
+  units::second_t inflectionT_forward;
+
+  // We need to handle 5 cases here:
+  //
+  // - Approaching -maxVelocity from below
+  // - Approaching -maxVelocity from above
+  // - Approaching maxVelocity from below
+  // - Approaching maxVelocity from above
+  // - At +-maxVelocity
+  //
+  // For cases 1 and 3, we want to subtract epsilon from the inflection point
+  // velocity For cases 2 and 4, we want to add epsilon to the inflection point
+  // velocity. For case 5, we have reached inflection point velocity.
+  auto epsilon = Velocity_t(1e-9);
+  if (units::math::abs(u_dir * m_constraints.MaxVelocity() -
+                       inflectionPoint.velocity) < epsilon) {
+    auto solvableV = inflectionPoint.velocity;
+    units::second_t t_to_solvable_v;
+    Distance_t x_at_solvable_v;
+    if (units::math::abs(current.velocity - inflectionPoint.velocity) <
+        epsilon) {
+      t_to_solvable_v = 0_s;
+      x_at_solvable_v = current.position;
+    } else {
+      if (units::math::abs(current.velocity) > m_constraints.MaxVelocity()) {
+        solvableV += u_dir * epsilon;
+      } else {
+        solvableV -= u_dir * epsilon;
+      }
+
+      t_to_solvable_v = ComputeTimeFromVelocity(solvableV, u, current.velocity);
+      x_at_solvable_v = ComputeDistanceFromVelocity(solvableV, u, current);
+    }
+
+    inflectionT_forward =
+        t_to_solvable_v + u_dir * (inflectionPoint.position - x_at_solvable_v) /
+                              m_constraints.MaxVelocity();
+  } else {
+    inflectionT_forward =
+        ComputeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
+  }
+
+  auto inflectionT_backward =
+      ComputeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
+
+  return {inflectionT_forward, inflectionT_forward - inflectionT_backward};
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Distance_t
+ExponentialProfile<Distance, Input>::ComputeDistanceFromTime(
+    const units::second_t& time, const Input_t& input,
+    const State& initial) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  return initial.position +
+         (-B * u * time +
+          (initial.velocity + B * u / A) * (units::math::exp(A * time) - 1)) /
+             A;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Velocity_t
+ExponentialProfile<Distance, Input>::ComputeVelocityFromTime(
+    const units::second_t& time, const Input_t& input,
+    const State& initial) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  return (initial.velocity + B * u / A) * units::math::exp(A * time) -
+         B * u / A;
+}
+
+template <class Distance, class Input>
+units::second_t ExponentialProfile<Distance, Input>::ComputeTimeFromVelocity(
+    const Velocity_t& velocity, const Input_t& input,
+    const Velocity_t& initial) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  return units::math::log((A * velocity + B * u) / (A * initial + B * u)) / A;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Distance_t
+ExponentialProfile<Distance, Input>::ComputeDistanceFromVelocity(
+    const Velocity_t& velocity, const Input_t& input,
+    const State& initial) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  return initial.position + (velocity - initial.velocity) / A -
+         B * u / (A * A) *
+             units::math::log((A * velocity + B * u) /
+                              (A * initial.velocity + B * u));
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Velocity_t
+ExponentialProfile<Distance, Input>::SolveForInflectionVelocity(
+    const Input_t& input, const State& current, const State& goal) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  auto u_dir = u / units::math::abs(u);
+
+  auto position_delta = goal.position - current.position;
+  auto velocity_delta = goal.velocity - current.velocity;
+
+  auto scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
+  auto power = -A / B / u * (A * position_delta - velocity_delta);
+
+  auto a = -A * A;
+  auto c = B * B * u * u + scalar * units::math::exp(power);
+
+  if (-1e-9 < c.value() && c.value() < 0) {
+    // numeric instability - the heuristic gets it right but c is around -1e-13
+    return Velocity_t(0);
+  }
+
+  return u_dir * units::math::sqrt(-c / a);
+}
+
+template <class Distance, class Input>
+bool ExponentialProfile<Distance, Input>::ShouldFlipInput(
+    const State& current, const State& goal) const {
+  auto u = m_constraints.maxInput;
+
+  auto v0 = current.velocity;
+  auto xf = goal.position;
+  auto vf = goal.velocity;
+
+  auto x_forward = ComputeDistanceFromVelocity(vf, u, current);
+  auto x_reverse = ComputeDistanceFromVelocity(vf, -u, current);
+
+  if (v0 >= m_constraints.MaxVelocity()) {
+    return xf < x_reverse;
+  }
+
+  if (v0 <= -m_constraints.MaxVelocity()) {
+    return xf < x_forward;
+  }
+
+  auto a = v0 >= Velocity_t(0);
+  auto b = vf >= Velocity_t(0);
+  auto c = xf >= x_forward;
+  auto d = xf >= x_reverse;
+
+  return (a && !d) || (b && !c) || (!c && !d);
+}
+}  // namespace frc

diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
index f5ee79b..ca97593 100644
--- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h

@@ -7,6 +7,7 @@
 #include <vector>
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Transform2d.h"
@@ -15,10 +16,6 @@
 #include "units/time.h"
 #include "units/velocity.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 /**
  * Represents a time-parameterized trajectory. The trajectory contains of
@@ -66,6 +63,8 @@
 
   /**
    * Constructs a trajectory from a vector of states.
+   *
+   * @throws std::invalid_argument if the vector of states is empty.
    */
   explicit Trajectory(const std::vector<State>& states);
 
@@ -77,6 +76,7 @@
 
   /**
    * Return the states of the trajectory.
+   *
    * @return The states of the trajectory.
    */
   const std::vector<State>& States() const { return m_states; }
@@ -86,6 +86,7 @@
    *
    * @param t The point in time since the beginning of the trajectory to sample.
    * @return The state at that point in time.
+   * @throws std::runtime_error if the trajectory has no states.
    */
   State Sample(units::second_t t) const;
 

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
index b1a0b52..4e6c1e6 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -74,8 +75,7 @@
    * Adds a user-defined constraint to the trajectory.
    * @param constraint The user-defined constraint.
    */
-  template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                     TrajectoryConstraint, Constraint>>>
+  template <std::derived_from<TrajectoryConstraint> Constraint>
   void AddConstraint(Constraint constraint) {
     m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
   }

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 24a8253..73aab38 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <wpi/deprecated.h>
+
 #include "units/time.h"
 #include "wpimath/MathShared.h"
 
@@ -21,13 +23,14 @@
  * @code{.cpp}
  * TrapezoidProfile::Constraints constraints{kMaxV, kMaxA};
  * double previousProfiledReference = initialReference;
+ * TrapezoidProfile profile{constraints};
  * @endcode
  *
  * Run on update:
  * @code{.cpp}
- * TrapezoidProfile profile{constraints, unprofiledReference,
- *                          previousProfiledReference};
- * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
+ *                                               unprofiledReference,
+ *                                               previousProfiledReference);
  * @endcode
  *
  * where `unprofiledReference` is free to change between calls. Note that when
@@ -75,9 +78,22 @@
    * Construct a TrapezoidProfile.
    *
    * @param constraints The constraints on the profile, like maximum velocity.
+   */
+  TrapezoidProfile(Constraints constraints);  // NOLINT
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
    * @param goal        The desired state when the profile is complete.
    * @param initial     The initial state (usually the current state).
+   * @deprecated Pass the desired and current state into calculate instead of
+   * constructing a new TrapezoidProfile with the desired and current state
    */
+  WPI_DEPRECATED(
+      "Pass the desired and current state into calculate instead of "
+      "constructing a new TrapezoidProfile with the desired and current "
+      "state")
   TrapezoidProfile(Constraints constraints, State goal,
                    State initial = State{Distance_t{0}, Velocity_t{0}});
 
@@ -91,10 +107,26 @@
    * where the beginning of the profile was at time t = 0.
    *
    * @param t The time since the beginning of the profile.
+   * @deprecated Pass the desired and current state into calculate instead of
+   * constructing a new TrapezoidProfile with the desired and current state
    */
+  [[deprecated(
+      "Pass the desired and current state into calculate instead of "
+      "constructing a new TrapezoidProfile with the desired and current "
+      "state")]]
   State Calculate(units::second_t t) const;
 
   /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the beginning of the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   * @param goal        The desired state when the profile is complete.
+   * @param current     The initial state (usually the current state).
+   */
+  State Calculate(units::second_t t, State goal, State current);
+
+  /**
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
@@ -141,8 +173,9 @@
   int m_direction;
 
   Constraints m_constraints;
-  State m_initial;
-  State m_goal;
+  State m_current;
+  State m_goal;   // TODO: remove
+  bool m_newAPI;  // TODO: remove
 
   units::second_t m_endAccel;
   units::second_t m_endFullSpeed;

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 19eb1f3..24e0a46 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc

@@ -11,21 +11,26 @@
 
 namespace frc {
 template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints)
+    : m_constraints(constraints), m_newAPI(true) {}
+
+template <class Distance>
 TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
                                              State goal, State initial)
     : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
       m_constraints(constraints),
-      m_initial(Direct(initial)),
-      m_goal(Direct(goal)) {
-  if (m_initial.velocity > m_constraints.maxVelocity) {
-    m_initial.velocity = m_constraints.maxVelocity;
+      m_current(Direct(initial)),
+      m_goal(Direct(goal)),
+      m_newAPI(false) {
+  if (m_current.velocity > m_constraints.maxVelocity) {
+    m_current.velocity = m_constraints.maxVelocity;
   }
 
   // Deal with a possibly truncated motion profile (with nonzero initial or
   // final velocity) by calculating the parameters as if the profile began and
   // ended at zero velocity
   units::second_t cutoffBegin =
-      m_initial.velocity / m_constraints.maxAcceleration;
+      m_current.velocity / m_constraints.maxAcceleration;
   Distance_t cutoffDistBegin =
       cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
 
@@ -37,7 +42,7 @@
   // of a truncated one
 
   Distance_t fullTrapezoidDist =
-      cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+      cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
   units::second_t accelerationTime =
       m_constraints.maxVelocity / m_constraints.maxAcceleration;
 
@@ -60,15 +65,19 @@
 template <class Distance>
 typename TrapezoidProfile<Distance>::State
 TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
-  State result = m_initial;
+  if (m_newAPI) {
+    throw std::runtime_error(
+        "Cannot use new constructor with deprecated Calculate()");
+  }
+  State result = m_current;
 
   if (t < m_endAccel) {
     result.velocity += t * m_constraints.maxAcceleration;
     result.position +=
-        (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+        (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
   } else if (t < m_endFullSpeed) {
     result.velocity = m_constraints.maxVelocity;
-    result.position += (m_initial.velocity +
+    result.position += (m_current.velocity +
                         m_endAccel * m_constraints.maxAcceleration / 2.0) *
                            m_endAccel +
                        m_constraints.maxVelocity * (t - m_endAccel);
@@ -86,12 +95,83 @@
 
   return Direct(result);
 }
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
+                                      State current) {
+  m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
+  m_current = Direct(current);
+  goal = Direct(goal);
+  if (m_current.velocity > m_constraints.maxVelocity) {
+    m_current.velocity = m_constraints.maxVelocity;
+  }
+
+  // Deal with a possibly truncated motion profile (with nonzero initial or
+  // final velocity) by calculating the parameters as if the profile began and
+  // ended at zero velocity
+  units::second_t cutoffBegin =
+      m_current.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistBegin =
+      cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+  units::second_t cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistEnd =
+      cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+  // Now we can calculate the parameters as if it was a full trapezoid instead
+  // of a truncated one
+
+  Distance_t fullTrapezoidDist =
+      cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
+  units::second_t accelerationTime =
+      m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+  Distance_t fullSpeedDist =
+      fullTrapezoidDist -
+      accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+  // Handle the case where the profile never reaches full speed
+  if (fullSpeedDist < Distance_t{0}) {
+    accelerationTime =
+        units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+    fullSpeedDist = Distance_t{0};
+  }
+
+  m_endAccel = accelerationTime - cutoffBegin;
+  m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+  m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+  State result = m_current;
+
+  if (t < m_endAccel) {
+    result.velocity += t * m_constraints.maxAcceleration;
+    result.position +=
+        (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+  } else if (t < m_endFullSpeed) {
+    result.velocity = m_constraints.maxVelocity;
+    result.position += (m_current.velocity +
+                        m_endAccel * m_constraints.maxAcceleration / 2.0) *
+                           m_endAccel +
+                       m_constraints.maxVelocity * (t - m_endAccel);
+  } else if (t <= m_endDeccel) {
+    result.velocity =
+        goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+    units::second_t timeLeft = m_endDeccel - t;
+    result.position =
+        goal.position -
+        (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
+            timeLeft;
+  } else {
+    result = goal;
+  }
+
+  return Direct(result);
+}
 
 template <class Distance>
 units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
     Distance_t target) const {
-  Distance_t position = m_initial.position * m_direction;
-  Velocity_t velocity = m_initial.velocity * m_direction;
+  Distance_t position = m_current.position * m_direction;
+  Velocity_t velocity = m_current.velocity * m_direction;
 
   units::second_t endAccel = m_endAccel * m_direction;
   units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
index 0edd8cc..0d96893 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h

@@ -20,9 +20,8 @@
 class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint
     : public TrajectoryConstraint {
  public:
-  DifferentialDriveKinematicsConstraint(
-      const DifferentialDriveKinematics& kinematics,
-      units::meters_per_second_t maxSpeed);
+  DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics,
+                                        units::meters_per_second_t maxSpeed);
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
index 40a0d8e..8f7ba2c 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h

@@ -34,7 +34,7 @@
    */
   DifferentialDriveVoltageConstraint(
       const SimpleMotorFeedforward<units::meter>& feedforward,
-      const DifferentialDriveKinematics& kinematics, units::volt_t maxVoltage);
+      DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
index f9f0d2e..74f3c55 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <limits>
 
 #include "frc/geometry/Rotation2d.h"
@@ -15,8 +16,7 @@
 /**
  * Enforces a particular constraint only within an elliptical region.
  */
-template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                   TrajectoryConstraint, Constraint>>>
+template <std::derived_from<TrajectoryConstraint> Constraint>
 class EllipticalRegionConstraint : public TrajectoryConstraint {
  public:
   /**

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
index 18522fe..f3b364b 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <limits>
 
 #include "frc/geometry/Rotation2d.h"
@@ -14,8 +15,7 @@
 /**
  * Enforces a particular constraint only within a rectangular region.
  */
-template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                   TrajectoryConstraint, Constraint>>>
+template <std::derived_from<TrajectoryConstraint> Constraint>
 class RectangularRegionConstraint : public TrajectoryConstraint {
  public:
   /**

diff --git a/wpimath/src/main/native/include/units/angular_acceleration.h b/wpimath/src/main/native/include/units/angular_acceleration.h
index 632982b..8174dac 100644
--- a/wpimath/src/main/native/include/units/angular_acceleration.h
+++ b/wpimath/src/main/native/include/units/angular_acceleration.h

@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include "units/angular_velocity.h"
+#include "units/angle.h"
 #include "units/base.h"
 #include "units/time.h"
 

diff --git a/wpimath/src/main/native/include/units/angular_jerk.h b/wpimath/src/main/native/include/units/angular_jerk.h
new file mode 100644
index 0000000..b58bc16
--- /dev/null
+++ b/wpimath/src/main/native/include/units/angular_jerk.h

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "units/angle.h"
+#include "units/base.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::angular_jerk
+ * @brief namespace for unit types and containers representing angular
+ *        jerk values
+ * @details The SI unit for angular jerk is
+ *          `radians_per_second_cubed`, and the corresponding `base_unit`
+ *          category is`angular_jerk_unit`.
+ * @anchor angularJerkContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+UNIT_ADD(angular_jerk, radians_per_second_cubed, radians_per_second_cubed,
+         rad_per_s_cu, unit<std::ratio<1>, units::category::angular_jerk_unit>)
+UNIT_ADD(angular_jerk, degrees_per_second_cubed, degrees_per_second_cubed,
+         deg_per_s_cu,
+         compound_unit<angle::degrees, inverse<cubed<time::seconds>>>)
+UNIT_ADD(angular_jerk, turns_per_second_cubed, turns_per_second_cubed,
+         tr_per_s_cu,
+         compound_unit<angle::turns, inverse<cubed<time::seconds>>>)
+
+UNIT_ADD_CATEGORY_TRAIT(angular_jerk)
+
+using namespace angular_jerk;
+}  // namespace units

diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h
index 9d021b1..34cd59e 100644
--- a/wpimath/src/main/native/include/units/base.h
+++ b/wpimath/src/main/native/include/units/base.h

@@ -76,7 +76,7 @@
 	#include <locale>
 	#include <string>
 #endif
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
 	#include <locale>
 	#include <string>
 	#include <fmt/format.h>
@@ -176,7 +176,7 @@
  * @param		abbrev - abbreviated unit name, e.g. 'm'
  * @note		When UNIT_LIB_ENABLE_IOSTREAM isn't defined, the macro does not generate any code
  */
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
 	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
 	}\
 	template <>\
@@ -463,13 +463,19 @@
 	//----------------------------------
 
 	/**
+	 * @defgroup 	Units Unit API
+	*/
+
+	/**
 	 * @defgroup	UnitContainers Unit Containers
+	 * @ingroup		Units
 	 * @brief		Defines a series of classes which contain dimensioned values. Unit containers
 	 *				store a value, and support various arithmetic operations.
 	 */
 
 	/**
 	 * @defgroup	UnitTypes Unit Types
+	 * @ingroup		Units
 	 * @brief		Defines a series of classes which represent units. These types are tags used by
 	 *				the conversion function, to create compound units, or to create `unit_t` types.
 	 *				By themselves, they are not containers and have no stored value.
@@ -477,6 +483,7 @@
 
 	/**
 	 * @defgroup	UnitManipulators Unit Manipulators
+	 * @ingroup		Units
 	 * @brief		Defines a series of classes used to manipulate unit types, such as `inverse<>`, `squared<>`, and metric prefixes.
 	 *				Unit manipulators can be chained together, e.g. `inverse<squared<pico<time::seconds>>>` to
 	 *				represent picoseconds^-2.
@@ -484,6 +491,7 @@
 
 	 /**
 	  * @defgroup	CompileTimeUnitManipulators Compile-time Unit Manipulators
+	  * @ingroup	Units
 	  * @brief		Defines a series of classes used to manipulate `unit_value_t` types at compile-time, such as `unit_value_add<>`, `unit_value_sqrt<>`, etc.
 	  *				Compile-time manipulators can be chained together, e.g. `unit_value_sqrt<unit_value_add<unit_value_power<a, 2>, unit_value_power<b, 2>>>` to
 	  *				represent `c = sqrt(a^2 + b^2).
@@ -491,17 +499,20 @@
 
 	 /**
 	 * @defgroup	UnitMath Unit Math
+	 * @ingroup		Units
 	 * @brief		Defines a collection of unit-enabled, strongly-typed versions of `<cmath>` functions.
 	 * @details		Includes most c++11 extensions.
 	 */
 
 	/**
 	 * @defgroup	Conversion Explicit Conversion
+	 * @ingroup		Units
 	 * @brief		Functions used to convert values of one logical type to another.
 	 */
 
 	/**
 	 * @defgroup	TypeTraits Type Traits
+	 * @ingroup		Units
 	 * @brief		Defines a series of classes to obtain unit type information at compile-time.
 	 */
 
@@ -810,6 +821,7 @@
 		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>,	std::ratio<1>>																						angular_velocity_unit;			///< Represents an SI derived unit of angular velocity
 		typedef base_unit<detail::meter_ratio<1>,	std::ratio<0>,	std::ratio<-2>>																										acceleration_unit;				///< Represents an SI derived unit of acceleration
 		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-2>,	std::ratio<1>>																						angular_acceleration_unit;			///< Represents an SI derived unit of angular acceleration
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-3>,	std::ratio<1>>																						angular_jerk_unit;			    ///< Represents an SI derived unit of angular jerk
 		typedef base_unit<detail::meter_ratio<1>,	std::ratio<1>,	std::ratio<-2>>																										force_unit;						///< Represents an SI derived unit of force
 		typedef base_unit<detail::meter_ratio<-1>,	std::ratio<1>,	std::ratio<-2>>																										pressure_unit;					///< Represents an SI derived unit of pressure
 		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<1>,	std::ratio<0>,	std::ratio<1>>																		charge_unit;					///< Represents an SI derived unit of charge
@@ -1394,7 +1406,7 @@
 	 *					error. This value should be chosen to be as high as possible before
 	 *					integer overflow errors occur in the compiler.
 	 * @note		USE WITH CAUTION. The is an approximate value. In general, squared<sqrt<meter>> != meter,
-	 *				i.e. the operation is not reversible, and it will result in propogated approximations.
+	 *				i.e. the operation is not reversible, and it will result in propagated approximations.
 	 *				Use only when absolutely necessary.
 	 */
 	template<class U, std::intmax_t Eps = 10000000000>
@@ -1758,7 +1770,7 @@
 #ifdef FOR_DOXYGEN_PURPOSOES_ONLY
 		/**
 		* @ingroup		TypeTraits
-		* @brief		Trait for accessing the publically defined types of `units::unit_t`
+		* @brief		Trait for accessing the publicly defined types of `units::unit_t`
 		* @details		The units library determines certain properties of the unit_t types passed to them
 		*				and what they represent by using the members of the corresponding unit_t_traits instantiation.
 		*/
@@ -1788,7 +1800,7 @@
 
 		/**
 		 * @ingroup		TypeTraits
-		 * @brief		Trait for accessing the publically defined types of `units::unit_t`
+		 * @brief		Trait for accessing the publicly defined types of `units::unit_t`
 		 * @details
 		 */
 		template<typename T>
@@ -2875,7 +2887,7 @@
 	}
 #endif
 }
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
 template <>
 struct fmt::formatter<units::dimensionless::dB_t> : fmt::formatter<double>
 {
@@ -2972,7 +2984,7 @@
 #ifdef FOR_DOXYGEN_PURPOSES_ONLY
 		/**
 		* @ingroup		TypeTraits
-		* @brief		Trait for accessing the publically defined types of `units::unit_value_t_traits`
+		* @brief		Trait for accessing the publicly defined types of `units::unit_value_t_traits`
 		* @details		The units library determines certain properties of the `unit_value_t` types passed to
 		*				them and what they represent by using the members of the corresponding `unit_value_t_traits`
 		*				instantiation.
@@ -2999,7 +3011,7 @@
 
 		/**
 		 * @ingroup		TypeTraits
-		 * @brief		Trait for accessing the publically defined types of `units::unit_value_t_traits`
+		 * @brief		Trait for accessing the publicly defined types of `units::unit_value_t_traits`
 		 * @details
 		 */
 		template<typename T>
@@ -3440,6 +3452,6 @@
 using namespace units::literals;
 #endif  // UNIT_HAS_LITERAL_SUPPORT
 
-#if !defined(UNIT_LIB_DISABLE_FMT)
-#include "frc/fmt/Units.h"
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
+#include "units/formatter.h"
 #endif

diff --git a/wpimath/src/main/native/include/frc/fmt/Units.h b/wpimath/src/main/native/include/units/formatter.h
similarity index 88%
rename from wpimath/src/main/native/include/frc/fmt/Units.h
rename to wpimath/src/main/native/include/units/formatter.h
index 1ec61ca..1c17b0a 100644
--- a/wpimath/src/main/native/include/frc/fmt/Units.h
+++ b/wpimath/src/main/native/include/units/formatter.h

@@ -4,43 +4,41 @@
 
 #pragma once
 
+#include <type_traits>
+
 #include <fmt/format.h>
 
 #include "units/base.h"
 
+// FIXME: Replace enable_if with requires clause and remove <type_traits>
+// include once using GCC >= 12. GCC 11 incorrectly emits a struct redefinition
+// error because it doesn't use the requires clause to disambiguate.
+
 /**
  * Formatter for unit types.
- *
- * @tparam Units Unit tag for which type of units the `unit_t` represents (e.g.
- *               meters).
- * @tparam T Underlying type of the storage. Defaults to double.
- * @tparam NonLinearScale Optional scale class for the units. Defaults to linear
- *                        (i.e. does not scale the unit value). Examples of
- *                        non-linear scales could be logarithmic, decibel, or
- *                        richter scales. Non-linear scales must adhere to the
- *                        non-linear-scale concept.
  */
-template <class Units, typename T, template <typename> class NonLinearScale>
-struct fmt::formatter<units::unit_t<Units, T, NonLinearScale>>
-    : fmt::formatter<double> {
+template <typename Unit, typename CharT>
+struct fmt::formatter<Unit, CharT,
+                      std::enable_if_t<units::traits::is_unit_t_v<Unit>>> {
+  constexpr auto parse(fmt::format_parse_context& ctx) {
+    return m_underlying.parse(ctx);
+  }
+
   /**
    * Writes out a formatted unit.
    *
-   * @tparam FormatContext Format string context type.
    * @param obj Unit instance.
    * @param ctx Format string context.
    */
-  template <typename FormatContext>
-  auto format(const units::unit_t<Units, T, NonLinearScale>& obj,
-              FormatContext& ctx) {
+  auto format(const Unit& obj, fmt::format_context& ctx) const {
+    using Units = typename Unit::unit_type;
     using BaseUnits =
         units::unit<std::ratio<1>,
                     typename units::traits::unit_traits<Units>::base_unit_type>;
 
     auto out = ctx.out();
 
-    out = fmt::formatter<double>::format(
-        units::convert<Units, BaseUnits>(obj()), ctx);
+    out = m_underlying.format(units::convert<Units, BaseUnits>(obj()), ctx);
 
     if constexpr (units::traits::unit_traits<
                       Units>::base_unit_type::meter_ratio::num != 0) {
@@ -215,4 +213,7 @@
 
     return out;
   }
+
+ private:
+  fmt::formatter<typename Unit::underlying_type, CharT> m_underlying;
 };

diff --git a/wpimath/src/main/native/include/wpimath/MathShared.h b/wpimath/src/main/native/include/wpimath/MathShared.h
index 3b9b3cd..8e0698d 100644
--- a/wpimath/src/main/native/include/wpimath/MathShared.h
+++ b/wpimath/src/main/native/include/wpimath/MathShared.h

@@ -9,6 +9,8 @@
 #include <fmt/format.h>
 #include <wpi/SymbolExports.h>
 
+#include "units/time.h"
+
 namespace wpi::math {
 
 enum class MathUsageId {
@@ -31,6 +33,7 @@
   virtual void ReportWarningV(fmt::string_view format,
                               fmt::format_args args) = 0;
   virtual void ReportUsage(MathUsageId id, int count) = 0;
+  virtual units::second_t GetTimestamp() = 0;
 
   template <typename S, typename... Args>
   inline void ReportError(const S& format, Args&&... args) {
@@ -70,6 +73,10 @@
   static void ReportUsage(MathUsageId id, int count) {
     GetMathShared().ReportUsage(id, count);
   }
+
+  static units::second_t GetTimestamp() {
+    return GetMathShared().GetTimestamp();
+  }
 };
 
 }  // namespace wpi::math

diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
deleted file mode 100644
index 47097ed..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
+++ /dev/null

@@ -1,162 +0,0 @@
-#pragma once
-
-#include <type_traits>
-
-/// @file
-/// Provides Drake's assertion implementation.  This is intended to be used
-/// both within Drake and by other software.  Drake's asserts can be armed
-/// and disarmed independently from the system-wide asserts.
-
-#ifdef DRAKE_DOXYGEN_CXX
-/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition)
-/// from the C++ system header @p <cassert>.  Unless Drake's assertions are
-/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT
-/// will evaluate @p condition and iff the value is false will trigger an
-/// assertion failure with a message showing at least the condition text,
-/// function name, file, and line.
-///
-/// By default, assertion failures will :abort() the program.  However, when
-/// using the pydrake python bindings, assertion failures will instead throw a
-/// C++ exception that causes a python SystemExit exception.
-///
-/// Assertions are enabled or disabled using the following pre-processor macros:
-///
-/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed.
-/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed.
-/// - If both macros are defined, then it is a compile-time error.
-/// - If neither are defined, then NDEBUG governs assertions as usual.
-///
-/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or
-/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed.
-///
-/// This header will define both `constexpr bool drake::kDrakeAssertIsArmed`
-/// and `constexpr bool drake::kDrakeAssertIsDisarmed` globals.
-///
-/// One difference versus the standard @p assert(condition) is that the
-/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if
-/// Drake's assertions are disarmed.
-///
-/// Treat @p DRAKE_ASSERT like a statement -- it must always be used
-/// in block scope, and must always be followed by a semicolon.
-#define DRAKE_ASSERT(condition)
-/// Like @p DRAKE_ASSERT, except that the expression must be void-valued; this
-/// allows for guarding expensive assertion-checking subroutines using the same
-/// macros as stand-alone assertions.
-#define DRAKE_ASSERT_VOID(expression)
-/// Evaluates @p condition and iff the value is false will trigger an assertion
-/// failure with a message showing at least the condition text, function name,
-/// file, and line.
-#define DRAKE_DEMAND(condition)
-/// Silences a "no return value" compiler warning by calling a function that
-/// always raises an exception or aborts (i.e., a function marked noreturn).
-/// Only use this macro at a point where (1) a point in the code is truly
-/// unreachable, (2) the fact that it's unreachable is knowable from only
-/// reading the function itself (and not, e.g., some larger design invariant),
-/// and (3) there is a compiler warning if this macro were removed.  The most
-/// common valid use is with a switch-case-return block where all cases are
-/// accounted for but the enclosing function is supposed to return a value.  Do
-/// *not* use this macro as a "logic error" assertion; it should *only* be used
-/// to silence false positive warnings.  When in doubt, throw an exception
-/// manually instead of using this macro.
-#define DRAKE_UNREACHABLE()
-#else  //  DRAKE_DOXYGEN_CXX
-
-// Users should NOT set these; only this header should set them.
-#ifdef DRAKE_ASSERT_IS_ARMED
-# error Unexpected DRAKE_ASSERT_IS_ARMED defined.
-#endif
-#ifdef DRAKE_ASSERT_IS_DISARMED
-# error Unexpected DRAKE_ASSERT_IS_DISARMED defined.
-#endif
-
-// Decide whether Drake assertions are enabled.
-#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS)
-# error Conflicting assertion toggles.
-#elif defined(DRAKE_ENABLE_ASSERTS)
-# define DRAKE_ASSERT_IS_ARMED
-#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG)
-# define DRAKE_ASSERT_IS_DISARMED
-#else
-# define DRAKE_ASSERT_IS_ARMED
-#endif
-
-namespace drake {
-namespace internal {
-// Abort the program with an error message.
-[[noreturn]] void Abort(const char* condition, const char* func,
-                        const char* file, int line);
-// Report an assertion failure; will either Abort(...) or throw.
-[[noreturn]] void AssertionFailed(const char* condition, const char* func,
-                                  const char* file, int line);
-}  // namespace internal
-namespace assert {
-// Allows for specialization of how to bool-convert Conditions used in
-// assertions, in case they are not intrinsically convertible.  See
-// common/symbolic/expression/formula.h for an example use.  This is a public
-// interface to extend; it is intended to be specialized by unusual Scalar
-// types that require special handling.
-template <typename Condition>
-struct ConditionTraits {
-  static constexpr bool is_valid = std::is_convertible_v<Condition, bool>;
-  static bool Evaluate(const Condition& value) {
-    return value;
-  }
-};
-}  // namespace assert
-}  // namespace drake
-
-#define DRAKE_UNREACHABLE()                                             \
-  ::drake::internal::Abort(                                             \
-      "Unreachable code was reached?!", __func__, __FILE__, __LINE__)
-
-#define DRAKE_DEMAND(condition)                                              \
-  do {                                                                       \
-    typedef ::drake::assert::ConditionTraits<                                \
-        typename std::remove_cv_t<decltype(condition)>> Trait;               \
-    static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
-    static_assert(                                                           \
-        !std::is_pointer_v<decltype(condition)>,                             \
-        "When using DRAKE_DEMAND on a raw pointer, always write out "        \
-        "DRAKE_DEMAND(foo != nullptr), do not write DRAKE_DEMAND(foo) "      \
-        "and rely on implicit pointer-to-bool conversion.");                 \
-    if (!Trait::Evaluate(condition)) {                                       \
-      ::drake::internal::AssertionFailed(                                    \
-           #condition, __func__, __FILE__, __LINE__);                        \
-    }                                                                        \
-  } while (0)
-
-#ifdef DRAKE_ASSERT_IS_ARMED
-// Assertions are enabled.
-namespace drake {
-constexpr bool kDrakeAssertIsArmed = true;
-constexpr bool kDrakeAssertIsDisarmed = false;
-}  // namespace drake
-# define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition)
-# define DRAKE_ASSERT_VOID(expression) do {                     \
-    static_assert(                                              \
-        std::is_convertible_v<decltype(expression), void>,      \
-        "Expression should be void.");                          \
-    expression;                                                 \
-  } while (0)
-#else
-// Assertions are disabled, so just typecheck the expression.
-namespace drake {
-constexpr bool kDrakeAssertIsArmed = false;
-constexpr bool kDrakeAssertIsDisarmed = true;
-}  // namespace drake
-# define DRAKE_ASSERT(condition) do {                                        \
-    typedef ::drake::assert::ConditionTraits<                                \
-        typename std::remove_cv_t<decltype(condition)>> Trait;               \
-    static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
-    static_assert(                                                           \
-        !std::is_pointer_v<decltype(condition)>,                             \
-        "When using DRAKE_ASSERT on a raw pointer, always write out "        \
-        "DRAKE_ASSERT(foo != nullptr), do not write DRAKE_ASSERT(foo) "      \
-        "and rely on implicit pointer-to-bool conversion.");                 \
-  } while (0)
-# define DRAKE_ASSERT_VOID(expression) static_assert(           \
-    std::is_convertible_v<decltype(expression), void>,          \
-    "Expression should be void.")
-#endif
-
-#endif  // DRAKE_DOXYGEN_CXX

diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
deleted file mode 100644
index b428474..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
+++ /dev/null

@@ -1,18 +0,0 @@
-#pragma once
-
-#include <stdexcept>
-#include <string>
-
-namespace drake {
-namespace internal {
-
-// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are
-// configured to throw.
-class assertion_error : public std::runtime_error {
- public:
-  explicit assertion_error(const std::string& what_arg)
-      : std::runtime_error(what_arg) {}
-};
-
-}  // namespace internal
-}  // namespace drake

diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
deleted file mode 100644
index a96a6fb..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
+++ /dev/null

@@ -1,90 +0,0 @@
-#pragma once
-
-// ============================================================================
-// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this
-// file must be kept in sync!
-// ============================================================================
-
-/** @file
-Provides careful macros to selectively enable or disable the special member
-functions for copy-construction, copy-assignment, move-construction, and
-move-assignment.
-
-http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions
-
-When enabled via these macros, the `= default` implementation is provided.
-Code that needs custom copy or move functions should not use these macros.
-*/
-
-/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for
-copy-construction, copy-assignment, move-construction, and move-assignment.
-Drake's Doxygen is customized to render the deletions in detail, with
-appropriate comments.  Invoke this macro in the public section of the class
-declaration, e.g.:
-<pre>
-class Foo {
- public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
-
-  // ...
-};
-</pre>
-*/
-#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)      \
-  Classname(const Classname&) = delete;                 \
-  void operator=(const Classname&) = delete;            \
-  Classname(Classname&&) = delete;                      \
-  void operator=(Classname&&) = delete;
-
-/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member
-functions for copy-construction, copy-assignment, move-construction, and
-move-assignment.  This macro should be used only when copy-construction and
-copy-assignment defaults are well-formed.  Note that the defaulted move
-functions could conceivably still be ill-formed, in which case they will
-effectively not be declared or used -- but because the copy constructor exists
-the type will still be MoveConstructible.  Drake's Doxygen is customized to
-render the functions in detail, with appropriate comments.  Typically, you
-should invoke this macro in the public section of the class declaration, e.g.:
-<pre>
-class Foo {
- public:
-  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
-
-  // ...
-};
-</pre>
-
-However, if Foo has a virtual destructor (i.e., is subclassable), then
-typically you should use either DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN in the
-public section or else DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN in the
-protected section, to prevent
-<a href="https://en.wikipedia.org/wiki/Object_slicing">object slicing</a>.
-
-The macro contains a built-in self-check that copy-assignment is well-formed.
-This self-check proves that the Classname is CopyConstructible, CopyAssignable,
-MoveConstructible, and MoveAssignable (in all but the most arcane cases where a
-member of the Classname is somehow CopyAssignable but not CopyConstructible).
-Therefore, classes that use this macro typically will not need to have any unit
-tests that check for the presence nor correctness of these functions.
-
-However, the self-check does not provide any checks of the runtime efficiency
-of the functions.  If it is important for performance that the move functions
-actually move (instead of making a copy), then you should consider capturing
-that in a unit test.
-*/
-#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)       \
-  Classname(const Classname&) = default;                        \
-  Classname& operator=(const Classname&) = default;             \
-  Classname(Classname&&) = default;                             \
-  Classname& operator=(Classname&&) = default;                  \
-  /* Fails at compile-time if copy-assign doesn't compile. */   \
-  /* Note that we do not test the copy-ctor here, because  */   \
-  /* it will not exist when Classname is abstract.         */   \
-  static void DrakeDefaultCopyAndMoveAndAssign_DoAssign(        \
-      Classname* a, const Classname& b) { *a = b; }             \
-  static_assert(                                                \
-      &DrakeDefaultCopyAndMoveAndAssign_DoAssign ==             \
-      &DrakeDefaultCopyAndMoveAndAssign_DoAssign,               \
-      "This assertion is never false; its only purpose is to "  \
-      "generate 'use of deleted function: operator=' errors "   \
-      "when Classname is a template.");

diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
deleted file mode 100644
index fdd07a2..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
+++ /dev/null

@@ -1,47 +0,0 @@
-#pragma once
-
-#include <type_traits>
-
-#include "drake/common/drake_assert.h"
-
-/// @file
-/// Provides a convenient wrapper to throw an exception when a condition is
-/// unmet.  This is similar to an assertion, but uses exceptions instead of
-/// ::abort(), and cannot be disabled.
-
-namespace drake {
-namespace internal {
-// Throw an error message.
-[[noreturn]] void Throw(const char* condition, const char* func,
-                        const char* file, int line);
-}  // namespace internal
-}  // namespace drake
-
-/// Evaluates @p condition and iff the value is false will throw an exception
-/// with a message showing at least the condition text, function name, file,
-/// and line.
-///
-/// The condition must not be a pointer, where we'd implicitly rely on its
-/// nullness. Instead, always write out "!= nullptr" to be precise.
-///
-/// Correct: `DRAKE_THROW_UNLESS(foo != nullptr);`
-/// Incorrect: `DRAKE_THROW_UNLESS(foo);`
-///
-/// Because this macro is intended to provide a useful exception message to
-/// users, we should err on the side of extra detail about the failure. The
-/// meaning of "foo" isolated within error message text does not make it
-/// clear that a null pointer is the proximate cause of the problem.
-#define DRAKE_THROW_UNLESS(condition)                                         \
-  do {                                                                        \
-    typedef ::drake::assert::ConditionTraits<                                 \
-        typename std::remove_cv_t<decltype(condition)>> Trait;                \
-    static_assert(Trait::is_valid, "Condition should be bool-convertible.");  \
-    static_assert(                                                            \
-        !std::is_pointer_v<decltype(condition)>,                              \
-        "When using DRAKE_THROW_UNLESS on a raw pointer, always write out "   \
-        "DRAKE_THROW_UNLESS(foo != nullptr), do not write DRAKE_THROW_UNLESS" \
-        "(foo) and rely on implicit pointer-to-bool conversion.");            \
-    if (!Trait::Evaluate(condition)) {                                        \
-      ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__);     \
-    }                                                                         \
-  } while (0)

diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
deleted file mode 100644
index b3f369c..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
+++ /dev/null

@@ -1,62 +0,0 @@
-#pragma once
-
-#include <vector>
-
-#include <Eigen/Core>
-
-namespace drake {
-
-/// Returns true if and only if the two matrices are equal to within a certain
-/// absolute elementwise @p tolerance.  Special values (infinities, NaN, etc.)
-/// do not compare as equal elements.
-template <typename DerivedA, typename DerivedB>
-bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
-                            const Eigen::MatrixBase<DerivedB>& m2,
-                            double tolerance) {
-  return (
-      (m1.rows() == m2.rows()) &&
-      (m1.cols() == m2.cols()) &&
-      ((m1 - m2).template lpNorm<Eigen::Infinity>() <= tolerance));
-}
-
-/// Returns true if and only if a simple greedy search reveals a permutation
-/// of the columns of m2 to make the matrix equal to m1 to within a certain
-/// absolute elementwise @p tolerance. E.g., there exists a P such that
-/// <pre>
-///    forall i,j,  |m1 - m2*P|_{i,j} <= tolerance
-///    where P is a permutation matrix:
-///       P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
-/// </pre>
-/// Note: Returns false for matrices of different sizes.
-/// Note: The current implementation is O(n^2) in the number of columns.
-/// Note: In marginal cases (with similar but not identical columns) this
-/// algorithm can fail to find a permutation P even if it exists because it
-/// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the
-/// pool. It is possible that other columns of m2 would also match m1(i) but
-/// that m2(j) is the only match possible for a later column of m1.
-template <typename DerivedA, typename DerivedB>
-bool IsApproxEqualAbsTolWithPermutedColumns(
-    const Eigen::MatrixBase<DerivedA>& m1,
-    const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
-  if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
-
-  std::vector<bool> available(m2.cols());
-  for (int i = 0; i < m2.cols(); i++) available[i] = true;
-
-  for (int i = 0; i < m1.cols(); i++) {
-    bool found_match = false;
-    for (int j = 0; j < m2.cols(); j++) {
-      if (available[j] &&
-          is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) {
-        found_match = true;
-        available[j] = false;
-        break;
-      }
-    }
-    if (!found_match) return false;
-  }
-  return true;
-}
-
-
-}  // namespace drake

diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
deleted file mode 100644
index 024b355..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
+++ /dev/null

@@ -1,103 +0,0 @@
-#pragma once
-
-#include <new>
-#include <type_traits>
-#include <utility>
-
-#include "drake/common/drake_copyable.h"
-
-namespace drake {
-
-/// Wraps an underlying type T such that its storage is a direct member field
-/// of this object (i.e., without any indirection into the heap), but *unlike*
-/// most member fields T's destructor is never invoked.
-///
-/// This is especially useful for function-local static variables that are not
-/// trivially destructable.  We shouldn't call their destructor at program exit
-/// because of the "indeterminate order of ... destruction" as mentioned in
-/// cppguide's
-/// <a href="https://drake.mit.edu/styleguide/cppguide.html#Static_and_Global_Variables">Static
-/// and Global Variables</a> section, but other solutions to this problem place
-///  the objects on the heap through an indirection.
-///
-/// Compared with other approaches, this mechanism more clearly describes the
-/// intent to readers, avoids "possible leak" warnings from memory-checking
-/// tools, and is probably slightly faster.
-///
-/// Example uses:
-///
-/// The singleton pattern:
-/// @code
-/// class Singleton {
-///  public:
-///   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton)
-///   static Singleton& getInstance() {
-///     static never_destroyed<Singleton> instance;
-///     return instance.access();
-///   }
-///  private:
-///   friend never_destroyed<Singleton>;
-///   Singleton() = default;
-/// };
-/// @endcode
-///
-/// A lookup table, created on demand the first time its needed, and then
-/// reused thereafter:
-/// @code
-/// enum class Foo { kBar, kBaz };
-/// Foo ParseFoo(const std::string& foo_string) {
-///   using Dict = std::unordered_map<std::string, Foo>;
-///   static const drake::never_destroyed<Dict> string_to_enum{
-///     std::initializer_list<Dict::value_type>{
-///       {"bar", Foo::kBar},
-///       {"baz", Foo::kBaz},
-///     }
-///   };
-///   return string_to_enum.access().at(foo_string);
-/// }
-/// @endcode
-///
-/// In cases where computing the static data is more complicated than an
-/// initializer_list, you can use a temporary lambda to populate the value:
-/// @code
-/// const std::vector<double>& GetConstantMagicNumbers() {
-///   static const drake::never_destroyed<std::vector<double>> result{[]() {
-///     std::vector<double> prototype;
-///     std::mt19937 random_generator;
-///     for (int i = 0; i < 10; ++i) {
-///       double new_value = random_generator();
-///       prototype.push_back(new_value);
-///     }
-///     return prototype;
-///   }()};
-///   return result.access();
-/// }
-/// @endcode
-///
-/// Note in particular the `()` after the lambda. That causes it to be invoked.
-//
-// The above examples are repeated in the unit test; keep them in sync.
-template <typename T>
-class never_destroyed {
- public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed)
-
-  /// Passes the constructor arguments along to T using perfect forwarding.
-  template <typename... Args>
-  explicit never_destroyed(Args&&... args) {
-    // Uses "placement new" to construct a `T` in `storage_`.
-    new (&storage_) T(std::forward<Args>(args)...);
-  }
-
-  /// Does nothing.  Guaranteed!
-  ~never_destroyed() = default;
-
-  /// Returns the underlying T reference.
-  T& access() { return *reinterpret_cast<T*>(&storage_); }
-  const T& access() const { return *reinterpret_cast<const T*>(&storage_); }
-
- private:
-  typename std::aligned_storage<sizeof(T), alignof(T)>::type storage_;
-};
-
-}  // namespace drake

diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
deleted file mode 100644
index 55b8442..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
+++ /dev/null

@@ -1,85 +0,0 @@
-#pragma once
-
-#include <cmath>
-#include <cstdlib>
-
-#include <Eigen/Core>
-#include <wpi/SymbolExports.h>
-
-namespace drake {
-namespace math {
-
-/**
-Computes the unique stabilizing solution X to the discrete-time algebraic
-Riccati equation:
-
-AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
-
-@throws std::exception if Q is not positive semi-definite.
-@throws std::exception if R is not positive definite.
-
-Based on the Schur Vector approach outlined in this paper:
-"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
-by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
-*/
-WPILIB_DLLEXPORT
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
-    const Eigen::Ref<const Eigen::MatrixXd>& B,
-    const Eigen::Ref<const Eigen::MatrixXd>& Q,
-    const Eigen::Ref<const Eigen::MatrixXd>& R);
-
-/**
-Computes the unique stabilizing solution X to the discrete-time algebraic
-Riccati equation:
-
-AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
-
-This is equivalent to solving the original DARE:
-
-A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
-
-where A₂ and Q₂ are a change of variables:
-
-A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
-
-This overload of the DARE is useful for finding the control law uₖ that
-minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
-
-@verbatim
-    ∞ [xₖ]ᵀ[Q  N][xₖ]
-J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
-   k=0
-@endverbatim
-
-This is a more general form of the following. The linear-quadratic regulator
-is the feedback control law uₖ that minimizes the following cost function
-subject to xₖ₊₁ = Axₖ + Buₖ:
-
-@verbatim
-    ∞
-J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
-   k=0
-@endverbatim
-
-This can be refactored as:
-
-@verbatim
-    ∞ [xₖ]ᵀ[Q 0][xₖ]
-J = Σ [uₖ] [0 R][uₖ] ΔT
-   k=0
-@endverbatim
-
-@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
-@throws std::runtime_error if R is not positive definite.
-*/
-WPILIB_DLLEXPORT
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
-    const Eigen::Ref<const Eigen::MatrixXd>& B,
-    const Eigen::Ref<const Eigen::MatrixXd>& Q,
-    const Eigen::Ref<const Eigen::MatrixXd>& R,
-    const Eigen::Ref<const Eigen::MatrixXd>& N);
-}  // namespace math
-}  // namespace drake
-

diff --git a/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp b/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
deleted file mode 100644
index 88e7e66..0000000
--- a/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
+++ /dev/null

@@ -1,87 +0,0 @@
-// This file contains the implementation of both drake_assert and drake_throw.
-/* clang-format off to disable clang-format-includes */
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_throw.h"
-/* clang-format on */
-
-#include <atomic>
-#include <cstdlib>
-#include <iostream>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-
-#include "drake/common/drake_assertion_error.h"
-#include "drake/common/never_destroyed.h"
-
-namespace drake {
-namespace internal {
-namespace {
-
-// Singleton to manage assertion configuration.
-struct AssertionConfig {
-  static AssertionConfig& singleton() {
-    static never_destroyed<AssertionConfig> global;
-    return global.access();
-  }
-
-  std::atomic<bool> assertion_failures_are_exceptions;
-};
-
-// Stream into @p out the given failure details; only @p condition may be null.
-void PrintFailureDetailTo(std::ostream& out, const char* condition,
-                          const char* func, const char* file, int line) {
-  out << "Failure at " << file << ":" << line << " in " << func << "()";
-  if (condition) {
-    out << ": condition '" << condition << "' failed.";
-  } else {
-    out << ".";
-  }
-}
-}  // namespace
-
-// Declared in drake_assert.h.
-void Abort(const char* condition, const char* func, const char* file,
-           int line) {
-  std::cerr << "abort: ";
-  PrintFailureDetailTo(std::cerr, condition, func, file, line);
-  std::cerr << std::endl;
-  std::abort();
-}
-
-// Declared in drake_throw.h.
-void Throw(const char* condition, const char* func, const char* file,
-           int line) {
-  std::ostringstream what;
-  PrintFailureDetailTo(what, condition, func, file, line);
-  throw assertion_error(what.str().c_str());
-}
-
-// Declared in drake_assert.h.
-void AssertionFailed(const char* condition, const char* func, const char* file,
-                     int line) {
-  if (AssertionConfig::singleton().assertion_failures_are_exceptions) {
-    Throw(condition, func, file, line);
-  } else {
-    Abort(condition, func, file, line);
-  }
-}
-
-}  // namespace internal
-}  // namespace drake
-
-// Configures the DRAKE_ASSERT and DRAKE_DEMAND assertion failure handling
-// behavior.
-//
-// By default, assertion failures will result in an ::abort().  If this method
-// has ever been called, failures will result in a thrown exception instead.
-//
-// Assertion configuration has process-wide scope.  Changes here will affect
-// all assertions within the current process.
-//
-// This method is intended ONLY for use by pydrake bindings, and thus is not
-// declared in any header file, to discourage anyone from using it.
-extern "C" void drake_set_assertion_failure_to_throw_exception() {
-  drake::internal::AssertionConfig::singleton().
-      assertion_failures_are_exceptions = true;
-}

diff --git a/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
deleted file mode 100644
index 20ea2b7..0000000
--- a/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
+++ /dev/null

@@ -1,475 +0,0 @@
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-
-#include <Eigen/Eigenvalues>
-#include <Eigen/QR>
-
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_throw.h"
-#include "drake/common/is_approx_equal_abstol.h"
-
-namespace drake {
-namespace math {
-namespace {
-/* helper functions */
-template <typename T>
-int sgn(T val) {
-  return (T(0) < val) - (val < T(0));
-}
-void check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
-                        const Eigen::Ref<const Eigen::MatrixXd>& B) {
-  // This function checks if (A,B) is a stabilizable pair.
-  // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
-  // A, if any, have absolute values less than one, where an eigenvalue is
-  // uncontrollable if Rank[lambda * I - A, B] < n.
-  int n = B.rows(), m = B.cols();
-  Eigen::EigenSolver<Eigen::MatrixXd> es(A);
-  for (int i = 0; i < n; i++) {
-    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
-            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
-        1)
-      continue;
-    Eigen::MatrixXcd E(n, n + m);
-    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
-    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
-    DRAKE_THROW_UNLESS(qr.rank() == n);
-  }
-}
-void check_detectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
-                      const Eigen::Ref<const Eigen::MatrixXd>& Q) {
-  // This function check if (A,C) is a detectable pair, where Q = C' * C.
-  // (A,C) is detectable if and only if the unobservable eigenvalues of A,
-  // if any, have absolute values less than one, where an eigenvalue is
-  // unobservable if Rank[lambda * I - A; C] < n.
-  // Also, (A,C) is detectable if and only if (A',C') is stabilizable.
-  int n = A.rows();
-  Eigen::LDLT<Eigen::MatrixXd> ldlt(Q);
-  Eigen::MatrixXd L = ldlt.matrixL();
-  Eigen::MatrixXd D = ldlt.vectorD();
-  Eigen::MatrixXd D_sqrt = Eigen::MatrixXd::Zero(n, n);
-  for (int i = 0; i < n; i++) {
-    D_sqrt(i, i) = sqrt(D(i));
-  }
-  Eigen::MatrixXd C = L * D_sqrt;
-  check_stabilizable(A.transpose(), C.transpose());
-}
-
-// "Givens rotation" computes an orthogonal 2x2 matrix R such that
-// it eliminates the 2nd coordinate of the vector [a,b]', i.e.,
-// R * [ a ] = [ a_hat ]
-//     [ b ]   [   0   ]
-// The implementation is based on
-// https://en.wikipedia.org/wiki/Givens_rotation#Stable_calculation
-void Givens_rotation(double a, double b, Eigen::Ref<Eigen::Matrix2d> R,
-                     double eps = 1e-10) {
-  double c, s;
-  if (fabs(b) < eps) {
-    c = (a < -eps ? -1 : 1);
-    s = 0;
-  } else if (fabs(a) < eps) {
-    c = 0;
-    s = -sgn(b);
-  } else if (fabs(a) > fabs(b)) {
-    double t = b / a;
-    double u = sgn(a) * fabs(sqrt(1 + t * t));
-    c = 1 / u;
-    s = -c * t;
-  } else {
-    double t = a / b;
-    double u = sgn(b) * fabs(sqrt(1 + t * t));
-    s = -1 / u;
-    c = -s * t;
-  }
-  R(0, 0) = c, R(0, 1) = -s, R(1, 0) = s, R(1, 1) = c;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_11(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
-  // Dooren, Case I, p124-125.
-  int n2 = S.rows();
-  Eigen::Matrix2d A = S.block<2, 2>(p, p);
-  Eigen::Matrix2d B = T.block<2, 2>(p, p);
-  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::Matrix2d H = A(1, 1) * B - B(1, 1) * A;
-  Givens_rotation(H(0, 1), H(0, 0), Z1.block<2, 2>(p, p));
-  S = (S * Z1).eval();
-  T = (T * Z1).eval();
-  Z = (Z * Z1).eval();
-  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n2, n2);
-  Givens_rotation(T(p, p), T(p + 1, p), Q.block<2, 2>(p, p));
-  S = (Q * S).eval();
-  T = (Q * T).eval();
-  S(p + 1, p) = 0;
-  T(p + 1, p) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_21(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
-  // Dooren, Case II, p126-127.
-  int n2 = S.rows();
-  Eigen::Matrix3d A = S.block<3, 3>(p, p);
-  Eigen::Matrix3d B = T.block<3, 3>(p, p);
-  // Compute H and eliminate H(1,0) by row operation.
-  Eigen::Matrix3d H = A(2, 2) * B - B(2, 2) * A;
-  Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
-  Givens_rotation(H(0, 0), H(1, 0), R.block<2, 2>(0, 0));
-  H = (R * H).eval();
-  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
-  // Compute Z1, Z2, Q1, Q2.
-  Givens_rotation(H(1, 2), H(1, 1), Z1.block<2, 2>(p + 1, p + 1));
-  H = (H * Z1.block<3, 3>(p, p)).eval();
-  Givens_rotation(H(0, 1), H(0, 0), Z2.block<2, 2>(p, p));
-  S = (S * Z1).eval();
-  T = (T * Z1).eval();
-  Z = (Z * Z1 * Z2).eval();
-  Givens_rotation(T(p + 1, p + 1), T(p + 2, p + 1),
-                  Q1.block<2, 2>(p + 1, p + 1));
-  S = (Q1 * S * Z2).eval();
-  T = (Q1 * T * Z2).eval();
-  Givens_rotation(T(p, p), T(p + 1, p), Q2.block<2, 2>(p, p));
-  S = (Q2 * S).eval();
-  T = (Q2 * T).eval();
-  S(p + 1, p) = 0;
-  S(p + 2, p) = 0;
-  T(p + 1, p) = 0;
-  T(p + 2, p) = 0;
-  T(p + 2, p + 1) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_12(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
-  int n2 = S.rows();
-  // Swap the role of S and T.
-  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q0 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
-  Givens_rotation(S(p + 1, p + 1), S(p + 2, p + 1),
-                  Q0.block<2, 2>(p + 1, p + 1));
-  S = (Q0 * S).eval();
-  T = (Q0 * T).eval();
-  Eigen::Matrix3d A = S.block<3, 3>(p, p);
-  Eigen::Matrix3d B = T.block<3, 3>(p, p);
-  // Compute H and eliminate H(2,1) by column operation.
-  Eigen::Matrix3d H = B(0, 0) * A - A(0, 0) * B;
-  Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
-  Givens_rotation(H(2, 2), H(2, 1), R.block<2, 2>(1, 1));
-  H = (H * R).eval();
-  // Compute Q1, Q2, Z1, Z2.
-  Givens_rotation(H(0, 1), H(1, 1), Q1.block<2, 2>(p, p));
-  H = (Q1.block<3, 3>(p, p) * H).eval();
-  Givens_rotation(H(1, 2), H(2, 2), Q2.block<2, 2>(p + 1, p + 1));
-  S = (Q1 * S).eval();
-  T = (Q1 * T).eval();
-  Givens_rotation(S(p + 1, p + 1), S(p + 1, p), Z1.block<2, 2>(p, p));
-  S = (Q2 * S * Z1).eval();
-  T = (Q2 * T * Z1).eval();
-  Givens_rotation(S(p + 2, p + 2), S(p + 2, p + 1),
-                  Z2.block<2, 2>(p + 1, p + 1));
-  S = (S * Z2).eval();
-  T = (T * Z2).eval();
-  Z = (Z * Z1 * Z2).eval();
-  // Swap back the role of S and T.
-  Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
-  S = (Q3 * S).eval();
-  T = (Q3 * T).eval();
-  S(p + 2, p) = 0;
-  S(p + 2, p + 1) = 0;
-  T(p + 1, p) = 0;
-  T(p + 2, p) = 0;
-  T(p + 2, p + 1) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_22(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
-  // Direct Swapping Algorithm based on
-  // "Numerical Methods for General and Structured Eigenvalue Problems" by
-  // Daniel Kressner, p108-111.
-  // ( http://sma.epfl.ch/~anchpcommon/publications/kressner_eigenvalues.pdf ).
-  // Also relevant but not applicable here:
-  // "On Swapping Diagonal Blocks in Real Schur Form" by Zhaojun Bai and James
-  // W. Demmelt;
-  int n2 = S.rows();
-  Eigen::MatrixXd A = S.block<4, 4>(p, p);
-  Eigen::MatrixXd B = T.block<4, 4>(p, p);
-  // Solve
-  // A11 * X - Y A22 = A12
-  // B11 * X - Y B22 = B12
-  // Reduce to solve Cx=D, where x=[x1;...;x4;y1;...;y4].
-  Eigen::Matrix<double, 8, 8> C = Eigen::Matrix<double, 8, 8>::Zero();
-  Eigen::Matrix<double, 8, 1> D;
-  // clang-format off
-  C << A(0, 0), 0, A(0, 1), 0, -A(2, 2), -A(3, 2), 0, 0,
-       0, A(0, 0), 0, A(0, 1), -A(2, 3), -A(3, 3), 0, 0,
-       A(1, 0), 0, A(1, 1), 0, 0, 0, -A(2, 2), -A(3, 2),
-       0, A(1, 0), 0, A(1, 1), 0, 0, -A(2, 3), -A(3, 3),
-       B(0, 0), 0, B(0, 1), 0, -B(2, 2), -B(3, 2), 0, 0,
-       0, B(0, 0), 0, B(0, 1), -B(2, 3), -B(3, 3), 0, 0,
-       B(1, 0), 0, B(1, 1), 0, 0, 0, -B(2, 2), -B(3, 2),
-       0, B(1, 0), 0, B(1, 1), 0, 0, -B(2, 3), -B(3, 3);
-  // clang-format on
-  D << A(0, 2), A(0, 3), A(1, 2), A(1, 3), B(0, 2), B(0, 3), B(1, 2), B(1, 3);
-  Eigen::MatrixXd x = C.colPivHouseholderQr().solve(D);
-  // Q * [ -Y ] = [ R_Y ] ,  Z' * [ -X ] = [ R_X ] .
-  //     [ I  ]   [  0  ]         [ I  ] = [  0  ]
-  Eigen::Matrix<double, 4, 2> X, Y;
-  X << -x(0, 0), -x(1, 0), -x(2, 0), -x(3, 0), Eigen::Matrix2d::Identity();
-  Y << -x(4, 0), -x(5, 0), -x(6, 0), -x(7, 0), Eigen::Matrix2d::Identity();
-  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr1(X);
-  Z1.block<4, 4>(p, p) = qr1.householderQ();
-  Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr2(Y);
-  Q1.block<4, 4>(p, p) = qr2.householderQ().adjoint();
-  // Apply transform Q1 * (S,T) * Z1.
-  S = (Q1 * S * Z1).eval();
-  T = (Q1 * T * Z1).eval();
-  Z = (Z * Z1).eval();
-  // Eliminate the T(p+3,p+2) entry.
-  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
-  Givens_rotation(T(p + 2, p + 2), T(p + 3, p + 2),
-                  Q2.block<2, 2>(p + 2, p + 2));
-  S = (Q2 * S).eval();
-  T = (Q2 * T).eval();
-  // Eliminate the T(p+1,p) entry.
-  Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
-  Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
-  S = (Q3 * S).eval();
-  T = (Q3 * T).eval();
-  S(p + 2, p) = 0;
-  S(p + 2, p + 1) = 0;
-  S(p + 3, p) = 0;
-  S(p + 3, p + 1) = 0;
-  T(p + 1, p) = 0;
-  T(p + 2, p) = 0;
-  T(p + 2, p + 1) = 0;
-  T(p + 3, p) = 0;
-  T(p + 3, p + 1) = 0;
-  T(p + 3, p + 2) = 0;
-}
-
-// Functionality of "swap_block" function:
-// swap the 1x1 or 2x2 blocks pointed by p and q.
-// There are four cases: swapping 1x1 and 1x1 matrices, swapping 2x2 and 1x1
-// matrices, swapping 1x1 and 2x2 matrices, and swapping 2x2 and 2x2 matrices.
-// Algorithms are described in the papers
-// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
-// Dooren, 1981 ( http://epubs.siam.org/doi/pdf/10.1137/0902010 ), and
-// "Numerical Methods for General and Structured Eigenvalue Problems" by
-// Daniel Kressner, 2005.
-void swap_block(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                Eigen::Ref<Eigen::MatrixXd> Z, int p, int q, int q_block_size,
-                double eps = 1e-10) {
-  int p_tmp = q, p_block_size;
-  while (p_tmp-- > p) {
-    p_block_size = 1;
-    if (p_tmp >= 1 && fabs(S(p_tmp, p_tmp - 1)) > eps) {
-      p_block_size = 2;
-      p_tmp--;
-    }
-    switch (p_block_size * 10 + q_block_size) {
-      case 11:
-        swap_block_11(S, T, Z, p_tmp);
-        break;
-      case 21:
-        swap_block_21(S, T, Z, p_tmp);
-        break;
-      case 12:
-        swap_block_12(S, T, Z, p_tmp);
-        break;
-      case 22:
-        swap_block_22(S, T, Z, p_tmp);
-        break;
-    }
-  }
-}
-
-// Functionality of "reorder_eigen" function:
-// Reorder the eigenvalues of (S,T) such that the top-left n by n matrix has
-// stable eigenvalues by multiplying Q's and Z's on the left and the right,
-// respectively.
-// Stable eigenvalues are inside the unit disk.
-//
-// Algorithm:
-// Go along the diagonals of (S,T) from the top left to the bottom right.
-// Once find a stable eigenvalue, push it to top left.
-// In implementation, use two pointers, p and q.
-// p points to the current block (1x1 or 2x2) and q points to the block with the
-// stable eigenvalue(s).
-// Push the block pointed by q to the position pointed by p.
-// Finish when n stable eigenvalues are placed at the top-left n by n matrix.
-// The algorithm for swapping blocks is described in the papers
-// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
-// Dooren, 1981, and "Numerical Methods for General and Structured Eigenvalue
-// Problems" by Daniel Kressner, 2005.
-void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, double eps = 1e-10) {
-  // abs(a) < eps => a = 0
-  int n2 = S.rows();
-  int n = n2 / 2, p = 0, q = 0;
-
-  // Find the first unstable p block.
-  while (p < n) {
-    if (fabs(S(p + 1, p)) < eps) {  // p block size = 1
-      if (fabs(T(p, p)) > eps && fabs(S(p, p)) <= fabs(T(p, p))) {  // stable
-        p++;
-        continue;
-      }
-    } else {  // p block size = 2
-      const double det_T =
-          T(p, p) * T(p + 1, p + 1) - T(p + 1, p) * T(p, p + 1);
-      if (fabs(det_T) > eps) {
-        const double det_S =
-            S(p, p) * S(p + 1, p + 1) - S(p + 1, p) * S(p, p + 1);
-        if (fabs(det_S) <= fabs(det_T)) {  // stable
-          p += 2;
-          continue;
-        }
-      }
-    }
-    break;
-  }
-  q = p;
-
-  // Make the first n generalized eigenvalues stable.
-  while (p < n && q < n2) {
-    // Update q.
-    int q_block_size = 0;
-    while (q < n2) {
-      if (q == n2 - 1 || fabs(S(q + 1, q)) < eps) {  // block size = 1
-        if (fabs(T(q, q)) > eps && fabs(S(q, q)) <= fabs(T(q, q))) {
-          q_block_size = 1;
-          break;
-        }
-        q++;
-      } else {  // block size = 2
-        const double det_T =
-            T(q, q) * T(q + 1, q + 1) - T(q + 1, q) * T(q, q + 1);
-        if (fabs(det_T) > eps) {
-          const double det_S =
-              S(q, q) * S(q + 1, q + 1) - S(q + 1, q) * S(q, q + 1);
-          if (fabs(det_S) <= fabs(det_T)) {
-            q_block_size = 2;
-            break;
-          }
-        }
-        q += 2;
-      }
-    }
-    if (q >= n2)
-      throw std::runtime_error("fail to find enough stable eigenvalues");
-    // Swap blocks pointed by p and q.
-    if (p != q) {
-      swap_block(S, T, Z, p, q, q_block_size);
-      p += q_block_size;
-      q += q_block_size;
-    }
-  }
-  if (p < n && q >= n2)
-    throw std::runtime_error("fail to find enough stable eigenvalues");
-}
-}  // namespace
-
-/**
- * DiscreteAlgebraicRiccatiEquation function
- * computes the unique stabilizing solution X to the discrete-time algebraic
- * Riccati equation:
- *
- * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
- *
- * @throws std::exception if Q is not positive semi-definite.
- * @throws std::exception if R is not positive definite.
- *
- * Based on the Schur Vector approach outlined in this paper:
- * "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
- * by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell, in TAC, 1980,
- * http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434
- *
- * Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
- * R_half are sampled from standard normal distributions, where
- * Q = Q_halfᵀ Q_half and similar for R, the absolute error of the solution
- * is 10⁻⁶, while the absolute error of the solution computed by Matlab is
- * 10⁻⁸.
- *
- * TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
- * accuracy, together with more thorough tests.
- */
-
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
-    const Eigen::Ref<const Eigen::MatrixXd>& B,
-    const Eigen::Ref<const Eigen::MatrixXd>& Q,
-    const Eigen::Ref<const Eigen::MatrixXd>& R) {
-  int n = B.rows(), m = B.cols();
-
-  DRAKE_DEMAND(m <= n);
-  DRAKE_DEMAND(A.rows() == n && A.cols() == n);
-  DRAKE_DEMAND(Q.rows() == n && Q.cols() == n);
-  DRAKE_DEMAND(R.rows() == m && R.cols() == m);
-  DRAKE_DEMAND(is_approx_equal_abstol(Q, Q.transpose(), 1e-10));
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Q);
-  for (int i = 0; i < n; i++) {
-    DRAKE_THROW_UNLESS(es.eigenvalues()[i] >= 0);
-  }
-  DRAKE_DEMAND(is_approx_equal_abstol(R, R.transpose(), 1e-10));
-  Eigen::LLT<Eigen::MatrixXd> R_cholesky(R);
-  DRAKE_THROW_UNLESS(R_cholesky.info() == Eigen::Success);
-  check_stabilizable(A, B);
-  check_detectable(A, Q);
-
-  Eigen::MatrixXd M(2 * n, 2 * n), L(2 * n, 2 * n);
-  M << A, Eigen::MatrixXd::Zero(n, n), -Q, Eigen::MatrixXd::Identity(n, n);
-  L << Eigen::MatrixXd::Identity(n, n), B * R.inverse() * B.transpose(),
-      Eigen::MatrixXd::Zero(n, n), A.transpose();
-
-  // QZ decomposition of M and L
-  // QMZ = S, QLZ = T
-  // where Q and Z are real orthogonal matrixes
-  // T is upper-triangular matrix, and S is upper quasi-triangular matrix
-  Eigen::RealQZ<Eigen::MatrixXd> qz(2 * n);
-  qz.compute(M, L);  // M = Q S Z,  L = Q T Z (Q and Z computed by Eigen package
-                     // are adjoints of Q and Z above)
-  Eigen::MatrixXd S = qz.matrixS(), T = qz.matrixT(),
-                  Z = qz.matrixZ().adjoint();
-
-  // Reorder the generalized eigenvalues of (S,T).
-  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(2 * n, 2 * n);
-  reorder_eigen(S, T, Z2);
-  Z = (Z * Z2).eval();
-
-  // The first n columns of Z is ( U1 ) .
-  //                             ( U2 )
-  //            -1
-  // X = U2 * U1   is a solution of the discrete time Riccati equation.
-  Eigen::MatrixXd U1 = Z.block(0, 0, n, n), U2 = Z.block(n, 0, n, n);
-  Eigen::MatrixXd X = U2 * U1.inverse();
-  X = (X + X.adjoint().eval()) / 2.0;
-  return X;
-}
-
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
-    const Eigen::Ref<const Eigen::MatrixXd>& B,
-    const Eigen::Ref<const Eigen::MatrixXd>& Q,
-    const Eigen::Ref<const Eigen::MatrixXd>& R,
-    const Eigen::Ref<const Eigen::MatrixXd>& N) {
-    DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
-
-    // This is a change of variables to make the DARE that includes Q, R, and N
-    // cost matrices fit the form of the DARE that includes only Q and R cost
-    // matrices.
-    Eigen::MatrixXd A2 = A - B * R.llt().solve(N.transpose());
-    Eigen::MatrixXd Q2 = Q - N * R.llt().solve(N.transpose());
-    return DiscreteAlgebraicRiccatiEquation(A2, B, Q2, R);
-}
-
-}  // namespace math
-}  // namespace drake

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
deleted file mode 100644
index bc68397..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
+++ /dev/null

@@ -1,27 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDDEQUE_MODULE_H
-#define EIGEN_STDDEQUE_MODULE_H
-
-#include "Core"
-#include <deque>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdDeque.h"
-
-#endif
-
-#endif // EIGEN_STDDEQUE_MODULE_H

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
deleted file mode 100644
index 4c6262c..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
+++ /dev/null

@@ -1,26 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDLIST_MODULE_H
-#define EIGEN_STDLIST_MODULE_H
-
-#include "Core"
-#include <list>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdList.h"
-
-#endif
-
-#endif // EIGEN_STDLIST_MODULE_H

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
deleted file mode 100644
index 0c4697a..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
+++ /dev/null

@@ -1,27 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDVECTOR_MODULE_H
-#define EIGEN_STDVECTOR_MODULE_H
-
-#include "Core"
-#include <vector>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdVector.h"
-
-#endif
-
-#endif // EIGEN_STDVECTOR_MODULE_H

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
index d973255..9a630e4 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h

@@ -78,7 +78,7 @@
     // This warning is a false positive
     #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
   #endif
-  #if __GNUC__==12
+  #if __GNUC__>=12
     // This warning is a false positive
     #pragma GCC diagnostic ignored "-Warray-bounds"
   #endif

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
index 986c3d4..81986b9 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h

@@ -58,6 +58,16 @@
 // Compiler identification, EIGEN_COMP_*
 //------------------------------------------------------------------------------------------
 
+/// \internal Disable NEON features in Intellisense
+#if __INTELLISENSE__
+#ifdef __ARM_NEON
+#undef __ARM_NEON
+#endif
+#ifdef __ARM_NEON__
+#undef __ARM_NEON__
+#endif
+#endif
+
 /// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
 #ifdef __GNUC__
   #define EIGEN_COMP_GNUC (__GNUC__*10+__GNUC_MINOR__)

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
index f9c56ba..7cb2c26 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h

@@ -270,11 +270,11 @@
       }
 
 
-      Index count = 0;
+//       Index count = 0;
       // FIXME compute a reference value to filter zeros
       for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
       {
-        ++ count;
+//         ++ count;
 //         std::cerr << "fill " << it.index() << ", " << col << "\n";
 //         std::cout << it.value() << "  ";
         // FIXME use insertBack

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
index 6f75d50..7aecbca 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h

@@ -75,8 +75,6 @@
   // Identify the relaxed supernodes by postorder traversal of the etree
   Index snode_start; // beginning of a snode 
   StorageIndex k;
-  Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree 
-  Index nsuper_et = 0; // Number of relaxed snodes in the original etree 
   StorageIndex l; 
   for (j = 0; j < n; )
   {
@@ -88,7 +86,6 @@
       parent = et(j);
     }
     // Found a supernode in postordered etree, j is the last column 
-    ++nsuper_et_post;
     k = StorageIndex(n);
     for (Index i = snode_start; i <= j; ++i)
       k = (std::min)(k, inv_post(i));
@@ -97,7 +94,6 @@
     {
       // This is also a supernode in the original etree
       relax_end(k) = l; // Record last column 
-      ++nsuper_et; 
     }
     else 
     {
@@ -107,7 +103,6 @@
         if (descendants(i) == 0) 
         {
           relax_end(l) = l;
-          ++nsuper_et;
         }
       }
     }

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
deleted file mode 100644
index 6d47e75..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
+++ /dev/null

@@ -1,116 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDDEQUE_H
-#define EIGEN_STDDEQUE_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::deque such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) \
-namespace std \
-{ \
-  template<> \
-  class deque<__VA_ARGS__, std::allocator<__VA_ARGS__> >           \
-    : public deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
-  { \
-    typedef deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > deque_base; \
-  public: \
-    typedef __VA_ARGS__ value_type; \
-    typedef deque_base::allocator_type allocator_type; \
-    typedef deque_base::size_type size_type;  \
-    typedef deque_base::iterator iterator;  \
-    explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {}  \
-    template<typename InputIterator> \
-    deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : deque_base(first, last, a) {} \
-    deque(const deque& c) : deque_base(c) {}  \
-    explicit deque(size_type num, const value_type& val = value_type()) : deque_base(num, val) {} \
-    deque(iterator start_, iterator end_) : deque_base(start_, end_) {}  \
-    deque& operator=(const deque& x) {  \
-      deque_base::operator=(x);  \
-      return *this;  \
-    } \
-  }; \
-}
-
-// check whether we really need the std::deque specialization
-#if !EIGEN_HAS_CXX11_CONTAINERS && !(defined(_GLIBCXX_DEQUE) && (!EIGEN_GNUC_AT_LEAST(4,1))) /* Note that before gcc-4.1 we already have: std::deque::resize(size_type,const T&). */
-
-namespace std {
-
-#define EIGEN_STD_DEQUE_SPECIALIZATION_BODY \
-  public:  \
-    typedef T value_type; \
-    typedef typename deque_base::allocator_type allocator_type; \
-    typedef typename deque_base::size_type size_type;  \
-    typedef typename deque_base::iterator iterator;  \
-    typedef typename deque_base::const_iterator const_iterator;  \
-    explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {}  \
-    template<typename InputIterator> \
-    deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
-    : deque_base(first, last, a) {} \
-    deque(const deque& c) : deque_base(c) {}  \
-    explicit deque(size_type num, const value_type& val = value_type()) : deque_base(num, val) {} \
-    deque(iterator start_, iterator end_) : deque_base(start_, end_) {}  \
-    deque& operator=(const deque& x) {  \
-      deque_base::operator=(x);  \
-      return *this;  \
-    }
-
-  template<typename T>
-  class deque<T,EIGEN_ALIGNED_ALLOCATOR<T> >
-    : public deque<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                   Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
-{
-  typedef deque<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > deque_base;
-  EIGEN_STD_DEQUE_SPECIALIZATION_BODY
-
-  void resize(size_type new_size)
-  { resize(new_size, T()); }
-
-#if defined(_DEQUE_)
-  // workaround MSVC std::deque implementation
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (deque_base::size() < new_size)
-      deque_base::_Insert_n(deque_base::end(), new_size - deque_base::size(), x);
-    else if (new_size < deque_base::size())
-      deque_base::erase(deque_base::begin() + new_size, deque_base::end());
-  }
-  void push_back(const value_type& x)
-  { deque_base::push_back(x); } 
-  void push_front(const value_type& x)
-  { deque_base::push_front(x); }
-  using deque_base::insert;  
-  iterator insert(const_iterator position, const value_type& x)
-  { return deque_base::insert(position,x); }
-  void insert(const_iterator position, size_type new_size, const value_type& x)
-  { deque_base::insert(position, new_size, x); }
-#else
-  // default implementation which should always work.
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (new_size < deque_base::size())
-      deque_base::erase(deque_base::begin() + new_size, deque_base::end());
-    else if (new_size > deque_base::size())
-      deque_base::insert(deque_base::end(), new_size - deque_base::size(), x);
-  }
-#endif
-  };
-}
-
-#endif // check whether specialization is actually required
-
-#endif // EIGEN_STDDEQUE_H

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
deleted file mode 100644
index 8ba3fad..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
+++ /dev/null

@@ -1,106 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDLIST_H
-#define EIGEN_STDLIST_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::list such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) \
-namespace std \
-{ \
-  template<> \
-  class list<__VA_ARGS__, std::allocator<__VA_ARGS__> >           \
-    : public list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
-  { \
-    typedef list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > list_base; \
-  public: \
-    typedef __VA_ARGS__ value_type; \
-    typedef list_base::allocator_type allocator_type; \
-    typedef list_base::size_type size_type;  \
-    typedef list_base::iterator iterator;  \
-    explicit list(const allocator_type& a = allocator_type()) : list_base(a) {}  \
-    template<typename InputIterator> \
-    list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : list_base(first, last, a) {} \
-    list(const list& c) : list_base(c) {}  \
-    explicit list(size_type num, const value_type& val = value_type()) : list_base(num, val) {} \
-    list(iterator start_, iterator end_) : list_base(start_, end_) {}  \
-    list& operator=(const list& x) {  \
-      list_base::operator=(x);  \
-      return *this;  \
-    } \
-  }; \
-}
-
-// check whether we really need the std::list specialization
-#if !EIGEN_HAS_CXX11_CONTAINERS && !(defined(_GLIBCXX_LIST) && (!EIGEN_GNUC_AT_LEAST(4,1))) /* Note that before gcc-4.1 we already have: std::list::resize(size_type,const T&). */
-
-namespace std
-{
-
-#define EIGEN_STD_LIST_SPECIALIZATION_BODY \
-  public:  \
-    typedef T value_type; \
-    typedef typename list_base::allocator_type allocator_type; \
-    typedef typename list_base::size_type size_type;  \
-    typedef typename list_base::iterator iterator;  \
-    typedef typename list_base::const_iterator const_iterator;  \
-    explicit list(const allocator_type& a = allocator_type()) : list_base(a) {}  \
-    template<typename InputIterator> \
-    list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
-    : list_base(first, last, a) {} \
-    list(const list& c) : list_base(c) {}  \
-    explicit list(size_type num, const value_type& val = value_type()) : list_base(num, val) {} \
-    list(iterator start_, iterator end_) : list_base(start_, end_) {}  \
-    list& operator=(const list& x) {  \
-    list_base::operator=(x);  \
-    return *this; \
-  }
-
-  template<typename T>
-  class list<T,EIGEN_ALIGNED_ALLOCATOR<T> >
-    : public list<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                  Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
-  {
-    typedef list<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                 Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > list_base;
-    EIGEN_STD_LIST_SPECIALIZATION_BODY
-
-    void resize(size_type new_size)
-    { resize(new_size, T()); }
-
-    void resize(size_type new_size, const value_type& x)
-    {
-      if (list_base::size() < new_size)
-        list_base::insert(list_base::end(), new_size - list_base::size(), x);
-      else
-        while (new_size < list_base::size()) list_base::pop_back();
-    }
-
-#if defined(_LIST_)
-    // workaround MSVC std::list implementation
-    void push_back(const value_type& x)
-    { list_base::push_back(x); } 
-    using list_base::insert;  
-    iterator insert(const_iterator position, const value_type& x)
-    { return list_base::insert(position,x); }
-    void insert(const_iterator position, size_type new_size, const value_type& x)
-    { list_base::insert(position, new_size, x); }
-#endif
-  };
-}
-
-#endif // check whether specialization is actually required
-
-#endif // EIGEN_STDLIST_H

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
deleted file mode 100644
index 9fcf19b..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
+++ /dev/null

@@ -1,131 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDVECTOR_H
-#define EIGEN_STDVECTOR_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::vector such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) \
-namespace std \
-{ \
-  template<> \
-  class vector<__VA_ARGS__, std::allocator<__VA_ARGS__> >  \
-    : public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
-  { \
-    typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > vector_base; \
-  public: \
-    typedef __VA_ARGS__ value_type; \
-    typedef vector_base::allocator_type allocator_type; \
-    typedef vector_base::size_type size_type;  \
-    typedef vector_base::iterator iterator;  \
-    explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {}  \
-    template<typename InputIterator> \
-    vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : vector_base(first, last, a) {} \
-    vector(const vector& c) : vector_base(c) {}  \
-    explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
-    vector(iterator start_, iterator end_) : vector_base(start_, end_) {}  \
-    vector& operator=(const vector& x) {  \
-      vector_base::operator=(x);  \
-      return *this;  \
-    } \
-  }; \
-}
-
-// Don't specialize if containers are implemented according to C++11
-#if !EIGEN_HAS_CXX11_CONTAINERS
-
-namespace std {
-
-#define EIGEN_STD_VECTOR_SPECIALIZATION_BODY \
-  public:  \
-    typedef T value_type; \
-    typedef typename vector_base::allocator_type allocator_type; \
-    typedef typename vector_base::size_type size_type;  \
-    typedef typename vector_base::iterator iterator;  \
-    typedef typename vector_base::const_iterator const_iterator;  \
-    explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {}  \
-    template<typename InputIterator> \
-    vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
-    : vector_base(first, last, a) {} \
-    vector(const vector& c) : vector_base(c) {}  \
-    explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
-    vector(iterator start_, iterator end_) : vector_base(start_, end_) {}  \
-    vector& operator=(const vector& x) {  \
-      vector_base::operator=(x);  \
-      return *this;  \
-    }
-
-  template<typename T>
-  class vector<T,EIGEN_ALIGNED_ALLOCATOR<T> >
-    : public vector<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                    Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
-{
-  typedef vector<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                 Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > vector_base;
-  EIGEN_STD_VECTOR_SPECIALIZATION_BODY
-
-  void resize(size_type new_size)
-  { resize(new_size, T()); }
-
-#if defined(_VECTOR_)
-  // workaround MSVC std::vector implementation
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (vector_base::size() < new_size)
-      vector_base::_Insert_n(vector_base::end(), new_size - vector_base::size(), x);
-    else if (new_size < vector_base::size())
-      vector_base::erase(vector_base::begin() + new_size, vector_base::end());
-  }
-  void push_back(const value_type& x)
-  { vector_base::push_back(x); } 
-  using vector_base::insert;  
-  iterator insert(const_iterator position, const value_type& x)
-  { return vector_base::insert(position,x); }
-  void insert(const_iterator position, size_type new_size, const value_type& x)
-  { vector_base::insert(position, new_size, x); }
-#elif defined(_GLIBCXX_VECTOR) && (!(EIGEN_GNUC_AT_LEAST(4,1)))
-  /* Note that before gcc-4.1 we already have: std::vector::resize(size_type,const T&).
-   * However, this specialization is still needed to make the above EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION trick to work. */
-  void resize(size_type new_size, const value_type& x)
-  {
-    vector_base::resize(new_size,x);
-  }
-#elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,2)
-  // workaround GCC std::vector implementation
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (new_size < vector_base::size())
-      vector_base::_M_erase_at_end(this->_M_impl._M_start + new_size);
-    else
-      vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
-  }
-#else
-  // either GCC 4.1 or non-GCC
-  // default implementation which should always work.
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (new_size < vector_base::size())
-      vector_base::erase(vector_base::begin() + new_size, vector_base::end());
-    else if (new_size > vector_base::size())
-      vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
-  }
-#endif
-  };
-}
-#endif // !EIGEN_HAS_CXX11_CONTAINERS
-
-
-#endif // EIGEN_STDVECTOR_H

diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
deleted file mode 100644
index 2cfd13e..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
+++ /dev/null

@@ -1,84 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STL_DETAILS_H
-#define EIGEN_STL_DETAILS_H
-
-#ifndef EIGEN_ALIGNED_ALLOCATOR
-  #define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator
-#endif
-
-namespace Eigen {
-
-  // This one is needed to prevent reimplementing the whole std::vector.
-  template <class T>
-  class aligned_allocator_indirection : public EIGEN_ALIGNED_ALLOCATOR<T>
-  {
-  public:
-    typedef std::size_t     size_type;
-    typedef std::ptrdiff_t  difference_type;
-    typedef T*              pointer;
-    typedef const T*        const_pointer;
-    typedef T&              reference;
-    typedef const T&        const_reference;
-    typedef T               value_type;
-
-    template<class U>
-    struct rebind
-    {
-      typedef aligned_allocator_indirection<U> other;
-    };
-
-    aligned_allocator_indirection() {}
-    aligned_allocator_indirection(const aligned_allocator_indirection& ) : EIGEN_ALIGNED_ALLOCATOR<T>() {}
-    aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<T>& ) {}
-    template<class U>
-    aligned_allocator_indirection(const aligned_allocator_indirection<U>& ) {}
-    template<class U>
-    aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<U>& ) {}
-    ~aligned_allocator_indirection() {}
-  };
-
-#if EIGEN_COMP_MSVC
-
-  // sometimes, MSVC detects, at compile time, that the argument x
-  // in std::vector::resize(size_t s,T x) won't be aligned and generate an error
-  // even if this function is never called. Whence this little wrapper.
-#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) \
-  typename Eigen::internal::conditional< \
-    Eigen::internal::is_arithmetic<T>::value, \
-    T, \
-    Eigen::internal::workaround_msvc_stl_support<T> \
-  >::type
-
-  namespace internal {
-  template<typename T> struct workaround_msvc_stl_support : public T
-  {
-    inline workaround_msvc_stl_support() : T() {}
-    inline workaround_msvc_stl_support(const T& other) : T(other) {}
-    inline operator T& () { return *static_cast<T*>(this); }
-    inline operator const T& () const { return *static_cast<const T*>(this); }
-    template<typename OtherT>
-    inline T& operator=(const OtherT& other)
-    { T::operator=(other); return *this; }
-    inline workaround_msvc_stl_support& operator=(const workaround_msvc_stl_support& other)
-    { T::operator=(other); return *this; }
-  };
-  }
-
-#else
-
-#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) T
-
-#endif
-
-}
-
-#endif // EIGEN_STL_DETAILS_H

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
index cb28ff0..650d05d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
index 0a3ada6..6d7b66d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
index 9d3bc07..a47003d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
index 79579d7..8767200 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
index 210d9fc..6a79e87 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
index dfad57e..a5f3ff6 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
index 9aea85b..3f46974 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
index 97c8d6a..5ca55b7 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
index 2e1cb76..dfb4dc3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
index e888e47..e43d4fc 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
index fb050a2..0fc17f3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
index e8570ab..ff1097b 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
index a3bab74..c1741f7 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
index 0e98012..82f4c60 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
index b3cebb8..fc89c0d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
index afec09e..d0bc83a 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
index a93f8db..412d686 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
index 0c66829..595ffc2 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -28,6 +28,30 @@
 namespace internal
 {
 
+// see https://en.wikipedia.org/wiki/Euler%27s_continued_fraction_formula
+
+#if __cplusplus >= 201402L // C++14 version
+
+template<typename T>
+constexpr
+T
+exp_cf_recur(const T x, const int depth_end)
+noexcept
+{
+    int depth = GCEM_EXP_MAX_ITER_SMALL - 1;
+    T res = T(1);
+
+    while (depth > depth_end - 1) {
+        res = T(1) + x/T(depth - 1) - x/depth/res;
+
+        --depth;
+    }
+
+    return res;
+}
+
+#else // C++11 version
+
 template<typename T>
 constexpr
 T
@@ -36,20 +60,20 @@
 {
     return( depth < GCEM_EXP_MAX_ITER_SMALL ? \
             // if
-                depth == 1 ? \
-                    T(1) - x/exp_cf_recur(x,depth+1) : 
-                    T(1) + x/T(depth - 1) - x/depth/exp_cf_recur(x,depth+1) : 
+                T(1) + x/T(depth - 1) - x/depth/exp_cf_recur(x,depth+1) : 
              // else
                 T(1) );
 }
 
+#endif
+
 template<typename T>
 constexpr
 T
 exp_cf(const T x)
 noexcept
 {
-    return( T(1)/exp_cf_recur(x,1) );
+    return( T(1) / (T(1) - x / exp_cf_recur(x,2)) );
 }
 
 template<typename T>
@@ -72,7 +96,7 @@
             //
             is_neginf(x) ? \
                 T(0) :
-            //
+            // indistinguishable from zero
             GCLIM<T>::min() > abs(x) ? \
                 T(1) : 
             //

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
index 11b2eb9..70c9ecf 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
index 539c3f3..ffb9c82 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
index 710adce..200e4e9 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -34,10 +34,20 @@
 find_exponent(const T x, const llint_t exponent)
 noexcept
 {
-    return( x < T(1)  ? \
-                find_exponent(x*T(10),exponent - llint_t(1)) :
+    return( // < 1
+            x < T(1e-03)  ? \
+                find_exponent(x * T(1e+04), exponent - llint_t(4)) :
+            x < T(1e-01)  ? \
+                find_exponent(x * T(1e+02), exponent - llint_t(2)) :
+            x < T(1)  ? \
+                find_exponent(x * T(10), exponent - llint_t(1)) :
+            // > 10
             x > T(10) ? \
-                find_exponent(x/T(10),exponent + llint_t(1)) :
+                find_exponent(x / T(10), exponent + llint_t(1)) :
+            x > T(1e+02) ? \
+                find_exponent(x / T(1e+02), exponent + llint_t(2)) :
+            x > T(1e+04) ? \
+                find_exponent(x / T(1e+04), exponent + llint_t(4)) :
             // else
                 exponent );
 }

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
index d9769e6..5ed3d26 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
index bd5e0b9..d193632 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
index c60ff6a..8b260ff 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
index c804ce6..02459ef 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
index 4a10bbe..1e277fb 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
index cd2747c..4113738 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -53,7 +53,7 @@
 #endif
 
 #ifndef GCEM_VERSION_MINOR
-    #define GCEM_VERSION_MINOR 16
+    #define GCEM_VERSION_MINOR 17
 #endif
 
 #ifndef GCEM_VERSION_PATCH

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
index 5a805ed..01ad4e9 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
index 5645dbe..dbb9f60 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
index f7fdfa0..9f575a3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
index 38734a5..9ee4146 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
index 1e57fc1..e5976d0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
index 0200f11..d0e33fb 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
index fa925bb..de0641d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
index 25f2e3c..b632fa3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
index 627c509..568614f 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
index a7a1af3..a3fcbc6 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
index e6da720..a74a8d3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
index 2213849..60c87b4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
index b0d8fb4..a7ca776 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
index 5d78eb3..507c6d4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
index 76bf833..58915dc 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
index 0d83e97..c2e24b0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -31,6 +31,28 @@
 // continued fraction seems to be a better approximation for small x
 // see http://functions.wolfram.com/ElementaryFunctions/Log/10/0005/
 
+#if __cplusplus >= 201402L // C++14 version
+
+template<typename T>
+constexpr
+T
+log_cf_main(const T xx, const int depth_end)
+noexcept
+{
+    int depth = GCEM_LOG_MAX_ITER_SMALL - 1;
+    T res = T(2*(depth+1) - 1);
+
+    while (depth > depth_end - 1) {
+        res = T(2*depth - 1) - T(depth*depth) * xx / res;
+
+        --depth;
+    }
+
+    return res;
+}
+
+#else // C++11 version
+
 template<typename T>
 constexpr
 T
@@ -39,11 +61,13 @@
 {
     return( depth < GCEM_LOG_MAX_ITER_SMALL ? \
             // if 
-                T(2*depth - 1) - T(depth*depth)*xx/log_cf_main(xx,depth+1) :
+                T(2*depth - 1) - T(depth*depth) * xx / log_cf_main(xx,depth+1) :
             // else 
                 T(2*depth - 1) );
 }
 
+#endif
+
 template<typename T>
 constexpr
 T

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
index 4a3c37d..cda8894 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
index 3883b22..ccd08b8 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
index 56b7f8e..a97fed4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
index 7aa9a2b..2bcaadd 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
index df6152b..af23ea2 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -35,9 +35,9 @@
 noexcept
 {
     return( x < T(1) ? \
-                mantissa(x*T(10)) : 
+                mantissa(x * T(10)) : 
             x > T(10) ? \
-                mantissa(x/T(10)) :
+                mantissa(x / T(10)) :
             // else
                 x );
 }

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
index 4aed84f..ddc3e4e 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
index d593dbc..5ce70b3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
index db33f87..79d24a4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
index 166a8c1..3891ede 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
index 3a902ca..4e67155 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
index e609b89..295f43d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
index 44281f9..d4e448c 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
index 43d7a5e..9ac4a09 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
index 605a35a..e2eec9e 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
index e207a5a..282e244 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
index 128cd32..56c8dca 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
index 5355301..fe3ecdd 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
index 0fd559d..1b2753c 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -37,16 +37,37 @@
     return( abs(xn - x/xn) / (T(1) + xn) < GCLIM<T>::min() ? \
             // if
                 xn :
-            count < GCEM_SQRT_MAX_ITER ? \
             // else
-                sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) :
-                xn );
+                count < GCEM_SQRT_MAX_ITER ? \
+                // if
+                    sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) :
+                // else
+                    xn );
 }
 
 template<typename T>
 constexpr
 T
-sqrt_check(const T x, const T m_val)
+sqrt_simplify(const T x, const T m_val)
+noexcept
+{
+    return( x > T(1e+08) ? \
+                sqrt_simplify(x / T(1e+08), T(1e+04) * m_val) :
+            x > T(1e+06) ? \
+                sqrt_simplify(x / T(1e+06), T(1e+03) * m_val) :
+            x > T(1e+04) ? \
+                sqrt_simplify(x / T(1e+04), T(1e+02) * m_val) :
+            x > T(100) ? \
+                sqrt_simplify(x / T(100), T(10) * m_val) :
+            x > T(4) ? \
+                sqrt_simplify(x / T(4), T(2) * m_val) :
+                m_val * sqrt_recur(x, x / T(2), 0) );
+}
+
+template<typename T>
+constexpr
+T
+sqrt_check(const T x)
 noexcept
 {
     return( is_nan(x) ? \
@@ -63,9 +84,7 @@
             GCLIM<T>::min() > abs(T(1) - x) ? \
                 x :
             // else
-            x > T(4) ? \
-                sqrt_check(x/T(4), T(2)*m_val) :
-                m_val * sqrt_recur(x, x/T(2), 0) );
+            sqrt_simplify(x, T(1)) );
 }
 
 }
@@ -84,7 +103,7 @@
 sqrt(const T x)
 noexcept
 {
-    return internal::sqrt_check( static_cast<return_t<T>>(x), return_t<T>(1) );
+    return internal::sqrt_check( static_cast<return_t<T>>(x) );
 }
 
 #endif

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
index e53f5c8..386cce0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
index 109d751..30b4318 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
index 5a9ae97..deffd3a 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
index 4e19ef9..af3f448 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp

@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##

diff --git a/wpimath/src/main/proto/controller.proto b/wpimath/src/main/proto/controller.proto
new file mode 100644
index 0000000..0d0d3fb
--- /dev/null
+++ b/wpimath/src/main/proto/controller.proto

@@ -0,0 +1,37 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufArmFeedforward {
+  double ks = 1;
+  double kg = 2;
+  double kv = 3;
+  double ka = 4;
+}
+
+message ProtobufDifferentialDriveFeedforward {
+  double kv_linear = 1;
+  double ka_linear = 2;
+  double kv_angular = 3;
+  double ka_angular = 4;
+}
+
+message ProtobufElevatorFeedforward {
+  double ks = 1;
+  double kg = 2;
+  double kv = 3;
+  double ka = 4;
+}
+
+message ProtobufSimpleMotorFeedforward {
+  double ks = 1;
+  double kv = 2;
+  double ka = 3;
+}
+
+message ProtobufDifferentialDriveWheelVoltages {
+  double left = 1;
+  double right = 2;
+}

diff --git a/wpimath/src/main/proto/geometry2d.proto b/wpimath/src/main/proto/geometry2d.proto
new file mode 100644
index 0000000..d52da45
--- /dev/null
+++ b/wpimath/src/main/proto/geometry2d.proto

@@ -0,0 +1,30 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTranslation2d {
+  double x = 1;
+  double y = 2;
+}
+
+message ProtobufRotation2d {
+  double value = 1;
+}
+
+message ProtobufPose2d {
+  ProtobufTranslation2d translation = 1;
+  ProtobufRotation2d rotation = 2;
+}
+
+message ProtobufTransform2d {
+  ProtobufTranslation2d translation = 1;
+  ProtobufRotation2d rotation = 2;
+}
+
+message ProtobufTwist2d {
+  double dx = 1;
+  double dy = 2;
+  double dtheta = 3;
+}

diff --git a/wpimath/src/main/proto/geometry3d.proto b/wpimath/src/main/proto/geometry3d.proto
new file mode 100644
index 0000000..23b4d64
--- /dev/null
+++ b/wpimath/src/main/proto/geometry3d.proto

@@ -0,0 +1,41 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTranslation3d {
+  double x = 1;
+  double y = 2;
+  double z = 3;
+}
+
+message ProtobufQuaternion {
+  double w = 1;
+  double x = 2;
+  double y = 3;
+  double z = 4;
+}
+
+message ProtobufRotation3d {
+  ProtobufQuaternion q = 1;
+}
+
+message ProtobufPose3d {
+  ProtobufTranslation3d translation = 1;
+  ProtobufRotation3d rotation = 2;
+}
+
+message ProtobufTransform3d {
+  ProtobufTranslation3d translation = 1;
+  ProtobufRotation3d rotation = 2;
+}
+
+message ProtobufTwist3d {
+  double dx = 1;
+  double dy = 2;
+  double dz = 3;
+  double rx = 4;
+  double ry = 5;
+  double rz = 6;
+}

diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto
new file mode 100644
index 0000000..8fdbf2d
--- /dev/null
+++ b/wpimath/src/main/proto/kinematics.proto

@@ -0,0 +1,64 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "geometry2d.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufChassisSpeeds {
+  double vx = 1;
+  double vy = 2;
+  double omega = 3;
+}
+
+message ProtobufDifferentialDriveKinematics {
+  double track_width = 1;
+}
+
+message ProtobufDifferentialDriveWheelSpeeds {
+  double left = 1;
+  double right = 2;
+}
+
+message ProtobufMecanumDriveKinematics {
+  ProtobufTranslation2d front_left = 1;
+  ProtobufTranslation2d front_right = 2;
+  ProtobufTranslation2d rear_left = 3;
+  ProtobufTranslation2d rear_right = 4;
+}
+
+message ProtobufMecanumDriveMotorVoltages {
+  double front_left = 1;
+  double front_right = 2;
+  double rear_left = 3;
+  double rear_right = 4;
+}
+
+message ProtobufMecanumDriveWheelPositions {
+  double front_left = 1;
+  double front_right = 2;
+  double rear_left = 3;
+  double rear_right = 4;
+}
+
+message ProtobufMecanumDriveWheelSpeeds {
+  double front_left = 1;
+  double front_right = 2;
+  double rear_left = 3;
+  double rear_right = 4;
+}
+
+message ProtobufSwerveDriveKinematics {
+  repeated ProtobufTranslation2d modules = 1;
+}
+
+message ProtobufSwerveModulePosition {
+  double distance = 1;
+  ProtobufRotation2d angle = 2;
+}
+
+message ProtobufSwerveModuleState {
+  double speed = 1;
+  ProtobufRotation2d angle = 2;
+}

diff --git a/wpimath/src/main/proto/plant.proto b/wpimath/src/main/proto/plant.proto
new file mode 100644
index 0000000..d0d9eab
--- /dev/null
+++ b/wpimath/src/main/proto/plant.proto

@@ -0,0 +1,16 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufDCMotor {
+  double nominal_voltage = 1;
+  double stall_torque = 2;
+  double stall_current = 3;
+  double free_current = 4;
+  double free_speed = 5;
+  double r = 6;
+  double kv = 7;
+  double kt = 8;
+}

diff --git a/wpimath/src/main/proto/spline.proto b/wpimath/src/main/proto/spline.proto
new file mode 100644
index 0000000..ef0a3c9
--- /dev/null
+++ b/wpimath/src/main/proto/spline.proto

@@ -0,0 +1,19 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufCubicHermiteSpline {
+  repeated double x_initial = 1;
+  repeated double x_final = 2;
+  repeated double y_initial = 3;
+  repeated double y_final = 4;
+}
+
+message ProtobufQuinticHermiteSpline {
+  repeated double x_initial = 1;
+  repeated double x_final = 2;
+  repeated double y_initial = 3;
+  repeated double y_final = 4;
+}

diff --git a/wpimath/src/main/proto/system.proto b/wpimath/src/main/proto/system.proto
new file mode 100644
index 0000000..4818c19
--- /dev/null
+++ b/wpimath/src/main/proto/system.proto

@@ -0,0 +1,17 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "wpimath.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufLinearSystem {
+  uint32 num_states = 1;
+  uint32 num_inputs = 2;
+  uint32 num_outputs = 3;
+  ProtobufMatrix a = 4;
+  ProtobufMatrix b = 5;
+  ProtobufMatrix c = 6;
+  ProtobufMatrix d = 7;
+}

diff --git a/wpimath/src/main/proto/trajectory.proto b/wpimath/src/main/proto/trajectory.proto
new file mode 100644
index 0000000..a37a501
--- /dev/null
+++ b/wpimath/src/main/proto/trajectory.proto

@@ -0,0 +1,20 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "geometry2d.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTrajectoryState {
+  double time = 1;
+  double velocity = 2;
+  double acceleration = 3;
+  ProtobufPose2d pose = 4;
+  double curvature = 5;
+}
+
+message ProtobufTrajectory {
+  double total_time = 1;
+  repeated ProtobufTrajectoryState states = 2;
+}

diff --git a/wpimath/src/main/proto/wpimath.proto b/wpimath/src/main/proto/wpimath.proto
new file mode 100644
index 0000000..06b993a
--- /dev/null
+++ b/wpimath/src/main/proto/wpimath.proto

@@ -0,0 +1,15 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufMatrix {
+  uint32 num_rows = 1;
+  uint32 num_cols = 2;
+  repeated double data = 3;
+}
+
+message ProtobufVector {
+  repeated double rows = 1;
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
index 2927847..3c281e5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java

@@ -11,9 +11,14 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.UtilityClassTest;
 import org.junit.jupiter.api.Test;
 
-class ComputerVisionUtilTest {
+class ComputerVisionUtilTest extends UtilityClassTest<ComputerVisionUtil> {
+  ComputerVisionUtilTest() {
+    super(ComputerVisionUtil.class);
+  }
+
   @Test
   void testObjectToRobotPose() {
     var robot = new Pose3d(1.0, 2.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)));

diff --git a/wpimath/src/test/java/edu/wpi/first/math/DARETest.java b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
new file mode 100644
index 0000000..0d697bd
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java

@@ -0,0 +1,335 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+import org.ejml.simple.SimpleMatrix;
+import org.junit.jupiter.api.Test;
+
+class DARETest extends UtilityClassTest<DARE> {
+  DARETest() {
+    super(DARE.class);
+  }
+
+  public static <R extends Num, C extends Num> void assertMatrixEqual(
+      Matrix<R, C> A, Matrix<R, C> B) {
+    for (int i = 0; i < A.getNumRows(); i++) {
+      for (int j = 0; j < A.getNumCols(); j++) {
+        assertEquals(A.get(i, j), B.get(i, j), 1e-4);
+      }
+    }
+  }
+
+  <States extends Num, Inputs extends Num> void assertDARESolution(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, States> X) {
+    // Check that X is the solution to the DARE
+    // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+    var Y =
+        (A.transpose().times(X).times(A))
+            .minus(X)
+            .minus(
+                (A.transpose().times(X).times(B))
+                    .times((B.transpose().times(X).times(B).plus(R)).inv())
+                    .times(B.transpose().times(X).times(A)))
+            .plus(Q);
+    assertMatrixEqual(
+        new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+  }
+
+  <States extends Num, Inputs extends Num> void assertDARESolution(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, Inputs> N,
+      Matrix<States, States> X) {
+    // Check that X is the solution to the DARE
+    // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+    var Y =
+        (A.transpose().times(X).times(A))
+            .minus(X)
+            .minus(
+                (A.transpose().times(X).times(B).plus(N))
+                    .times((B.transpose().times(X).times(B).plus(R)).inv())
+                    .times(B.transpose().times(X).times(A).plus(N.transpose())))
+            .plus(Q);
+    assertMatrixEqual(
+        new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+  }
+
+  @Test
+  void testNonInvertibleA_ABQR() {
+    // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+    // Riccati Equation"
+
+    var A =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+    var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
+    var Q =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testNonInvertibleA_ABQRN() {
+    // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+    // Riccati Equation"
+
+    var A =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+    var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
+    var Q =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
+
+    var Aref =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+    Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+    R = B.transpose().times(Q).times(B).plus(R);
+    var N = A.minus(Aref).transpose().times(Q).times(B);
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testInvertibleA_ABQR() {
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
+    var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testInvertibleA_ABQRN() {
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
+    var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
+
+    var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.5, 1, 0, 1});
+    Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+    R = B.transpose().times(Q).times(B).plus(R);
+    var N = A.minus(Aref).transpose().times(Q).times(B);
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testFirstGeneralizedEigenvalueOfSTIsStable_ABQR() {
+    // The first generalized eigenvalue of (S, T) is stable
+
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
+    var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testFirstGeneralizedEigenvalueOfSTIsStable_ABQRN() {
+    // The first generalized eigenvalue of (S, T) is stable
+
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
+    var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
+
+    var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 0.5, 0, 0});
+    Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+    R = B.transpose().times(Q).times(B).plus(R);
+    var N = A.minus(Aref).transpose().times(Q).times(B);
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testIdentitySystem_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testIdentitySystem_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+    var N = Matrix.eye(Nat.N2());
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testMoreInputsThanStates_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N3());
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testMoreInputsThanStates_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N3());
+    var N = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 1, 0});
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testQNotSymmetricPositiveSemidefinite_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+    var R = Matrix.eye(Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+  }
+
+  @Test
+  void testQNotSymmetricPositiveSemidefinite_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+    var N = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {2.0, 0.0, 0.0, 2.0});
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+  }
+
+  @Test
+  void testRNotSymmetricPositiveDefinite_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+
+    var R1 = new Matrix<>(Nat.N2(), Nat.N2());
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1));
+
+    var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2));
+  }
+
+  @Test
+  void testRNotSymmetricPositiveDefinite_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var N = Matrix.eye(Nat.N2());
+
+    var R1 = new Matrix<>(Nat.N2(), Nat.N2());
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1, N));
+
+    var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2, N));
+  }
+
+  @Test
+  void testABNotStabilizable_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = new Matrix<>(Nat.N2(), Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+  }
+
+  @Test
+  void testABNotStabilizable_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = new Matrix<>(Nat.N2(), Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+    var N = Matrix.eye(Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+  }
+
+  @Test
+  void testACNotDetectable_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = new Matrix<>(Nat.N2(), Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+  }
+
+  @Test
+  void testACNotDetectable_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = new Matrix<>(Nat.N2(), Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+    var N = new Matrix<>(Nat.N2(), Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+  }
+
+  @Test
+  void testQDecomposition() {
+    // Ensures the decomposition of Q into CᵀC is correct
+
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1.0, 0.0, 0.0, 0.0});
+    var B = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    // (A, C₁) should be detectable pair
+    var C_1 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 0.0, 1.0, 0.0});
+    var Q_1 = C_1.transpose().times(C_1);
+    assertDoesNotThrow(() -> DARE.dare(A, B, Q_1, R));
+
+    // (A, C₂) shouldn't be detectable pair
+    var C_2 = C_1.transpose();
+    var Q_2 = C_2.transpose().times(C_2);
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q_2, R));
+  }
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
deleted file mode 100644
index 3050867..0000000
--- a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
+++ /dev/null

@@ -1,73 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.math;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-import org.ejml.simple.SimpleMatrix;
-import org.junit.jupiter.api.Test;
-
-class DrakeTest {
-  public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
-    for (int i = 0; i < A.numRows(); i++) {
-      for (int j = 0; j < A.numCols(); j++) {
-        assertEquals(A.get(i, j), B.get(i, j), 1e-4);
-      }
-    }
-  }
-
-  private boolean solveDAREandVerify(
-      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
-    var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R);
-
-    // expect that x is the same as it's transpose
-    assertEquals(X.numRows(), X.numCols());
-    assertMatrixEqual(X, X.transpose());
-
-    // Verify that this is a solution to the DARE.
-    SimpleMatrix Y =
-        A.transpose()
-            .mult(X)
-            .mult(A)
-            .minus(X)
-            .minus(
-                A.transpose()
-                    .mult(X)
-                    .mult(B)
-                    .mult(((B.transpose().mult(X).mult(B)).plus(R)).invert())
-                    .mult(B.transpose())
-                    .mult(X)
-                    .mult(A))
-            .plus(Q);
-    assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols()));
-
-    return true;
-  }
-
-  @Test
-  void testDiscreteAlgebraicRicattiEquation() {
-    int n1 = 4;
-    int m1 = 1;
-
-    // we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]]
-    SimpleMatrix A1 =
-        new SimpleMatrix(
-                n1, n1, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0})
-            .transpose();
-    SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[] {0, 0, 0, 1});
-    SimpleMatrix Q1 =
-        new SimpleMatrix(
-            n1, n1, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
-    SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[] {0.25});
-    assertTrue(solveDAREandVerify(A1, B1, Q1, R1));
-
-    SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
-    SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[] {0, 1});
-    SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
-    SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[] {0.3});
-    assertTrue(solveDAREandVerify(A2, B2, Q2, R2));
-  }
-}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
index a3de9cc..44f7c50 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java

@@ -5,10 +5,32 @@
 package edu.wpi.first.math;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.wpilibj.UtilityClassTest;
 import org.junit.jupiter.api.Test;
 
-class MathUtilTest {
+class MathUtilTest extends UtilityClassTest<MathUtil> {
+  MathUtilTest() {
+    super(MathUtil.class);
+  }
+
+  @Test
+  void testClamp() {
+    // int
+    assertEquals(5, MathUtil.clamp(10, 1, 5));
+
+    // double
+    assertEquals(5.5, MathUtil.clamp(10.5, 1.5, 5.5));
+
+    // negative int
+    assertEquals(-5, MathUtil.clamp(-10, -5, -1));
+
+    // negative double
+    assertEquals(-5.5, MathUtil.clamp(-10.5, -5.5, -1.5));
+  }
+
   @Test
   void testApplyDeadbandUnityScale() {
     // < 0
@@ -88,4 +110,61 @@
     assertEquals(MathUtil.angleModulus(Math.PI / 2), Math.PI / 2);
     assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2);
   }
+
+  @Test
+  void testInterpolate() {
+    assertEquals(50, MathUtil.interpolate(0, 100, 0.5));
+    assertEquals(-50, MathUtil.interpolate(0, -100, 0.5));
+    assertEquals(0, MathUtil.interpolate(-50, 50, 0.5));
+    assertEquals(-25, MathUtil.interpolate(-50, 50, 0.25));
+    assertEquals(25, MathUtil.interpolate(-50, 50, 0.75));
+
+    assertEquals(0, MathUtil.interpolate(0, -100, -0.5));
+  }
+
+  @Test
+  void testIsNear() {
+    // The answer is always 42
+    // Positive integer checks
+    assertTrue(MathUtil.isNear(42, 42, 1));
+    assertTrue(MathUtil.isNear(42, 41, 2));
+    assertTrue(MathUtil.isNear(42, 43, 2));
+    assertFalse(MathUtil.isNear(42, 44, 1));
+
+    // Negative integer checks
+    assertTrue(MathUtil.isNear(-42, -42, 1));
+    assertTrue(MathUtil.isNear(-42, -41, 2));
+    assertTrue(MathUtil.isNear(-42, -43, 2));
+    assertFalse(MathUtil.isNear(-42, -44, 1));
+
+    // Mixed sign integer checks
+    assertFalse(MathUtil.isNear(-42, 42, 1));
+    assertFalse(MathUtil.isNear(-42, 41, 2));
+    assertFalse(MathUtil.isNear(-42, 43, 2));
+    assertFalse(MathUtil.isNear(42, -42, 1));
+    assertFalse(MathUtil.isNear(42, -41, 2));
+    assertFalse(MathUtil.isNear(42, -43, 2));
+
+    // Floating point checks
+    assertTrue(MathUtil.isNear(42, 41.5, 1));
+    assertTrue(MathUtil.isNear(42, 42.5, 1));
+    assertTrue(MathUtil.isNear(42, 41.5, 0.75));
+    assertTrue(MathUtil.isNear(42, 42.5, 0.75));
+
+    // Wraparound checks
+    assertTrue(MathUtil.isNear(0, 356, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, -356, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, 4, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, -4, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, 41, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, -319, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, 401, 5, 0, 360));
+    assertFalse(MathUtil.isNear(0, 356, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, -356, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, 4, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, -4, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(400, 35, 5, 0, 360));
+    assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
+    assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
index 054c29a..ee33f01 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java

@@ -12,13 +12,18 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.wpilibj.UtilityClassTest;
 import java.util.ArrayList;
 import java.util.List;
 import org.ejml.dense.row.MatrixFeatures_DDRM;
 import org.ejml.simple.SimpleMatrix;
 import org.junit.jupiter.api.Test;
 
-class StateSpaceUtilTest {
+class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
+  StateSpaceUtilTest() {
+    super(StateSpaceUtil.class);
+  }
+
   @Test
   void testCostArray() {
     var mat = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(1.0, 2.0, 3.0));

diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
index 5488b68..008953f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java

@@ -10,6 +10,44 @@
 
 class VectorTest {
   @Test
+  void testVectorPlus() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+    var result1 = vec1.plus(vec2);
+
+    assertEquals(5.0, result1.get(0, 0));
+    assertEquals(7.0, result1.get(1, 0));
+    assertEquals(9.0, result1.get(2, 0));
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+    var result2 = vec3.plus(vec4);
+
+    assertEquals(3.0, result2.get(0, 0));
+    assertEquals(-3.0, result2.get(1, 0));
+    assertEquals(3.0, result2.get(2, 0));
+  }
+
+  @Test
+  void testVectorMinus() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+    var result1 = vec1.minus(vec2);
+
+    assertEquals(-3.0, result1.get(0, 0));
+    assertEquals(-3.0, result1.get(1, 0));
+    assertEquals(-3.0, result1.get(2, 0));
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+    var result2 = vec3.minus(vec4);
+
+    assertEquals(-5.0, result2.get(0, 0));
+    assertEquals(7.0, result2.get(1, 0));
+    assertEquals(-9.0, result2.get(2, 0));
+  }
+
+  @Test
   void testVectorDot() {
     var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
     var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
new file mode 100644
index 0000000..7b5cf77
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardTest {
+  private static final double ks = 0.5;
+  private static final double kg = 1;
+  private static final double kv = 1.5;
+  private static final double ka = 2;
+  private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+  @Test
+  void testCalculate() {
+    assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002);
+    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002);
+    assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002);
+    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 0.002);
+  }
+
+  @Test
+  void testAcheviableVelocity() {
+    assertEquals(6, m_armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-9, m_armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
+  }
+
+  @Test
+  void testAcheviableAcceleration() {
+    assertEquals(4.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(6.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+    assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+  }
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
index b23ee1e..faf5563 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java

@@ -231,7 +231,6 @@
               .times(xAccelLimiter)
               .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
-      System.out.println(a);
       assertTrue(Math.abs(a) > maxA);
       assertTrue(Math.abs(a) > -minA);
     }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
new file mode 100644
index 0000000..6c978ac
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardTest {
+  private static final double ks = 0.5;
+  private static final double kg = 1;
+  private static final double kv = 1.5;
+  private static final double ka = 2;
+
+  private final ElevatorFeedforward m_elevatorFF = new ElevatorFeedforward(ks, kg, kv, ka);
+
+  @Test
+  void testCalculate() {
+    assertEquals(1, m_elevatorFF.calculate(0), 0.002);
+    assertEquals(4.5, m_elevatorFF.calculate(2), 0.002);
+    assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
+    assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
+
+    var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka);
+    var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka);
+    final double dt = 0.02;
+    var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
+
+    var r = VecBuilder.fill(2.0);
+    var nextR = VecBuilder.fill(3.0);
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
+        m_elevatorFF.calculate(2.0, 3.0, dt),
+        0.002);
+  }
+
+  @Test
+  void testAcheviableVelocity() {
+    assertEquals(5, m_elevatorFF.maxAchievableVelocity(11, 1), 0.002);
+    assertEquals(-9, m_elevatorFF.minAchievableVelocity(11, 1), 0.002);
+  }
+
+  @Test
+  void testAcheviableAcceleration() {
+    assertEquals(3.75, m_elevatorFF.maxAchievableAcceleration(12, 2), 0.002);
+    assertEquals(7.25, m_elevatorFF.maxAchievableAcceleration(12, -2), 0.002);
+    assertEquals(-8.25, m_elevatorFF.minAchievableAcceleration(12, 2), 0.002);
+    assertEquals(-4.75, m_elevatorFF.minAchievableAcceleration(12, -2), 0.002);
+  }
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
index 31213f6..634ed1c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java

@@ -148,10 +148,10 @@
     assertEquals(0.51182128351092726, K.get(0, 1), 1e-10);
 
     // QRN overload
-    var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 2.0));
+    var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 5.0));
     var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
     assertEquals(0.0, Kimf.get(0, 0), 1e-10);
-    assertEquals(-5.367540084534802e-05, Kimf.get(0, 1), 1e-10);
+    assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10);
   }
 
   @Test

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
index 7b8484d..7ea8caa 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java

@@ -58,12 +58,12 @@
     TrapezoidProfile profile;
     TrapezoidProfile.State state;
     for (int i = 0; i < 1000; i++) {
-      profile =
-          new TrapezoidProfile(
-              constraints,
+      profile = new TrapezoidProfile(constraints);
+      state =
+          profile.calculate(
+              kDt,
               new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
               new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
-      state = profile.calculate(kDt);
       m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
 
       updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
index 1fe4cb1..b25240e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java

@@ -55,4 +55,24 @@
 
     assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
   }
+
+  @Test
+  void iZoneNoOutputTest() {
+    m_controller.setI(1);
+    m_controller.setIZone(1);
+
+    double out = m_controller.calculate(2, 0);
+
+    assertEquals(0, out, 1e-5);
+  }
+
+  @Test
+  void iZoneOutputTest() {
+    m_controller.setI(1);
+    m_controller.setIZone(1);
+
+    double out = m_controller.calculate(1, 0);
+
+    assertEquals(-1 * m_controller.getPeriod(), out, 1e-5);
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
index b525f49..4fdb867 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java

@@ -19,7 +19,7 @@
     try (var controller = new PIDController(0.05, 0.0, 0.0)) {
       controller.enableContinuousInput(-kRange / 2, kRange / 2);
 
-      assertTrue(controller.atSetpoint());
+      assertFalse(controller.atSetpoint());
     }
   }
 
@@ -28,10 +28,7 @@
     try (var controller = new PIDController(0.05, 0.0, 0.0)) {
       controller.enableContinuousInput(-kRange / 2, kRange / 2);
 
-      assertTrue(
-          controller.atSetpoint(),
-          "Error was not in tolerance when it should have been. Error was "
-              + controller.getPositionError());
+      assertFalse(controller.atSetpoint());
 
       controller.setTolerance(kTolerance);
       controller.setSetpoint(kSetpoint);

diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index c3906d4..e0e202f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java

@@ -143,8 +143,6 @@
 
     double t = 0.0;
 
-    System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
-
     final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
 
     double maxError = Double.NEGATIVE_INFINITY;
@@ -191,16 +189,6 @@
               leftDistanceMeters,
               rightDistanceMeters);
 
-      System.out.printf(
-          "%f, %f, %f, %f, %f, %f, %f\n",
-          t,
-          xHat.getX(),
-          xHat.getY(),
-          xHat.getRotation().getRadians(),
-          groundTruthState.poseMeters.getX(),
-          groundTruthState.poseMeters.getY(),
-          groundTruthState.poseMeters.getRotation().getRadians());
-
       double error =
           groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
       if (error > maxError) {
@@ -227,4 +215,92 @@
       assertEquals(0.0, maxError, 0.2, "Incorrect max error");
     }
   }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements appled at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics = new DifferentialDriveKinematics(1);
+
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            0,
+            0,
+            new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+            VecBuilder.fill(0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    estimator.updateWithTime(0, new Rotation2d(), 0, 0);
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+          new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement.toString();
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsStaleVisionMeasurements() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            0,
+            0,
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(time, new Rotation2d(), 0, 0);
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index 02d2d52..d844c5d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java

@@ -151,8 +151,6 @@
 
     double t = 0.0;
 
-    System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
-
     final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
 
     double maxError = Double.NEGATIVE_INFINITY;
@@ -200,16 +198,6 @@
                   .minus(trajectory.getInitialPose().getRotation()),
               wheelPositions);
 
-      System.out.printf(
-          "%f, %f, %f, %f, %f, %f, %f\n",
-          t,
-          xHat.getX(),
-          xHat.getY(),
-          xHat.getRotation().getRadians(),
-          groundTruthState.poseMeters.getX(),
-          groundTruthState.poseMeters.getY(),
-          groundTruthState.poseMeters.getRotation().getRadians());
-
       double error =
           groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
       if (error > maxError) {
@@ -236,4 +224,100 @@
       assertEquals(0.0, maxError, 0.2, "Incorrect max error");
     }
   }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements appled at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            wheelPositions,
+            new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.1));
+
+    estimator.updateWithTime(0, new Rotation2d(), wheelPositions);
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+          new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement.toString();
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsOldVisionMeasurements() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            new MecanumDriveWheelPositions(),
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(time, new Rotation2d(), new MecanumDriveWheelPositions());
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index fde2c39..9fcd852 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java

@@ -165,11 +165,6 @@
 
     double t = 0.0;
 
-    System.out.print(
-        "time, est_x, est_y, est_theta, true_x, true_y, true_theta, "
-            + "distance_1, distance_2, distance_3, distance_4, "
-            + "angle_1, angle_2, angle_3, angle_4\n");
-
     final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
 
     double maxError = Double.NEGATIVE_INFINITY;
@@ -219,24 +214,6 @@
                   .minus(trajectory.getInitialPose().getRotation()),
               positions);
 
-      System.out.printf(
-          "%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
-          t,
-          xHat.getX(),
-          xHat.getY(),
-          xHat.getRotation().getRadians(),
-          groundTruthState.poseMeters.getX(),
-          groundTruthState.poseMeters.getY(),
-          groundTruthState.poseMeters.getRotation().getRadians(),
-          positions[0].distanceMeters,
-          positions[1].distanceMeters,
-          positions[2].distanceMeters,
-          positions[3].distanceMeters,
-          positions[0].angle.getRadians(),
-          positions[1].angle.getRadians(),
-          positions[2].angle.getRadians(),
-          positions[3].angle.getRadians());
-
       double error =
           groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
       if (error > maxError) {
@@ -263,4 +240,118 @@
       assertEquals(0.0, maxError, 0.2, "Incorrect max error");
     }
   }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements appled at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    estimator.updateWithTime(0, new Rotation2d(), new SwerveModulePosition[] {fl, fr, bl, br});
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+          new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement.toString();
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsOldVisionMeasurements() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(
+          time,
+          new Rotation2d(),
+          new SwerveModulePosition[] {
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition()
+          });
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
index 68babdd..4dc1826 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java

@@ -38,18 +38,12 @@
     assertEquals(
         transformTo.getTranslation(),
         CoordinateSystem.convert(transformFrom.getTranslation(), coordFrom, coordTo));
-    assertEquals(
-        transformTo.getRotation(),
-        CoordinateSystem.convert(transformFrom.getRotation(), coordFrom, coordTo));
     assertEquals(transformTo, CoordinateSystem.convert(transformFrom, coordFrom, coordTo));
 
     // "to" to "from"
     assertEquals(
         transformFrom.getTranslation(),
         CoordinateSystem.convert(transformTo.getTranslation(), coordTo, coordFrom));
-    assertEquals(
-        transformFrom.getRotation(),
-        CoordinateSystem.convert(transformTo.getRotation(), coordTo, coordFrom));
     assertEquals(transformFrom, CoordinateSystem.convert(transformTo, coordTo, coordFrom));
   }
 
@@ -158,9 +152,7 @@
     // No rotation from EDN to NWU
     checkTransform3dConvert(
         new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
-        new Transform3d(
-            new Translation3d(3.0, -1.0, -2.0),
-            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+        new Transform3d(new Translation3d(3.0, -1.0, -2.0), new Rotation3d()),
         CoordinateSystem.EDN(),
         CoordinateSystem.NWU());
 
@@ -171,7 +163,7 @@
             new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
         new Transform3d(
             new Translation3d(3.0, -1.0, -2.0),
-            new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+            new Rotation3d(0.0, Units.degreesToRadians(-45.0), 0.0)),
         CoordinateSystem.EDN(),
         CoordinateSystem.NWU());
 
@@ -182,7 +174,7 @@
             new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
         new Transform3d(
             new Translation3d(3.0, -1.0, -2.0),
-            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(-45.0))),
         CoordinateSystem.EDN(),
         CoordinateSystem.NWU());
 
@@ -193,10 +185,7 @@
             new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
         new Transform3d(
             new Translation3d(3.0, -1.0, -2.0),
-            new Rotation3d(
-                Units.degreesToRadians(-90.0),
-                Units.degreesToRadians(45.0),
-                Units.degreesToRadians(-90.0))),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
         CoordinateSystem.EDN(),
         CoordinateSystem.NWU());
   }
@@ -206,9 +195,7 @@
     // No rotation from EDN to NED
     checkTransform3dConvert(
         new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
-        new Transform3d(
-            new Translation3d(3.0, 1.0, 2.0),
-            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+        new Transform3d(new Translation3d(3.0, 1.0, 2.0), new Rotation3d()),
         CoordinateSystem.EDN(),
         CoordinateSystem.NED());
 
@@ -219,7 +206,7 @@
             new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
         new Transform3d(
             new Translation3d(3.0, 1.0, 2.0),
-            new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+            new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
         CoordinateSystem.EDN(),
         CoordinateSystem.NED());
 
@@ -230,7 +217,7 @@
             new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
         new Transform3d(
             new Translation3d(3.0, 1.0, 2.0),
-            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
         CoordinateSystem.EDN(),
         CoordinateSystem.NED());
 
@@ -241,10 +228,7 @@
             new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
         new Transform3d(
             new Translation3d(3.0, 1.0, 2.0),
-            new Rotation3d(
-                Units.degreesToRadians(90.0),
-                Units.degreesToRadians(-45.0),
-                Units.degreesToRadians(90.0))),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
         CoordinateSystem.EDN(),
         CoordinateSystem.NED());
   }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
index 780c816..d57362d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java

@@ -8,12 +8,35 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class Pose2dTest {
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testRotateBy() {
+    final double x = 1.0;
+    final double y = 2.0;
+    var initial = new Pose2d(new Translation2d(x, y), Rotation2d.fromDegrees(45.0));
+
+    var rotation = Rotation2d.fromDegrees(5.0);
+    var rotated = initial.rotateBy(rotation);
+
+    // Translation is rotated by CCW rotation matrix
+    double c = rotation.getCos();
+    double s = rotation.getSin();
+    assertAll(
+        () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+        () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getDegrees() + rotation.getDegrees(),
+                rotated.getRotation().getDegrees(),
+                kEpsilon));
+  }
+
+  @Test
   void testTransformBy() {
     var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
     var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
@@ -65,4 +88,50 @@
         () -> assertEquals(0.0, transform.getY(), kEpsilon),
         () -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon));
   }
+
+  @Test
+  void testNearest() {
+    var origin = new Pose2d();
+
+    // Distance sort
+    // each poseX is X units away from the origin at a random angle.
+    final var pose1 =
+        new Pose2d(new Translation2d(1, Rotation2d.fromDegrees(45)), new Rotation2d());
+    final var pose2 =
+        new Pose2d(new Translation2d(2, Rotation2d.fromDegrees(90)), new Rotation2d());
+    final var pose3 =
+        new Pose2d(new Translation2d(3, Rotation2d.fromDegrees(135)), new Rotation2d());
+    final var pose4 =
+        new Pose2d(new Translation2d(4, Rotation2d.fromDegrees(180)), new Rotation2d());
+    final var pose5 =
+        new Pose2d(new Translation2d(5, Rotation2d.fromDegrees(270)), new Rotation2d());
+
+    assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4)));
+    assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3)));
+    assertEquals(pose2, origin.nearest(List.of(pose4, pose2, pose3)));
+
+    // Rotation component sort (when distance is the same)
+    // Use the same translation because using different angles at the same distance can cause
+    // rounding error.
+    final var translation = new Translation2d(1, new Rotation2d());
+
+    final var poseA = new Pose2d(translation, Rotation2d.fromDegrees(0));
+    final var poseB = new Pose2d(translation, Rotation2d.fromDegrees(30));
+    final var poseC = new Pose2d(translation, Rotation2d.fromDegrees(120));
+    final var poseD = new Pose2d(translation, Rotation2d.fromDegrees(90));
+    final var poseE = new Pose2d(translation, Rotation2d.fromDegrees(-180));
+
+    assertEquals(
+        poseA, new Pose2d(0, 0, Rotation2d.fromDegrees(360)).nearest(List.of(poseA, poseB, poseD)));
+    assertEquals(
+        poseB,
+        new Pose2d(0, 0, Rotation2d.fromDegrees(-335)).nearest(List.of(poseB, poseC, poseD)));
+    assertEquals(
+        poseC,
+        new Pose2d(0, 0, Rotation2d.fromDegrees(-120)).nearest(List.of(poseB, poseC, poseD)));
+    assertEquals(
+        poseD, new Pose2d(0, 0, Rotation2d.fromDegrees(85)).nearest(List.of(poseA, poseC, poseD)));
+    assertEquals(
+        poseE, new Pose2d(0, 0, Rotation2d.fromDegrees(170)).nearest(List.of(poseA, poseD, poseE)));
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
index f13819f..ee1d98c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java

@@ -6,16 +6,50 @@
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.util.Units;
+import java.util.Arrays;
 import org.junit.jupiter.api.Test;
 
 class Pose3dTest {
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testRotateBy() {
+    final double x = 1.0;
+    final double y = 2.0;
+    var initial =
+        new Pose3d(
+            new Translation3d(x, y, 0.0),
+            new Rotation3d(
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(45.0)));
+
+    double yaw = Units.degreesToRadians(5.0);
+    var rotation = new Rotation3d(Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), yaw);
+    var rotated = initial.rotateBy(rotation);
+
+    // Translation is rotated by CCW rotation matrix
+    double c = Math.cos(yaw);
+    double s = Math.sin(yaw);
+    assertAll(
+        () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+        () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getZ(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getRotation().getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getZ() + rotation.getZ(),
+                rotated.getRotation().getZ(),
+                kEpsilon));
+  }
+
+  @Test
   void testTransformByRotations() {
     var initialPose =
         new Pose3d(
@@ -49,10 +83,13 @@
                 Units.degreesToRadians(-45.0),
                 Units.degreesToRadians(0.0)));
 
-    // This sequence of rotations should diverge from the origin and eventually return to it. When
-    // each rotation occurs, it should be performed intrinsicly, i.e. 'from the viewpoint' of and
+    // This sequence of rotations should diverge from the origin and eventually
+    // return to it. When
+    // each rotation occurs, it should be performed intrinsicly, i.e. 'from the
+    // viewpoint' of and
     // with
-    // the axes of the pose that is being transformed, just like how the translation is done 'from
+    // the axes of the pose that is being transformed, just like how the translation
+    // is done 'from
     // the
     // viewpoint' of the pose that is being transformed.
     var finalPose =
@@ -153,4 +190,115 @@
 
     assertEquals(expected, pose.toPose2d());
   }
+
+  @Test
+  void testComplexTwists() {
+    var initial_poses =
+        Arrays.asList(
+            new Pose3d(
+                new Translation3d(0.698303, -0.959096, 0.271076),
+                new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
+            new Pose3d(
+                new Translation3d(0.634892, -0.765209, 0.117543),
+                new Rotation3d(new Quaternion(0.84987, -0.070829, 0.162097, 0.496415))),
+            new Pose3d(
+                new Translation3d(0.584827, -0.590303, -0.02557),
+                new Rotation3d(new Quaternion(0.832743, -0.041991, 0.202188, 0.513708))),
+            new Pose3d(
+                new Translation3d(0.505038, -0.451479, -0.112835),
+                new Rotation3d(new Quaternion(0.816515, -0.002673, 0.226182, 0.531166))),
+            new Pose3d(
+                new Translation3d(0.428178, -0.329692, -0.189707),
+                new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
+
+    var final_poses =
+        Arrays.asList(
+            new Pose3d(
+                new Translation3d(-0.230448, -0.511957, 0.198406),
+                new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
+            new Pose3d(
+                new Translation3d(-0.088932, -0.343253, 0.095018),
+                new Rotation3d(new Quaternion(0.638738, 0.413016, 0.536281, 0.365833))),
+            new Pose3d(
+                new Translation3d(-0.107908, -0.317552, 0.133946),
+                new Rotation3d(new Quaternion(0.653444, 0.417069, 0.465505, 0.427046))),
+            new Pose3d(
+                new Translation3d(-0.123383, -0.156411, -0.047435),
+                new Rotation3d(new Quaternion(0.652983, 0.40644, 0.431566, 0.47135))),
+            new Pose3d(
+                new Translation3d(-0.084654, -0.019305, -0.030022),
+                new Rotation3d(new Quaternion(0.620243, 0.429104, 0.479384, 0.44873))));
+
+    final var eps = 1E-5;
+    for (int i = 0; i < initial_poses.size(); i++) {
+      var start = initial_poses.get(i);
+      var end = final_poses.get(i);
+
+      var twist = start.log(end);
+      var start_exp = start.exp(twist);
+
+      assertAll(
+          () -> assertEquals(start_exp.getX(), end.getX(), eps),
+          () -> assertEquals(start_exp.getY(), end.getY(), eps),
+          () -> assertEquals(start_exp.getZ(), end.getZ(), eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getW(),
+                  end.getRotation().getQuaternion().getW(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getX(),
+                  end.getRotation().getQuaternion().getX(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getY(),
+                  end.getRotation().getQuaternion().getY(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getZ(),
+                  end.getRotation().getQuaternion().getZ(),
+                  eps));
+    }
+  }
+
+  @Test
+  void testTwistNaN() {
+    var initial_poses =
+        Arrays.asList(
+            new Pose3d(
+                new Translation3d(6.32, 4.12, 0.00),
+                new Rotation3d(
+                    new Quaternion(-0.9999999999999999, 0.0, 0.0, 1.9208309264993548E-8))),
+            new Pose3d(
+                new Translation3d(3.75, 2.95, 0.00),
+                new Rotation3d(
+                    new Quaternion(0.9999999999999793, 0.0, 0.0, 2.0352360299846772E-7))));
+    var final_poses =
+        Arrays.asList(
+            new Pose3d(
+                new Translation3d(6.33, 4.15, 0.00),
+                new Rotation3d(
+                    new Quaternion(-0.9999999999999999, 0.0, 0.0, 2.416890209039172E-8))),
+            new Pose3d(
+                new Translation3d(3.66, 2.93, 0.00),
+                new Rotation3d(
+                    new Quaternion(0.9999999999999782, 0.0, 0.0, 2.0859477994905617E-7))));
+
+    for (int i = 0; i < initial_poses.size(); i++) {
+      var start = initial_poses.get(i);
+      var end = final_poses.get(i);
+
+      var twist = start.log(end);
+      assertAll(
+          () -> assertFalse(((Double) twist.dx).isNaN()),
+          () -> assertFalse(((Double) twist.dy).isNaN()),
+          () -> assertFalse(((Double) twist.dz).isNaN()),
+          () -> assertFalse(((Double) twist.rx).isNaN()),
+          () -> assertFalse(((Double) twist.ry).isNaN()),
+          () -> assertFalse(((Double) twist.rz).isNaN()));
+    }
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
index 7c7e103..458b14d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java

@@ -4,7 +4,9 @@
 
 package edu.wpi.first.math.geometry;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
 import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Test;
@@ -14,37 +16,91 @@
   void testInit() {
     // Identity
     var q1 = new Quaternion();
-    assertEquals(1.0, q1.getW());
-    assertEquals(0.0, q1.getX());
-    assertEquals(0.0, q1.getY());
-    assertEquals(0.0, q1.getZ());
+    assertAll(
+        () -> assertEquals(1.0, q1.getW()),
+        () -> assertEquals(0.0, q1.getX()),
+        () -> assertEquals(0.0, q1.getY()),
+        () -> assertEquals(0.0, q1.getZ()));
 
     // Normalized
     var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5);
-    assertEquals(0.5, q2.getW());
-    assertEquals(0.5, q2.getX());
-    assertEquals(0.5, q2.getY());
-    assertEquals(0.5, q2.getZ());
+    assertAll(
+        () -> assertEquals(0.5, q2.getW()),
+        () -> assertEquals(0.5, q2.getX()),
+        () -> assertEquals(0.5, q2.getY()),
+        () -> assertEquals(0.5, q2.getZ()));
 
     // Unnormalized
     var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5);
-    assertEquals(0.75, q3.getW());
-    assertEquals(0.3, q3.getX());
-    assertEquals(0.4, q3.getY());
-    assertEquals(0.5, q3.getZ());
+    assertAll(
+        () -> assertEquals(0.75, q3.getW()),
+        () -> assertEquals(0.3, q3.getX()),
+        () -> assertEquals(0.4, q3.getY()),
+        () -> assertEquals(0.5, q3.getZ()));
 
-    q3 = q3.normalize();
+    var q3_norm = q3.normalize();
     double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
-    assertEquals(0.75 / norm, q3.getW());
-    assertEquals(0.3 / norm, q3.getX());
-    assertEquals(0.4 / norm, q3.getY());
-    assertEquals(0.5 / norm, q3.getZ());
-    assertEquals(
-        1.0,
-        q3.getW() * q3.getW()
-            + q3.getX() * q3.getX()
-            + q3.getY() * q3.getY()
-            + q3.getZ() * q3.getZ());
+    assertAll(
+        () -> assertEquals(0.75 / norm, q3_norm.getW()),
+        () -> assertEquals(0.3 / norm, q3_norm.getX()),
+        () -> assertEquals(0.4 / norm, q3_norm.getY()),
+        () -> assertEquals(0.5 / norm, q3_norm.getZ()),
+        () -> assertEquals(1.0, q3_norm.dot(q3_norm)));
+  }
+
+  @Test
+  void testAddition() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
+
+    var sum = q.plus(p);
+    assertAll(
+        () -> assertEquals(q.getW() + p.getW(), sum.getW()),
+        () -> assertEquals(q.getX() + p.getX(), sum.getX()),
+        () -> assertEquals(q.getY() + p.getY(), sum.getY()),
+        () -> assertEquals(q.getZ() + p.getZ(), sum.getZ()));
+  }
+
+  @Test
+  void testSubtraction() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
+
+    var difference = q.minus(p);
+
+    assertAll(
+        () -> assertEquals(q.getW() - p.getW(), difference.getW()),
+        () -> assertEquals(q.getX() - p.getX(), difference.getX()),
+        () -> assertEquals(q.getY() - p.getY(), difference.getY()),
+        () -> assertEquals(q.getZ() - p.getZ(), difference.getZ()));
+  }
+
+  @Test
+  void testScalarMultiplication() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var scalar = 2;
+
+    var product = q.times(scalar);
+
+    assertAll(
+        () -> assertEquals(q.getW() * scalar, product.getW()),
+        () -> assertEquals(q.getX() * scalar, product.getX()),
+        () -> assertEquals(q.getY() * scalar, product.getY()),
+        () -> assertEquals(q.getZ() * scalar, product.getZ()));
+  }
+
+  @Test
+  void testScalarDivision() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var scalar = 2;
+
+    var product = q.divide(scalar);
+
+    assertAll(
+        () -> assertEquals(q.getW() / scalar, product.getW()),
+        () -> assertEquals(q.getX() / scalar, product.getX()),
+        () -> assertEquals(q.getY() / scalar, product.getY()),
+        () -> assertEquals(q.getZ() / scalar, product.getZ()));
   }
 
   @Test
@@ -59,31 +115,131 @@
     // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
     // produce a 90° CCW Y rotation
     var expected = yRot;
-    var actual = zRot.times(yRot).times(xRot);
-    assertEquals(expected.getW(), actual.getW(), 1e-9);
-    assertEquals(expected.getX(), actual.getX(), 1e-9);
-    assertEquals(expected.getY(), actual.getY(), 1e-9);
-    assertEquals(expected.getZ(), actual.getZ(), 1e-9);
+    final var actual = zRot.times(yRot).times(xRot);
+    assertAll(
+        () -> assertEquals(expected.getW(), actual.getW(), 1e-9),
+        () -> assertEquals(expected.getX(), actual.getX(), 1e-9),
+        () -> assertEquals(expected.getY(), actual.getY(), 1e-9),
+        () -> assertEquals(expected.getZ(), actual.getZ(), 1e-9));
 
     // Identity
     var q =
         new Quaternion(
             0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594);
-    actual = q.times(q.inverse());
-    assertEquals(1.0, actual.getW());
-    assertEquals(0.0, actual.getX());
-    assertEquals(0.0, actual.getY());
-    assertEquals(0.0, actual.getZ());
+    final var actual2 = q.times(q.inverse());
+    assertAll(
+        () -> assertEquals(1.0, actual2.getW()),
+        () -> assertEquals(0.0, actual2.getX()),
+        () -> assertEquals(0.0, actual2.getY()),
+        () -> assertEquals(0.0, actual2.getZ()));
+  }
+
+  @Test
+  void testConjugate() {
+    var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
+    var inv = q.conjugate();
+
+    assertAll(
+        () -> assertEquals(q.getW(), inv.getW()),
+        () -> assertEquals(-q.getX(), inv.getX()),
+        () -> assertEquals(-q.getY(), inv.getY()),
+        () -> assertEquals(-q.getZ(), inv.getZ()));
   }
 
   @Test
   void testInverse() {
     var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
     var inv = q.inverse();
+    var norm = q.norm();
 
-    assertEquals(q.getW(), inv.getW());
-    assertEquals(-q.getX(), inv.getX());
-    assertEquals(-q.getY(), inv.getY());
-    assertEquals(-q.getZ(), inv.getZ());
+    assertAll(
+        () -> assertEquals(q.getW() / (norm * norm), inv.getW(), 1e-10),
+        () -> assertEquals(-q.getX() / (norm * norm), inv.getX(), 1e-10),
+        () -> assertEquals(-q.getY() / (norm * norm), inv.getY(), 1e-10),
+        () -> assertEquals(-q.getZ() / (norm * norm), inv.getZ(), 1e-10));
+  }
+
+  @Test
+  void testNorm() {
+    var q = new Quaternion(3, 4, 12, 84);
+
+    // pythagorean triples (3, 4, 5), (5, 12, 13), (13, 84, 85)
+    assertEquals(q.norm(), 85, 1e-10);
+  }
+
+  @Test
+  void testExponential() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_exp =
+        new Quaternion(
+            2.81211398529184, -0.392521193481878, -0.588781790222817, -0.785042386963756);
+
+    assertEquals(q_exp, q.exp());
+  }
+
+  @Test
+  void testLogarithm() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_log =
+        new Quaternion(1.7959088706354, 0.515190292664085, 0.772785438996128, 1.03038058532817);
+
+    assertEquals(q_log, q.log());
+
+    var zero = new Quaternion(0, 0, 0, 0);
+    var one = new Quaternion();
+
+    assertEquals(zero, one.log());
+
+    var i = new Quaternion(0, 1, 0, 0);
+    assertEquals(i.times(Math.PI / 2), i.log());
+
+    var j = new Quaternion(0, 0, 1, 0);
+    assertEquals(j.times(Math.PI / 2), j.log());
+
+    var k = new Quaternion(0, 0, 0, 1);
+    assertEquals(k.times(Math.PI / 2), k.log());
+    assertEquals(i.times(-Math.PI), one.times(-1).log());
+
+    var ln_half = Math.log(0.5);
+    assertEquals(new Quaternion(ln_half, -Math.PI, 0, 0), one.times(-0.5).log());
+  }
+
+  @Test
+  void testLogarithmIsInverseOfExponential() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+
+    // These operations are order-dependent: ln(exp(q)) is congruent
+    // but not necessarily equal to exp(ln(q)) due to the multi-valued nature of the complex
+    // logarithm.
+
+    var q_log_exp = q.log().exp();
+
+    assertEquals(q, q_log_exp);
+
+    var start = new Quaternion(1, 2, 3, 4);
+    var expect = new Quaternion(5, 6, 7, 8);
+
+    var twist = start.log(expect);
+    var actual = start.exp(twist);
+
+    assertEquals(expect, actual);
+  }
+
+  @Test
+  void testDotProduct() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var p = new Quaternion(5.5, 6.6, 7.7, 8.8);
+
+    assertEquals(
+        q.getW() * p.getW() + q.getX() * p.getX() + q.getY() * p.getY() + q.getZ() * p.getZ(),
+        q.dot(p));
+  }
+
+  @Test
+  void testDotProductAsEquality() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_conj = q.conjugate();
+
+    assertAll(() -> assertEquals(q, q), () -> assertNotEquals(q, q_conj));
   }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
index 072a2e6..d80344d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java

@@ -20,21 +20,60 @@
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testGimbalLockAccuracy() {
+    var rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    var rot2 = new Rotation3d(Math.PI, 0, 0);
+    var rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
+    final var result1 = rot1.plus(rot2).plus(rot3);
+    final var expected1 = new Rotation3d(0, -Math.PI / 2, Math.PI / 2);
+    assertAll(
+        () -> assertEquals(expected1, result1),
+        () -> assertEquals(Math.PI / 2, result1.getX() + result1.getZ(), kEpsilon),
+        () -> assertEquals(-Math.PI / 2, result1.getY(), kEpsilon));
+
+    rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    rot2 = new Rotation3d(-Math.PI, 0, 0);
+    rot3 = new Rotation3d(Math.PI / 2, 0, 0);
+    final var result2 = rot1.plus(rot2).plus(rot3);
+    final var expected2 = new Rotation3d(0, Math.PI / 2, Math.PI / 2);
+    assertAll(
+        () -> assertEquals(expected2, result2),
+        () -> assertEquals(Math.PI / 2, result2.getZ() - result2.getX(), kEpsilon),
+        () -> assertEquals(Math.PI / 2, result2.getY(), kEpsilon));
+
+    rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    rot2 = new Rotation3d(0, Math.PI / 3, 0);
+    rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
+    final var result3 = rot1.plus(rot2).plus(rot3);
+    final var expected3 = new Rotation3d(0, Math.PI / 2, Math.PI / 6);
+    assertAll(
+        () -> assertEquals(expected3, result3),
+        () -> assertEquals(Math.PI / 6, result3.getZ() - result3.getX(), kEpsilon),
+        () -> assertEquals(Math.PI / 2, result3.getY(), kEpsilon));
+  }
+
+  @Test
   void testInitAxisAngleAndRollPitchYaw() {
     final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
     final var rot1 = new Rotation3d(xAxis, Math.PI / 3);
     final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0);
+    final var rvec1 = new Rotation3d(xAxis.times(Math.PI / 3));
     assertEquals(rot1, rot2);
+    assertEquals(rot1, rvec1);
 
     final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
     final var rot3 = new Rotation3d(yAxis, Math.PI / 3);
     final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0);
+    final var rvec2 = new Rotation3d(yAxis.times(Math.PI / 3));
     assertEquals(rot3, rot4);
+    assertEquals(rot3, rvec2);
 
     final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
     final var rot5 = new Rotation3d(zAxis, Math.PI / 3);
     final var rot6 = new Rotation3d(0.0, 0.0, Math.PI / 3);
+    final var rvec3 = new Rotation3d(zAxis.times(Math.PI / 3));
     assertEquals(rot5, rot6);
+    assertEquals(rot5, rvec3);
   }
 
   @Test

diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
index c8f5690..6ed8a61 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java

@@ -8,6 +8,7 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class Translation2dTest {
@@ -114,4 +115,20 @@
         () -> assertEquals(1.0, two.getX(), kEpsilon),
         () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon));
   }
+
+  @Test
+  void testNearest() {
+    var origin = new Translation2d();
+
+    // each translationX is X units away from the origin at a random angle.
+    var translation1 = new Translation2d(1, Rotation2d.fromDegrees(45));
+    var translation2 = new Translation2d(2, Rotation2d.fromDegrees(90));
+    var translation3 = new Translation2d(3, Rotation2d.fromDegrees(135));
+    var translation4 = new Translation2d(4, Rotation2d.fromDegrees(180));
+    var translation5 = new Translation2d(5, Rotation2d.fromDegrees(270));
+
+    assertEquals(origin.nearest(List.of(translation5, translation3, translation4)), translation3);
+    assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);
+    assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2);
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java
new file mode 100644
index 0000000..fb74b92
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java

@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingDoubleTreeMapTest {
+  @Test
+  void testInterpolationDouble() {
+    InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+  }
+
+  @Test
+  void testInterpolationClear() {
+    InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java
new file mode 100644
index 0000000..b04b40a
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingTreeMapTest {
+  @Test
+  void testInterpolation() {
+    InterpolatingTreeMap<Double, Double> table =
+        new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Interpolator.forDouble());
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
index b9c3785..2401342 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java

@@ -7,14 +7,45 @@
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import org.junit.jupiter.api.Test;
 
 class ChassisSpeedsTest {
   private static final double kEpsilon = 1E-9;
 
   @Test
-  void testFieldRelativeConstruction() {
+  void testDiscretize() {
+    final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
+    final var duration = 1.0;
+    final var dt = 0.01;
+
+    final var speeds = ChassisSpeeds.discretize(target, duration);
+    final var twist =
+        new Twist2d(
+            speeds.vxMetersPerSecond * dt,
+            speeds.vyMetersPerSecond * dt,
+            speeds.omegaRadiansPerSecond * dt);
+
+    var pose = new Pose2d();
+    for (double time = 0; time < duration; time += dt) {
+      pose = pose.exp(twist);
+    }
+
+    final var result = pose; // For lambda capture
+    assertAll(
+        () -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon),
+        () -> assertEquals(target.vyMetersPerSecond * duration, result.getY(), kEpsilon),
+        () ->
+            assertEquals(
+                target.omegaRadiansPerSecond * duration,
+                result.getRotation().getRadians(),
+                kEpsilon));
+  }
+
+  @Test
+  void testFromFieldRelativeSpeeds() {
     final var chassisSpeeds =
         ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0));
 
@@ -23,4 +54,71 @@
         () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
         () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
   }
+
+  @Test
+  void testFromRobotRelativeSpeeds() {
+    final var chassisSpeeds =
+        ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));
+
+    assertAll(
+        () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testPlus() {
+    final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+    final var right = new ChassisSpeeds(2.0, 1.5, 0.25);
+
+    final var chassisSpeeds = left.plus(right);
+
+    assertAll(
+        () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(2.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(1.0, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testMinus() {
+    final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+    final var right = new ChassisSpeeds(2.0, 0.5, 0.25);
+
+    final var chassisSpeeds = left.minus(right);
+
+    assertAll(
+        () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testUnaryMinus() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus();
+
+    assertAll(
+        () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(-0.5, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(-0.75, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testMultiplication() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0);
+
+    assertAll(
+        () -> assertEquals(2.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testDivision() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0);
+
+    assertAll(
+        () -> assertEquals(0.5, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(0.25, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(0.375, chassisSpeeds.omegaRadiansPerSecond));
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java
new file mode 100644
index 0000000..7f26180
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelSpeedsTest {
+  @Test
+  void testPlus() {
+    final var left = new DifferentialDriveWheelSpeeds(1.0, 0.5);
+    final var right = new DifferentialDriveWheelSpeeds(2.0, 1.5);
+
+    final var wheelSpeeds = left.plus(right);
+
+    assertAll(
+        () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(2.0, wheelSpeeds.rightMetersPerSecond));
+  }
+
+  @Test
+  void testMinus() {
+    final var left = new DifferentialDriveWheelSpeeds(1.0, 0.5);
+    final var right = new DifferentialDriveWheelSpeeds(2.0, 0.5);
+
+    final var wheelSpeeds = left.minus(right);
+
+    assertAll(
+        () -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond));
+  }
+
+  @Test
+  void testUnaryMinus() {
+    final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).unaryMinus();
+
+    assertAll(
+        () -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(-0.5, wheelSpeeds.rightMetersPerSecond));
+  }
+
+  @Test
+  void testMultiplication() {
+    final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).times(2.0);
+
+    assertAll(
+        () -> assertEquals(2.0, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(1.0, wheelSpeeds.rightMetersPerSecond));
+  }
+
+  @Test
+  void testDivision() {
+    final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).div(2.0);
+
+    assertAll(
+        () -> assertEquals(0.5, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(0.25, wheelSpeeds.rightMetersPerSecond));
+  }
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java
new file mode 100644
index 0000000..22efb44
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java

@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelSpeedsTest {
+  @Test
+  void testPlus() {
+    final var left = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5);
+    final var right = new MecanumDriveWheelSpeeds(2.0, 1.5, 0.5, 1.0);
+
+    final var wheelSpeeds = left.plus(right);
+
+    assertAll(
+        () -> assertEquals(3.0, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(2.0, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(2.5, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(2.5, wheelSpeeds.rearRightMetersPerSecond));
+  }
+
+  @Test
+  void testMinus() {
+    final var left = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5);
+    final var right = new MecanumDriveWheelSpeeds(2.0, 0.5, 0.5, 1.0);
+
+    final var wheelSpeeds = left.minus(right);
+
+    assertAll(
+        () -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(0.0, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(1.5, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(0.5, wheelSpeeds.rearRightMetersPerSecond));
+  }
+
+  @Test
+  void testUnaryMinus() {
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).unaryMinus();
+
+    assertAll(
+        () -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(-0.5, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(-2.0, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(-1.5, wheelSpeeds.rearRightMetersPerSecond));
+  }
+
+  @Test
+  void testMultiplication() {
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).times(2.0);
+
+    assertAll(
+        () -> assertEquals(2.0, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(1.0, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(4.0, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(3.0, wheelSpeeds.rearRightMetersPerSecond));
+  }
+
+  @Test
+  void testDivision() {
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).div(2.0);
+
+    assertAll(
+        () -> assertEquals(0.5, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(0.25, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(1.0, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(0.75, wheelSpeeds.rearRightMetersPerSecond));
+  }
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
index 43dd02a..f429a68 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java

@@ -120,6 +120,28 @@
   }
 
   @Test
+  void testResetWheelAngle() {
+    Rotation2d fl = new Rotation2d(0);
+    Rotation2d fr = new Rotation2d(Math.PI / 2);
+    Rotation2d bl = new Rotation2d(Math.PI);
+    Rotation2d br = new Rotation2d(3 * Math.PI / 2);
+    m_kinematics.resetHeadings(fl, fr, bl, br);
+    var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
+
+    // Robot is stationary, but module angles are preserved.
+
+    assertAll(
+        () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(270.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+  }
+
+  @Test
   void testTurnInPlaceInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
     var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
@@ -371,4 +393,21 @@
         () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
         () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
   }
+
+  @Test
+  void testDesaturateNegativeSpeed() {
+    SwerveModuleState fl = new SwerveModuleState(1, new Rotation2d());
+    SwerveModuleState fr = new SwerveModuleState(1, new Rotation2d());
+    SwerveModuleState bl = new SwerveModuleState(-2, new Rotation2d());
+    SwerveModuleState br = new SwerveModuleState(-2, new Rotation2d());
+
+    SwerveModuleState[] arr = {fl, fr, bl, br};
+    SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);
+
+    assertAll(
+        () -> assertEquals(0.5, arr[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.5, arr[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(-1.0, arr[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(-1.0, arr[3].speedMetersPerSecond, kEpsilon));
+  }
 }

diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
index 2182674..7f62933 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java

@@ -122,98 +122,6 @@
             + discQIntegrated);
   }
 
-  // Test that the Taylor series discretization produces nearly identical results.
-  @Test
-  void testDiscretizeSlowModelAQTaylor() {
-    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
-    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
-
-    final var dt = 1.0;
-
-    // Continuous Q should be positive semidefinite
-    final var esCont = contQ.getStorage().eig();
-    for (int i = 0; i < contQ.getNumRows(); ++i) {
-      assertTrue(esCont.getEigenvalue(i).real >= 0);
-    }
-
-    //       T
-    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-    //       0
-    final var discQIntegrated =
-        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
-            (Double t, Matrix<N2, N2> x) ->
-                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
-            0.0,
-            new Matrix<>(Nat.N2(), Nat.N2()),
-            dt);
-
-    var discA = Discretization.discretizeA(contA, dt);
-
-    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
-    var discATaylor = discAQPair.getFirst();
-    var discQTaylor = discAQPair.getSecond();
-
-    assertTrue(
-        discQIntegrated.minus(discQTaylor).normF() < 1e-10,
-        "Expected these to be nearly equal:\ndiscQTaylor:\n"
-            + discQTaylor
-            + "\ndiscQIntegrated:\n"
-            + discQIntegrated);
-    assertTrue(discA.minus(discATaylor).normF() < 1e-10);
-
-    // Discrete Q should be positive semidefinite
-    final var esDisc = discQTaylor.getStorage().eig();
-    for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
-      assertTrue(esDisc.getEigenvalue(i).real >= 0);
-    }
-  }
-
-  // Test that the Taylor series discretization produces nearly identical results.
-  @Test
-  void testDiscretizeFastModelAQTaylor() {
-    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500);
-    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
-
-    final var dt = 0.005;
-
-    // Continuous Q should be positive semidefinite
-    final var esCont = contQ.getStorage().eig();
-    for (int i = 0; i < contQ.getNumRows(); ++i) {
-      assertTrue(esCont.getEigenvalue(i).real >= 0);
-    }
-
-    //       T
-    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-    //       0
-    final var discQIntegrated =
-        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
-            (Double t, Matrix<N2, N2> x) ->
-                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
-            0.0,
-            new Matrix<>(Nat.N2(), Nat.N2()),
-            dt);
-
-    var discA = Discretization.discretizeA(contA, dt);
-
-    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
-    var discATaylor = discAQPair.getFirst();
-    var discQTaylor = discAQPair.getSecond();
-
-    assertTrue(
-        discQIntegrated.minus(discQTaylor).normF() < 1e-3,
-        "Expected these to be nearly equal:\ndiscQTaylor:\n"
-            + discQTaylor
-            + "\ndiscQIntegrated:\n"
-            + discQIntegrated);
-    assertTrue(discA.minus(discATaylor).normF() < 1e-10);
-
-    // Discrete Q should be positive semidefinite
-    final var esDisc = discQTaylor.getStorage().eig();
-    for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
-      assertTrue(esDisc.getEigenvalue(i).real >= 0);
-    }
-  }
-
   // Test that DiscretizeR() works
   @Test
   void testDiscretizeR() {

diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
index 72fb5ea..9b47148 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java

@@ -31,6 +31,20 @@
   }
 
   @Test
+  void testZeroRKDP() {
+    var y1 =
+        NumericalIntegration.rkdp(
+            (x, u) -> {
+              return VecBuilder.fill(0);
+            },
+            VecBuilder.fill(0),
+            VecBuilder.fill(0),
+            0.1);
+
+    assertEquals(0.0, y1.get(0, 0), 1e-3);
+  }
+
+  @Test
   void testExponentialRKDP() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 

diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
new file mode 100644
index 0000000..e9c535e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java

@@ -0,0 +1,364 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class ExponentialProfileTest {
+  private static final double kDt = 0.01;
+  private static final SimpleMotorFeedforward feedforward =
+      new SimpleMotorFeedforward(0, 2.5629, 0.43277);
+  private static final ExponentialProfile.Constraints constraints =
+      ExponentialProfile.Constraints.fromCharacteristics(12, 2.5629, 0.43277);
+
+  /**
+   * Asserts "val1" is within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertNear(double val1, double val2, double eps) {
+    assertTrue(
+        Math.abs(val1 - val2) <= eps,
+        "Difference between " + val1 + " and " + val2 + " is greater than " + eps);
+  }
+
+  private static void assertNear(
+      ExponentialProfile.State val1, ExponentialProfile.State val2, double eps) {
+    assertAll(
+        () -> assertNear(val1.position, val2.position, eps),
+        () -> assertNear(val1.position, val2.position, eps));
+  }
+
+  private static ExponentialProfile.State checkDynamics(
+      ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
+    var next = profile.calculate(kDt, current, goal);
+
+    var signal = feedforward.calculate(current.velocity, next.velocity, kDt);
+
+    assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);
+
+    return next;
+  }
+
+  @Test
+  void reachesGoal() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    for (int i = 0; i < 450; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Tests that decreasing the maximum velocity in the middle when it is already
+  // moving faster than the new max is handled correctly
+  @Test
+  void posContinuousUnderVelChange() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    for (int i = 0; i < 300; ++i) {
+      if (i == 150) {
+        profile =
+            new ExponentialProfile(
+                ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B));
+      }
+
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Tests that decreasing the maximum velocity in the middle when it is already
+  // moving faster than the new max is handled correctly
+  @Test
+  void posContinuousUnderVelChangeBackward() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    for (int i = 0; i < 300; ++i) {
+      if (i == 150) {
+        profile =
+            new ExponentialProfile(
+                ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B));
+      }
+
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  // There is some somewhat tricky code for dealing with going backwards
+  @Test
+  void backwards() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    for (int i = 0; i < 400; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void switchGoalInMiddle() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    for (int i = 0; i < 50; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+    assertNotEquals(state, goal);
+
+    goal = new ExponentialProfile.State(0.0, 0.0);
+    for (int i = 0; i < 100; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Checks to make sure that it hits top speed
+  @Test
+  void topSpeed() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    double maxSpeed = 0;
+    for (int i = 0; i < 900; ++i) {
+      state = checkDynamics(profile, state, goal);
+      maxSpeed = Math.max(maxSpeed, state.velocity);
+    }
+
+    assertNear(constraints.maxVelocity(), maxSpeed, 10e-5);
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void topSpeedBackward() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    double maxSpeed = 0;
+    for (int i = 0; i < 900; ++i) {
+      state = checkDynamics(profile, state, goal);
+      maxSpeed = Math.min(maxSpeed, state.velocity);
+    }
+
+    assertNear(-constraints.maxVelocity(), maxSpeed, 10e-5);
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void largeInitialVelocity() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 8);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    for (int i = 0; i < 900; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void largeNegativeInitialVelocity() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, -8);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    for (int i = 0; i < 900; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+
+    assertEquals(state, goal);
+  }
+
+  @SuppressWarnings("PMD.TestClassWithoutTestCases")
+  static class TestCase {
+    public final ExponentialProfile.State initial;
+    public final ExponentialProfile.State goal;
+    public final ExponentialProfile.State inflectionPoint;
+
+    TestCase(
+        ExponentialProfile.State initial,
+        ExponentialProfile.State goal,
+        ExponentialProfile.State inflectionPoint) {
+      this.initial = initial;
+      this.goal = goal;
+      this.inflectionPoint = inflectionPoint;
+    }
+  }
+
+  @Test
+  void testHeuristic() {
+    List<TestCase> testCases =
+        List.of(
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(0.75, -4),
+                new ExponentialProfile.State(1.3758, 4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(1.4103, 4),
+                new ExponentialProfile.State(1.3758, 4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(0.75, -4),
+                new ExponentialProfile.State(1.3758, 4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(1.4103, 4),
+                new ExponentialProfile.State(1.3758, 4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(0.5, -2),
+                new ExponentialProfile.State(0.4367, 3.7217)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(0.546, 2),
+                new ExponentialProfile.State(0.4367, 3.7217)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(0.5, -2),
+                new ExponentialProfile.State(0.5560, -2.9616)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(0.546, 2),
+                new ExponentialProfile.State(0.5560, -2.9616)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(-0.75, -4),
+                new ExponentialProfile.State(-0.7156, -4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(-0.0897, 4),
+                new ExponentialProfile.State(-0.7156, -4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(-0.75, -4),
+                new ExponentialProfile.State(-0.7156, -4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(-0.0897, 4),
+                new ExponentialProfile.State(-0.7156, -4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(-0.5, -4.5),
+                new ExponentialProfile.State(1.095, 4.314)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(1.0795, 4.5),
+                new ExponentialProfile.State(-0.5122, -4.351)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(-0.5, -4.5),
+                new ExponentialProfile.State(1.095, 4.314)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(1.0795, 4.5),
+                new ExponentialProfile.State(-0.5122, -4.351)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -8),
+                new ExponentialProfile.State(0, 0),
+                new ExponentialProfile.State(-0.1384, 3.342)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -8),
+                new ExponentialProfile.State(-1, 0),
+                new ExponentialProfile.State(-0.562, -6.792)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, 8),
+                new ExponentialProfile.State(1, 0),
+                new ExponentialProfile.State(0.562, 6.792)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, 8),
+                new ExponentialProfile.State(-1, 0),
+                new ExponentialProfile.State(-0.785, -4.346)));
+
+    var profile = new ExponentialProfile(constraints);
+
+    for (var testCase : testCases) {
+      var state = profile.calculateInflectionPoint(testCase.initial, testCase.goal);
+      assertNear(testCase.inflectionPoint, state, 1e-3);
+    }
+  }
+
+  @Test
+  void timingToCurrent() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(2, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    for (int i = 0; i < 400; i++) {
+      state = checkDynamics(profile, state, goal);
+      assertNear(profile.timeLeftUntil(state, state), 0, 2e-2);
+    }
+  }
+
+  @Test
+  void timingToGoal() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(2, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    double predictedTimeLeft = profile.timeLeftUntil(state, goal);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      state = checkDynamics(profile, state, goal);
+
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingToNegativeGoal() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(-2, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    double predictedTimeLeft = profile.timeLeftUntil(state, goal);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      state = checkDynamics(profile, state, goal);
+
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+}

diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
index ee6cc8f..fb28b69 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java

@@ -57,9 +57,9 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 450; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertEquals(state, goal);
   }
@@ -67,21 +67,21 @@
   // Tests that decreasing the maximum velocity in the middle when it is already
   // moving faster than the new max is handled correctly
   @Test
-  void posContinousUnderVelChange() {
+  void posContinuousUnderVelChange() {
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double lastPos = state.position;
     for (int i = 0; i < 1600; ++i) {
       if (i == 400) {
         constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+        profile = new TrapezoidProfile(constraints);
       }
 
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       double estimatedVel = (state.position - lastPos) / kDt;
 
       if (i >= 400) {
@@ -105,9 +105,9 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 400; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertEquals(state, goal);
   }
@@ -118,16 +118,16 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 200; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertNotEquals(state, goal);
 
     goal = new TrapezoidProfile.State(0.0, 0.0);
+    profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 550; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertEquals(state, goal);
   }
@@ -139,15 +139,15 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 200; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertNear(constraints.maxVelocity, state.velocity, 10e-5);
 
+    profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 2000; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertEquals(state, goal);
   }
@@ -158,9 +158,9 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 400; i++) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
     }
   }
@@ -170,14 +170,13 @@
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double predictedTimeLeft = profile.timeLeftUntil(goal.position);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       if (!reachedGoal && state.equals(goal)) {
         // Expected value using for loop index is just an approximation since
         // the time left in the profile doesn't increase linearly at the
@@ -193,14 +192,13 @@
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double predictedTimeLeft = profile.timeLeftUntil(1);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) {
         assertNear(predictedTimeLeft, i / 100.0, 2e-2);
         reachedGoal = true;
@@ -213,14 +211,13 @@
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double predictedTimeLeft = profile.timeLeftUntil(goal.position);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       if (!reachedGoal && state.equals(goal)) {
         // Expected value using for loop index is just an approximation since
         // the time left in the profile doesn't increase linearly at the
@@ -236,14 +233,13 @@
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double predictedTimeLeft = profile.timeLeftUntil(-1);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) {
         assertNear(predictedTimeLeft, i / 100.0, 2e-2);
         reachedGoal = true;

diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
index e397af1..bbada9e 100644
--- a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp

@@ -2,8 +2,9 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/ComputerVisionUtil.h"
-#include "gtest/gtest.h"
 
 TEST(ComputerVisionUtilTest, ObjectToRobotPose) {
   frc::Pose3d robot{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 30_deg}};

diff --git a/wpimath/src/test/native/cpp/DARETest.cpp b/wpimath/src/test/native/cpp/DARETest.cpp
new file mode 100644
index 0000000..e19fa3c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest.cpp

@@ -0,0 +1,299 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <stdexcept>
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
+#include "DARETestUtil.h"
+#include "frc/DARE.h"
+#include "frc/EigenCore.h"
+#include "frc/fmt/Eigen.h"
+
+// 2x1
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 2, 1>& N);
+
+// 4x1
+extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+    const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+    const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
+extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+    const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+    const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 4, 1>& N);
+
+// 2x2
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+    const Eigen::Matrix<double, 2, 2>& N);
+
+// 2x3
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
+    const Eigen::Matrix<double, 2, 3>& N);
+
+TEST(DARETest, NonInvertibleA_ABQR) {
+  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+  // Riccati Equation"
+
+  frc::Matrixd<4, 4> A{
+      {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+  frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
+  frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+  frc::Matrixd<1, 1> R{0.25};
+
+  frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, NonInvertibleA_ABQRN) {
+  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+  // Riccati Equation"
+
+  frc::Matrixd<4, 4> A{
+      {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+  frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
+  frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+  frc::Matrixd<1, 1> R{0.25};
+
+  frc::Matrixd<4, 4> Aref{
+      {0.25, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+  Q = (A - Aref).transpose() * Q * (A - Aref);
+  R = B.transpose() * Q * B + R;
+  frc::Matrixd<4, 1> N = (A - Aref).transpose() * Q * B;
+
+  frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, InvertibleA_ABQR) {
+  frc::Matrixd<2, 2> A{{1, 1}, {0, 1}};
+  frc::Matrixd<2, 1> B{{0}, {1}};
+  frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
+  frc::Matrixd<1, 1> R{{0.3}};
+
+  frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, InvertibleA_ABQRN) {
+  frc::Matrixd<2, 2> A{{1, 1}, {0, 1}};
+  frc::Matrixd<2, 1> B{{0}, {1}};
+  frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
+  frc::Matrixd<1, 1> R{0.3};
+
+  frc::Matrixd<2, 2> Aref{{0.5, 1}, {0, 1}};
+  Q = (A - Aref).transpose() * Q * (A - Aref);
+  R = B.transpose() * Q * B + R;
+  frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
+
+  frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
+  // The first generalized eigenvalue of (S, T) is stable
+
+  frc::Matrixd<2, 2> A{{0, 1}, {0, 0}};
+  frc::Matrixd<2, 1> B{{0}, {1}};
+  frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
+  frc::Matrixd<1, 1> R{1};
+
+  frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
+  // The first generalized eigenvalue of (S, T) is stable
+
+  frc::Matrixd<2, 2> A{{0, 1}, {0, 0}};
+  frc::Matrixd<2, 1> B{{0}, {1}};
+  frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
+  frc::Matrixd<1, 1> R{1};
+
+  frc::Matrixd<2, 2> Aref{{0, 0.5}, {0, 0}};
+  Q = (A - Aref).transpose() * Q * (A - Aref);
+  R = B.transpose() * Q * B + R;
+  frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
+
+  frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, IdentitySystem_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, IdentitySystem_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+  Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, MoreInputsThanStates_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
+
+  Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, MoreInputsThanStates_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
+  const frc::Matrixd<2, 3> N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
+
+  Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{-Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+
+  const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1)), std::invalid_argument);
+
+  const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2)), std::invalid_argument);
+}
+
+TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+  const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1, N)), std::invalid_argument);
+
+  const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2, N)), std::invalid_argument);
+}
+
+TEST(DARETest, ABNotStabilizable_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Zero()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, ABNotStabilizable_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Zero()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, ACNotDetectable_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, ACNotDetectable_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{Eigen::Matrix2d::Zero()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, QDecomposition) {
+  // Ensures the decomposition of Q into CᵀC is correct
+
+  const Eigen::Matrix2d A{{1.0, 0.0}, {0.0, 0.0}};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  // (A, C₁) should be detectable pair
+  const Eigen::Matrix2d C_1{{0.0, 0.0}, {1.0, 0.0}};
+  const Eigen::Matrix2d Q_1 = C_1.transpose() * C_1;
+  EXPECT_NO_THROW((frc::DARE<2, 2>(A, B, Q_1, R)));
+
+  // (A, C₂) shouldn't be detectable pair
+  const Eigen::Matrix2d C_2 = C_1.transpose();
+  const Eigen::Matrix2d Q_2 = C_2.transpose() * C_2;
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q_2, R)), std::invalid_argument);
+}

diff --git a/wpimath/src/test/native/cpp/DARETestUtil.cpp b/wpimath/src/test/native/cpp/DARETestUtil.cpp
new file mode 100644
index 0000000..452e6ce
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETestUtil.cpp

@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "DARETestUtil.h"
+
+#include <Eigen/Eigenvalues>
+#include <fmt/core.h>
+#include <gtest/gtest.h>
+
+#include "frc/fmt/Eigen.h"
+
+void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
+                       double tolerance) {
+  for (int row = 0; row < lhs.rows(); ++row) {
+    for (int col = 0; col < lhs.cols(); ++col) {
+      EXPECT_NEAR(lhs(row, col), rhs(row, col), tolerance)
+          << fmt::format("row = {}, col = {}", row, col);
+    }
+  }
+
+  if (::testing::Test::HasFailure()) {
+    fmt::print("lhs =\n{}\n", lhs);
+    fmt::print("rhs =\n{}\n", rhs);
+    fmt::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs});
+  }
+}
+
+void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X) {
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
+                                                      Eigen::EigenvaluesOnly};
+  for (int i = 0; i < X.rows(); ++i) {
+    EXPECT_GE(eigX.eigenvalues()[i], 0.0);
+  }
+}
+
+void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B,
+                        const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                        const Eigen::Ref<const Eigen::MatrixXd>& R,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X) {
+  // Check that X is the solution to the DARE
+  // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+  // clang-format off
+  Eigen::MatrixXd Y =
+      A.transpose() * X * A
+      - X
+      - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
+        * B.transpose() * X * A)
+      + Q;
+  // clang-format on
+  ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
+}
+
+void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B,
+                        const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                        const Eigen::Ref<const Eigen::MatrixXd>& R,
+                        const Eigen::Ref<const Eigen::MatrixXd>& N,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X) {
+  // Check that X is the solution to the DARE
+  // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+  // clang-format off
+  Eigen::MatrixXd Y =
+      A.transpose() * X * A
+      - X
+      - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
+        * (B.transpose() * X * A + N.transpose()))
+      + Q;
+  // clang-format on
+  ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
+}

diff --git a/wpimath/src/test/native/cpp/DARETestUtil.h b/wpimath/src/test/native/cpp/DARETestUtil.h
new file mode 100644
index 0000000..f0d2f66
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETestUtil.h

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+
+void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
+                       double tolerance);
+
+void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X);
+
+void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B,
+                        const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                        const Eigen::Ref<const Eigen::MatrixXd>& R,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X);
+
+void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B,
+                        const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                        const Eigen::Ref<const Eigen::MatrixXd>& R,
+                        const Eigen::Ref<const Eigen::MatrixXd>& N,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X);

diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
new file mode 100644
index 0000000..3ba3d71
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp

@@ -0,0 +1,13 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 2, 1>& N);

diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
new file mode 100644
index 0000000..75b17b8
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp

@@ -0,0 +1,13 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+    const Eigen::Matrix<double, 2, 2>& N);

diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
new file mode 100644
index 0000000..8d98553
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp

@@ -0,0 +1,13 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
+    const Eigen::Matrix<double, 2, 3>& N);

diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
new file mode 100644
index 0000000..840285c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp

@@ -0,0 +1,13 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DARE.h"
+
+template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+    const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+    const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
+template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+    const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+    const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 4, 1>& N);

diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp
index 03be9b3..74c8577 100644
--- a/wpimath/src/test/native/cpp/EigenTest.cpp
+++ b/wpimath/src/test/native/cpp/EigenTest.cpp

@@ -2,9 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "Eigen/LU"
+#include <Eigen/LU>
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
-#include "gtest/gtest.h"
 
 TEST(EigenTest, Multiplication) {
   frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
@@ -29,17 +30,17 @@
 }
 
 TEST(EigenTest, Transpose) {
-  frc::Vectord<3> vec{1, 2, 3};
+  Eigen::Vector3d vec{1, 2, 3};
 
   const auto transpose = vec.transpose();
 
-  Eigen::RowVector<double, 3> expectedTranspose{1, 2, 3};
+  Eigen::RowVector3d expectedTranspose{1, 2, 3};
 
   EXPECT_TRUE(expectedTranspose.isApprox(transpose));
 }
 
 TEST(EigenTest, Inverse) {
-  frc::Matrixd<3, 3> mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
+  Eigen::Matrix3d mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
 
   const auto inverse = mat.inverse();
   const auto identity = Eigen::MatrixXd::Identity(3, 3);

diff --git a/wpimath/src/test/native/cpp/FormatterTest.cpp b/wpimath/src/test/native/cpp/FormatterTest.cpp
index 0e2f77c..0b6d590 100644
--- a/wpimath/src/test/native/cpp/FormatterTest.cpp
+++ b/wpimath/src/test/native/cpp/FormatterTest.cpp

@@ -5,10 +5,9 @@
 #include <vector>
 
 #include <fmt/format.h>
+#include <gtest/gtest.h>
 
 #include "frc/fmt/Eigen.h"
-#include "frc/fmt/Units.h"
-#include "gtest/gtest.h"
 #include "units/velocity.h"
 
 TEST(FormatterTest, Eigen) {
@@ -17,14 +16,14 @@
       "  0.000000  1.000000\n"
       "  2.000000  3.000000\n"
       "  4.000000  5.000000",
-      fmt::format("{}", A));
+      fmt::format("{:f}", A));
 
   Eigen::MatrixXd B{{0.0, 1.0}, {2.0, 3.0}, {4.0, 5.0}};
   EXPECT_EQ(
       "  0.000000  1.000000\n"
       "  2.000000  3.000000\n"
       "  4.000000  5.000000",
-      fmt::format("{}", B));
+      fmt::format("{:f}", B));
 
   Eigen::SparseMatrix<double> C{3, 2};
   std::vector<Eigen::Triplet<double>> triplets;
@@ -38,7 +37,7 @@
       "  0.000000  1.000000\n"
       "  2.000000  3.000000\n"
       "  4.000000  5.000000",
-      fmt::format("{}", C));
+      fmt::format("{:f}", C));
 }
 
 TEST(FormatterTest, Units) {

diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp
index a836a77..cee9831 100644
--- a/wpimath/src/test/native/cpp/MathUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp

@@ -4,8 +4,9 @@
 
 #include <limits>
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 
 #define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
@@ -117,3 +118,49 @@
   EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
                   units::radian_t{-std::numbers::pi / 2});
 }
+
+TEST(MathUtilTest, IsNear) {
+  // The answer is always 42
+  // Positive integer checks
+  EXPECT_TRUE(frc::IsNear(42, 42, 1));
+  EXPECT_TRUE(frc::IsNear(42, 41, 2));
+  EXPECT_TRUE(frc::IsNear(42, 43, 2));
+  EXPECT_FALSE(frc::IsNear(42, 44, 1));
+
+  // Negative integer checks
+  EXPECT_TRUE(frc::IsNear(-42, -42, 1));
+  EXPECT_TRUE(frc::IsNear(-42, -41, 2));
+  EXPECT_TRUE(frc::IsNear(-42, -43, 2));
+  EXPECT_FALSE(frc::IsNear(-42, -44, 1));
+
+  // Mixed sign integer checks
+  EXPECT_FALSE(frc::IsNear(-42, 42, 1));
+  EXPECT_FALSE(frc::IsNear(-42, 41, 2));
+  EXPECT_FALSE(frc::IsNear(-42, 43, 2));
+  EXPECT_FALSE(frc::IsNear(42, -42, 1));
+  EXPECT_FALSE(frc::IsNear(42, -41, 2));
+  EXPECT_FALSE(frc::IsNear(42, -43, 2));
+
+  // Floating point checks
+  EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 1));
+  EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 1));
+  EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 0.75));
+  EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 0.75));
+
+  // Wraparound checks
+  EXPECT_TRUE(frc::IsNear(0_deg, 356_deg, 5_deg, 0_deg, 360_deg));
+  EXPECT_TRUE(frc::IsNear(0, -356, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(0, 4, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(0, -4, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, 41, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, -319, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, 401, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, 356, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, -356, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, 4, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, -4, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, 35, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, -315, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
+}

diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
index ee7842c..07907e2 100644
--- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp

@@ -2,11 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 #include <random>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/controller/LinearQuadraticRegulator.h"

diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 105a434..758dc30 100644
--- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp

@@ -2,10 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <array>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/NumericalIntegration.h"

diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp
index 1059260..1464dcd 100644
--- a/wpimath/src/test/native/cpp/UnitsTest.cpp
+++ b/wpimath/src/test/native/cpp/UnitsTest.cpp

@@ -7,10 +7,12 @@
 #include <string>
 #include <type_traits>
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
 #include "units/acceleration.h"
 #include "units/angle.h"
 #include "units/angular_acceleration.h"
+#include "units/angular_jerk.h"
 #include "units/angular_velocity.h"
 #include "units/area.h"
 #include "units/capacitance.h"
@@ -51,6 +53,7 @@
 using namespace units::acceleration;
 using namespace units::angle;
 using namespace units::angular_acceleration;
+using namespace units::angular_jerk;
 using namespace units::angular_velocity;
 using namespace units::area;
 using namespace units::capacitance;
@@ -1420,7 +1423,7 @@
 }
 #endif
 
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
 TEST_F(UnitContainer, fmtlib) {
   testing::internal::CaptureStdout();
   fmt::print("{}", degree_t(349.87));
@@ -2002,6 +2005,23 @@
   EXPECT_NEAR(1.537e-16, test, 5.0e-20);
 }
 
+TEST_F(UnitConversion, angular_jerk) {
+  double test;
+  bool same;
+
+  same =
+      std::is_same_v<radians_per_second_cubed,
+                   unit<std::ratio<1>, category::angular_jerk_unit>>;
+  EXPECT_TRUE(same);
+  same = traits::is_convertible_unit_v<deg_per_s_cu, radians_per_second_cubed>;
+  EXPECT_TRUE(same);
+
+  test = convert<degrees_per_second_cubed, radians_per_second_cubed>(1.0);
+  EXPECT_NEAR(0.0174533, test, 5.0e-8);
+  test = convert<turns_per_second_cubed, radians_per_second_cubed>(1.0);
+  EXPECT_NEAR(6.283185307, test, 5.0e-6);
+}
+
 TEST_F(UnitConversion, acceleration) {
   double test;
 

diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
new file mode 100644
index 0000000..b7853b1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp

@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cmath>
+#include <numbers>
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/ArmFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+static constexpr auto Ks = 0.5_V;
+static constexpr auto Kv = 1.5_V * 1_s / 1_rad;
+static constexpr auto Ka = 2_V * 1_s * 1_s / 1_rad;
+static constexpr auto Kg = 1_V;
+
+TEST(ArmFeedforwardTest, Calculate) {
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(
+      armFF.Calculate(std::numbers::pi * 1_rad / 3, 0_rad / 1_s).value(), 0.5,
+      0.002);
+  EXPECT_NEAR(
+      armFF.Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s).value(), 2.5,
+      0.002);
+  EXPECT_NEAR(armFF
+                  .Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s,
+                             2_rad / 1_s / 1_s)
+                  .value(),
+              6.5, 0.002);
+  EXPECT_NEAR(armFF
+                  .Calculate(std::numbers::pi * 1_rad / 3, -1_rad / 1_s,
+                             2_rad / 1_s / 1_s)
+                  .value(),
+              2.5, 0.002);
+}
+
+TEST(ArmFeedforwardTest, AchievableVelocity) {
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(armFF
+                  .MaxAchievableVelocity(12_V, std::numbers::pi * 1_rad / 3,
+                                         1_rad / 1_s / 1_s)
+                  .value(),
+              6, 0.002);
+  EXPECT_NEAR(armFF
+                  .MinAchievableVelocity(11.5_V, std::numbers::pi * 1_rad / 3,
+                                         1_rad / 1_s / 1_s)
+                  .value(),
+              -9, 0.002);
+}
+
+TEST(ArmFeedforwardTest, AchievableAcceleration) {
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(armFF
+                  .MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+                                             1_rad / 1_s)
+                  .value(),
+              4.75, 0.002);
+  EXPECT_NEAR(armFF
+                  .MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+                                             -1_rad / 1_s)
+                  .value(),
+              6.75, 0.002);
+  EXPECT_NEAR(armFF
+                  .MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+                                             1_rad / 1_s)
+                  .value(),
+              -7.25, 0.002);
+  EXPECT_NEAR(armFF
+                  .MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+                                             -1_rad / 1_s)
+                  .value(),
+              -5.25, 0.002);
+}

diff --git a/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
index 4dfaa0e..f931590 100644
--- a/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp

@@ -2,8 +2,9 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/controller/BangBangController.h"
-#include "gtest/gtest.h"
 
 class BangBangInputOutputTest : public testing::Test {
  protected:

diff --git a/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
index e582720..3484e6e 100644
--- a/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp

@@ -2,8 +2,9 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/controller/BangBangController.h"
-#include "gtest/gtest.h"
 
 class BangBangToleranceTest : public testing::Test {
  protected:

diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index b2bcee6..9db64da 100644
--- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp

@@ -2,10 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/ControlAffinePlantInversionFeedforward.h"
 #include "units/time.h"

diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
index 74d945c..5428d84 100644
--- a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp

@@ -2,11 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
-#include "frc/EigenCore.h"
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
 #include "frc/controller/DifferentialDriveFeedforward.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/system/plant/LinearSystemId.h"
@@ -38,9 +38,9 @@
           auto [left, right] = differentialDriveFeedforward.Calculate(
               currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
               nextRightVelocity, dt);
-          frc::Matrixd<2, 1> nextX = plant.CalculateX(
-              frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
-              frc::Vectord<2>{left, right}, dt);
+          Eigen::Vector2d nextX = plant.CalculateX(
+              Eigen::Vector2d{currentLeftVelocity, currentRightVelocity},
+              Eigen::Vector2d{left, right}, dt);
           EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
           EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
         }
@@ -72,9 +72,9 @@
           auto [left, right] = differentialDriveFeedforward.Calculate(
               currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
               nextRightVelocity, dt);
-          frc::Matrixd<2, 1> nextX = plant.CalculateX(
-              frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
-              frc::Vectord<2>{left, right}, dt);
+          Eigen::Vector2d nextX = plant.CalculateX(
+              Eigen::Vector2d{currentLeftVelocity, currentRightVelocity},
+              Eigen::Vector2d{left, right}, dt);
           EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
           EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
         }

diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
new file mode 100644
index 0000000..a2db70d
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/ElevatorFeedforward.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+static constexpr auto Ks = 0.5_V;
+static constexpr auto Kv = 1.5_V * 1_s / 1_m;
+static constexpr auto Ka = 2_V * 1_s * 1_s / 1_m;
+static constexpr auto Kg = 1_V;
+
+TEST(ElevatorFeedforwardTest, Calculate) {
+  frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+
+  EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
+  EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
+  EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5,
+              0.002);
+  EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5,
+              0.002);
+
+  frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
+  frc::Matrixd<1, 1> B{1.0 / Ka.value()};
+  constexpr units::second_t dt = 20_ms;
+  frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
+
+  frc::Vectord<1> r{2.0};
+  frc::Vectord<1> nextR{3.0};
+  EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
+              elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+}
+
+TEST(ElevatorFeedforwardTest, AchievableVelocity) {
+  frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(elevatorFF.MaxAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
+              5, 0.002);
+  EXPECT_NEAR(elevatorFF.MinAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
+              -9, 0.002);
+}
+
+TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
+  frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, 2_m / 1_s).value(),
+              3.75, 0.002);
+  EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, -2_m / 1_s).value(),
+              7.25, 0.002);
+  EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, 2_m / 1_s).value(),
+              -8.25, 0.002);
+  EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, -2_m / 1_s).value(),
+              -4.75, 0.002);
+}

diff --git a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
index 134a4e0..4b4a883 100644
--- a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp

@@ -4,10 +4,11 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
 #include "frc/controller/HolonomicDriveController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/angular_acceleration.h"
 #include "units/math.h"
 #include "units/time.h"
@@ -21,7 +22,7 @@
 
 TEST(HolonomicDriveControllerTest, ReachesReference) {
   frc::HolonomicDriveController controller{
-      frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0},
+      frc::PIDController{1.0, 0.0, 0.0}, frc::PIDController{1.0, 0.0, 0.0},
       frc::ProfiledPIDController<units::radian>{
           1.0, 0.0, 0.0,
           frc::TrapezoidProfile<units::radian>::Constraints{
@@ -53,7 +54,7 @@
 
 TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
   frc::HolonomicDriveController controller{
-      frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
+      frc::PIDController{1, 0, 0}, frc::PIDController{1, 0, 0},
       frc::ProfiledPIDController<units::radian>{
           1, 0, 0,
           frc::TrapezoidProfile<units::radian>::Constraints{

diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
index 753ea34..d96c67b 100644
--- a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp

@@ -4,12 +4,13 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
 #include "frc/controller/LTVDifferentialDriveController.h"
 #include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/math.h"
 
 #define EXPECT_NEAR_UNITS(val1, val2, eps) \

diff --git a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
index 56faf1d..ff2c7d3 100644
--- a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp

@@ -2,10 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
 #include "frc/controller/LTVUnicycleController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/math.h"
 
 #define EXPECT_NEAR_UNITS(val1, val2, eps) \

diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
index e7107d0..a92d4f0 100644
--- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp

@@ -2,10 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "units/time.h"

diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
index 1127fc2..45f6473 100644
--- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp

@@ -2,10 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "frc/system/LinearSystem.h"
@@ -158,10 +158,10 @@
   EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
 
   // QRN overload
-  Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
+  Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 5.0)}};
   Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
   EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
-  EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
+  EXPECT_NEAR(-6.9190500116751458e-05, Kimf(0, 1), 1e-10);
 }
 
 TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {

diff --git a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index f1034f5..7992622 100644
--- a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp

@@ -2,14 +2,15 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/controller/PIDController.h"
-#include "gtest/gtest.h"
 
 class PIDInputOutputTest : public testing::Test {
  protected:
-  frc2::PIDController* controller;
+  frc::PIDController* controller;
 
-  void SetUp() override { controller = new frc2::PIDController{0, 0, 0}; }
+  void SetUp() override { controller = new frc::PIDController{0, 0, 0}; }
 
   void TearDown() override { delete controller; }
 };
@@ -49,3 +50,21 @@
   EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
                    controller->Calculate(0.0025, 0));
 }
+
+TEST_F(PIDInputOutputTest, IZoneNoOutput) {
+  controller->SetI(1);
+  controller->SetIZone(1);
+
+  double out = controller->Calculate(2, 0);
+
+  EXPECT_DOUBLE_EQ(0, out);
+}
+
+TEST_F(PIDInputOutputTest, IZoneOutput) {
+  controller->SetI(1);
+  controller->SetIZone(1);
+
+  double out = controller->Calculate(1, 0);
+
+  EXPECT_DOUBLE_EQ(-1 * controller->GetPeriod().value(), out);
+}

diff --git a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
index 0aec438..f37be89 100644
--- a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp

@@ -2,27 +2,26 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/controller/PIDController.h"
-#include "gtest/gtest.h"
 
 static constexpr double kSetpoint = 50.0;
 static constexpr double kRange = 200;
 static constexpr double kTolerance = 10.0;
 
 TEST(PIDToleranceTest, InitialTolerance) {
-  frc2::PIDController controller{0.5, 0.0, 0.0};
+  frc::PIDController controller{0.5, 0.0, 0.0};
   controller.EnableContinuousInput(-kRange / 2, kRange / 2);
 
-  EXPECT_TRUE(controller.AtSetpoint());
+  EXPECT_FALSE(controller.AtSetpoint());
 }
 
 TEST(PIDToleranceTest, AbsoluteTolerance) {
-  frc2::PIDController controller{0.5, 0.0, 0.0};
+  frc::PIDController controller{0.5, 0.0, 0.0};
   controller.EnableContinuousInput(-kRange / 2, kRange / 2);
 
-  EXPECT_TRUE(controller.AtSetpoint())
-      << "Error was not in tolerance when it should have been. Error was "
-      << controller.GetPositionError();
+  EXPECT_FALSE(controller.AtSetpoint());
 
   controller.SetTolerance(kTolerance);
   controller.SetSetpoint(kSetpoint);

diff --git a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
index 44d1f41..5e4f01a 100644
--- a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp

@@ -4,8 +4,9 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/controller/ProfiledPIDController.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/angular_acceleration.h"
 #include "units/angular_velocity.h"

diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
index 2fd26bb..d39c679 100644
--- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp

@@ -2,10 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
 #include "frc/controller/RamseteController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/math.h"
 
 #define EXPECT_NEAR_UNITS(val1, val2, eps) \

diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
index 6d10e05..7878011 100644
--- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp

@@ -2,10 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/controller/SimpleMotorFeedforward.h"

diff --git a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
deleted file mode 100644
index 8631d6f..0000000
--- a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
+++ /dev/null

@@ -1,124 +0,0 @@
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-
-#include <Eigen/Eigenvalues>
-#include <gtest/gtest.h>
-
-#include "drake/common/test_utilities/eigen_matrix_compare.h"
-// #include "drake/math/autodiff.h"
-
-using Eigen::MatrixXd;
-
-namespace drake {
-namespace math {
-namespace {
-void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
-                        const Eigen::Ref<const MatrixXd>& B,
-                        const Eigen::Ref<const MatrixXd>& Q,
-                        const Eigen::Ref<const MatrixXd>& R) {
-  MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R);
-  // Check that X is positive semi-definite.
-  EXPECT_TRUE(
-      CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
-  int n = X.rows();
-  Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
-  for (int i = 0; i < n; i++) {
-    EXPECT_GE(es.eigenvalues()[i], 0);
-  }
-  // Check that X is the solution to the discrete time ARE.
-  // clang-format off
-  MatrixXd Y =
-      A.transpose() * X * A
-      - X
-      - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
-        * B.transpose() * X * A)
-      + Q;
-  // clang-format on
-  EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
-                              MatrixCompareType::absolute));
-}
-
-void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
-                        const Eigen::Ref<const MatrixXd>& B,
-                        const Eigen::Ref<const MatrixXd>& Q,
-                        const Eigen::Ref<const MatrixXd>& R,
-                        const Eigen::Ref<const MatrixXd>& N) {
-  MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N);
-  // Check that X is positive semi-definite.
-  EXPECT_TRUE(
-      CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
-  int n = X.rows();
-  Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
-  for (int i = 0; i < n; i++) {
-    EXPECT_GE(es.eigenvalues()[i], 0);
-  }
-  // Check that X is the solution to the discrete time ARE.
-  // clang-format off
-  MatrixXd Y =
-      A.transpose() * X * A
-      - X
-      - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
-        * (B.transpose() * X * A + N.transpose()))
-      + Q;
-  // clang-format on
-  EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
-                              MatrixCompareType::absolute));
-}
-
-GTEST_TEST(DARE, SolveDAREandVerify) {
-  // Test 1: non-invertible A
-  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
-  // Riccati Equation"
-  int n1 = 4, m1 = 1;
-  MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1);
-  A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
-  B1 << 0, 0, 0, 1;
-  Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
-  R1 << 0.25;
-  SolveDAREandVerify(A1, B1, Q1, R1);
-
-  MatrixXd Aref1(n1, n1);
-  Aref1 << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
-  SolveDAREandVerify(A1, B1, (A1 - Aref1).transpose() * Q1 * (A1 - Aref1),
-      B1.transpose() * Q1 * B1 + R1, (A1 - Aref1).transpose() * Q1 * B1);
-
-  // Test 2: invertible A
-  int n2 = 2, m2 = 1;
-  MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2);
-  A2 << 1, 1, 0, 1;
-  B2 << 0, 1;
-  Q2 << 1, 0, 0, 0;
-  R2 << 0.3;
-  SolveDAREandVerify(A2, B2, Q2, R2);
-
-  MatrixXd Aref2(n2, n2);
-  Aref2 << 0.5, 1, 0, 1;
-  SolveDAREandVerify(A2, B2, (A2 - Aref2).transpose() * Q2 * (A2 - Aref2),
-      B2.transpose() * Q2 * B2 + R2, (A2 - Aref2).transpose() * Q2 * B2);
-
-  // Test 3: the first generalized eigenvalue of (S,T) is stable
-  int n3 = 2, m3 = 1;
-  MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3);
-  A3 << 0, 1, 0, 0;
-  B3 << 0, 1;
-  Q3 << 1, 0, 0, 1;
-  R3 << 1;
-  SolveDAREandVerify(A3, B3, Q3, R3);
-
-  MatrixXd Aref3(n3, n3);
-  Aref3 << 0, 0.5, 0, 0;
-  SolveDAREandVerify(A3, B3, (A3 - Aref3).transpose() * Q3 * (A3 - Aref3),
-      B3.transpose() * Q3 * B3 + R3, (A3 - Aref3).transpose() * Q3 * B3);
-
-  // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
-  const Eigen::MatrixXd A4{Eigen::Matrix2d::Identity()};
-  const Eigen::MatrixXd B4{Eigen::Matrix2d::Identity()};
-  const Eigen::MatrixXd Q4{Eigen::Matrix2d::Identity()};
-  const Eigen::MatrixXd R4{Eigen::Matrix2d::Identity()};
-  SolveDAREandVerify(A4, B4, Q4, R4);
-
-  const Eigen::MatrixXd N4{Eigen::Matrix2d::Identity()};
-  SolveDAREandVerify(A4, B4, Q4, R4, N4);
-}
-}  // namespace
-}  // namespace math
-}  // namespace drake

diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
index b2ee87d..d62decd 100644
--- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp

@@ -2,10 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/estimator/AngleStatistics.h"
 

diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index 1d623ca..3103068 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp

@@ -7,13 +7,14 @@
 #include <tuple>
 #include <utility>
 
+#include <gtest/gtest.h>
+
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/DifferentialDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/length.h"
 #include "units/time.h"
@@ -138,6 +139,7 @@
               0.15);
 
   if (checkError) {
+    // NOLINTNEXTLINE(bugprone-integer-division)
     EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.05);
     EXPECT_LT(maxError, 0.2);
   }
@@ -207,3 +209,87 @@
     }
   }
 }
+
+TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+  // This tests for multiple vision measurements appled at the same time.
+  // The expected behavior is that all measurements affect the estimated pose.
+  // The alternative result is that only one vision measurement affects the
+  // outcome. If that were the case, after 1000 measurements, the estimated
+  // pose would converge to that measurement.
+  frc::DifferentialDriveKinematics kinematics{1.0_m};
+
+  frc::DifferentialDrivePoseEstimator estimator{
+      kinematics,
+      frc::Rotation2d{},
+      0_m,
+      0_m,
+      frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+      {0.02, 0.02, 0.01},
+      {0.1, 0.1, 0.1}};
+
+  estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m);
+
+  for (int i = 0; i < 1000; i++) {
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+}
+
+TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+  frc::DifferentialDriveKinematics kinematics{1_m};
+
+  frc::DifferentialDrivePoseEstimator estimator{
+      kinematics,      frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+      {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+  // Add enough measurements to fill up the buffer
+  for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+    estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m);
+  }
+
+  auto odometryPose = estimator.GetEstimatedPosition();
+
+  // Apply a vision measurement from 3 seconds ago
+  estimator.AddVisionMeasurement(
+      frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+      1_s, {0.1, 0.1, 0.1});
+
+  EXPECT_NEAR(odometryPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              1e-6);
+}

diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index 1924b73..2a5ce80 100644
--- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp

@@ -2,12 +2,12 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <array>
 #include <cmath>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/ExtendedKalmanFilter.h"

diff --git a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
index fc373ca..6697ec0 100644
--- a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp

@@ -2,12 +2,12 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <array>
 #include <cmath>
 
-#include "Eigen/Core"
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
 #include "frc/estimator/KalmanFilter.h"
 #include "frc/system/plant/DCMotor.h"
 #include "frc/system/plant/LinearSystemId.h"

diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index 13dc5aa..8b3a586 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp

@@ -6,11 +6,12 @@
 #include <random>
 #include <tuple>
 
+#include <gtest/gtest.h>
+
 #include "frc/estimator/MecanumDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 void testFollowTrajectory(
     const frc::MecanumDriveKinematics& kinematics,
@@ -129,6 +130,7 @@
               0.15);
 
   if (checkError) {
+    // NOLINTNEXTLINE(bugprone-integer-division)
     EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051);
     EXPECT_LT(maxError, 0.2);
   }
@@ -206,3 +208,90 @@
     }
   }
 }
+
+TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+  // This tests for multiple vision measurements appled at the same time.
+  // The expected behavior is that all measurements affect the estimated pose.
+  // The alternative result is that only one vision measurement affects the
+  // outcome. If that were the case, after 1000 measurements, the estimated
+  // pose would converge to that measurement.
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDriveWheelPositions wheelPositions;
+
+  frc::MecanumDrivePoseEstimator estimator{
+      kinematics,      frc::Rotation2d{},
+      wheelPositions,  frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+      {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
+
+  estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions);
+
+  for (int i = 0; i < 1000; i++) {
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+}
+
+TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDrivePoseEstimator estimator{
+      kinematics,    frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
+      frc::Pose2d{}, {0.1, 0.1, 0.1},   {0.45, 0.45, 0.45}};
+
+  // Add enough measurements to fill up the buffer
+  for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+    estimator.UpdateWithTime(time, frc::Rotation2d{},
+                             frc::MecanumDriveWheelPositions{});
+  }
+
+  auto odometryPose = estimator.GetEstimatedPosition();
+
+  // Apply a vision measurement from 3 seconds ago
+  estimator.AddVisionMeasurement(
+      frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+      1_s, {0.1, 0.1, 0.1});
+
+  EXPECT_NEAR(odometryPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              1e-6);
+}

diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index 19a734b..4471dc9 100644
--- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp

@@ -6,8 +6,6 @@
 
 #include "frc/estimator/MerweScaledSigmaPoints.h"
 
-namespace drake::math {
-namespace {
 TEST(MerweScaledSigmaPointsTest, ZeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
   auto points = sigmaPoints.SquareRootSigmaPoints(
@@ -30,5 +28,3 @@
                                    {2.0, 2.0, 2.00548, 2.0, 1.99452}})
           .norm() < 1e-3);
 }
-}  // namespace
-}  // namespace drake::math

diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index 554ca59..f1024ef 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp

@@ -8,12 +8,13 @@
 #include <tuple>
 
 #include <fmt/format.h>
+#include <gtest/gtest.h>
+#include <wpi/timestamp.h>
 
 #include "frc/estimator/SwerveDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 void testFollowTrajectory(
     const frc::SwerveDriveKinematics<4>& kinematics,
@@ -132,6 +133,7 @@
               0.15);
 
   if (checkError) {
+    // NOLINTNEXTLINE(bugprone-integer-division)
     EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058);
     EXPECT_LT(maxError, 0.2);
   }
@@ -215,3 +217,97 @@
     }
   }
 }
+
+TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+  // This tests for multiple vision measurements appled at the same time.
+  // The expected behavior is that all measurements affect the estimated pose.
+  // The alternative result is that only one vision measurement affects the
+  // outcome. If that were the case, after 1000 measurements, the estimated
+  // pose would converge to that measurement.
+  frc::SwerveDriveKinematics<4> kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::SwerveModulePosition fl;
+  frc::SwerveModulePosition fr;
+  frc::SwerveModulePosition bl;
+  frc::SwerveModulePosition br;
+
+  frc::SwerveDrivePoseEstimator<4> estimator{
+      kinematics,       frc::Rotation2d{},
+      {fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+      {0.1, 0.1, 0.1},  {0.45, 0.45, 0.45}};
+
+  estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br});
+
+  for (int i = 0; i < 1000; i++) {
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+}
+
+TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+  frc::SwerveDriveKinematics<4> kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::SwerveModulePosition fl;
+  frc::SwerveModulePosition fr;
+  frc::SwerveModulePosition bl;
+  frc::SwerveModulePosition br;
+
+  frc::SwerveDrivePoseEstimator<4> estimator{
+      kinematics,    frc::Rotation2d{}, {fl, fr, bl, br},
+      frc::Pose2d{}, {0.1, 0.1, 0.1},   {0.45, 0.45, 0.45}};
+
+  // Add enough measurements to fill up the buffer
+  for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+    estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br});
+  }
+
+  auto odometryPose = estimator.GetEstimatedPosition();
+
+  // Apply a vision measurement from 3 seconds ago
+  estimator.AddVisionMeasurement(
+      frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+      1_s, {0.1, 0.1, 0.1});
+
+  EXPECT_NEAR(odometryPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              1e-6);
+}

diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 68f9c40..0f97a88 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp

@@ -2,12 +2,12 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <array>
 #include <cmath>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"

diff --git a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
index 2f64908..d232739 100644
--- a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp

@@ -2,10 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
 #include <wpi/timestamp.h>
 
 #include "frc/filter/Debouncer.h"
-#include "gtest/gtest.h"
 #include "units/time.h"
 
 static units::second_t now = 0_s;

diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
index 8299a71..e0dea2d 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp

@@ -8,7 +8,8 @@
 #include <numbers>
 #include <random>
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
 #include "units/time.h"
 
 // Filter constants

diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
index 152737d..9195525 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp

@@ -10,9 +10,9 @@
 #include <numbers>
 #include <random>
 
+#include <gtest/gtest.h>
 #include <wpi/array.h>
 
-#include "gtest/gtest.h"
 #include "units/time.h"
 
 // Filter constants

diff --git a/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp b/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
index 8151a45..f0f7d99 100644
--- a/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp

@@ -2,8 +2,9 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/filter/MedianFilter.h"
-#include "gtest/gtest.h"
 
 TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
   frc::MedianFilter<double> filter{10};

diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
index 5dbe8c8..9657543 100644
--- a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp

@@ -2,10 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
 #include <wpi/timestamp.h>
 
 #include "frc/filter/SlewRateLimiter.h"
-#include "gtest/gtest.h"
 #include "units/length.h"
 #include "units/time.h"
 #include "units/velocity.h"

diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
index fc44fa5..198f90f 100644
--- a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp

@@ -2,10 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/CoordinateSystem.h"
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Transform3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
@@ -37,9 +38,6 @@
   EXPECT_EQ(transformTo.Translation(),
             CoordinateSystem::Convert(transformFrom.Translation(), coordFrom,
                                       coordTo));
-  EXPECT_EQ(
-      transformTo.Rotation(),
-      CoordinateSystem::Convert(transformFrom.Rotation(), coordFrom, coordTo));
   EXPECT_EQ(transformTo,
             CoordinateSystem::Convert(transformFrom, coordFrom, coordTo));
 
@@ -47,9 +45,6 @@
   EXPECT_EQ(
       transformFrom.Translation(),
       CoordinateSystem::Convert(transformTo.Translation(), coordTo, coordFrom));
-  EXPECT_EQ(
-      transformFrom.Rotation(),
-      CoordinateSystem::Convert(transformTo.Rotation(), coordTo, coordFrom));
   EXPECT_EQ(transformFrom,
             CoordinateSystem::Convert(transformTo, coordTo, coordFrom));
 }
@@ -106,29 +101,28 @@
   // No rotation from EDN to NWU
   CheckTransform3dConvert(
       Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
-      Transform3d{Translation3d{3_m, -1_m, -2_m},
-                  Rotation3d{-90_deg, 0_deg, -90_deg}},
+      Transform3d{Translation3d{3_m, -1_m, -2_m}, Rotation3d{}},
       CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
   // 45° roll from EDN to NWU
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{45_deg, 0_deg, 0_deg}},
                           Transform3d{Translation3d{3_m, -1_m, -2_m},
-                                      Rotation3d{-45_deg, 0_deg, -90_deg}},
+                                      Rotation3d{0_deg, -45_deg, 0_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
   // 45° pitch from EDN to NWU
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{0_deg, 45_deg, 0_deg}},
                           Transform3d{Translation3d{3_m, -1_m, -2_m},
-                                      Rotation3d{-90_deg, 0_deg, -135_deg}},
+                                      Rotation3d{0_deg, 0_deg, -45_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
   // 45° yaw from EDN to NWU
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{0_deg, 0_deg, 45_deg}},
                           Transform3d{Translation3d{3_m, -1_m, -2_m},
-                                      Rotation3d{-90_deg, 45_deg, -90_deg}},
+                                      Rotation3d{45_deg, 0_deg, 0_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NWU());
 }
 
@@ -136,28 +130,27 @@
   // No rotation from EDN to NED
   CheckTransform3dConvert(
       Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
-      Transform3d{Translation3d{3_m, 1_m, 2_m},
-                  Rotation3d{90_deg, 0_deg, 90_deg}},
+      Transform3d{Translation3d{3_m, 1_m, 2_m}, Rotation3d{}},
       CoordinateSystem::EDN(), CoordinateSystem::NED());
 
   // 45° roll from EDN to NED
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{45_deg, 0_deg, 0_deg}},
                           Transform3d{Translation3d{3_m, 1_m, 2_m},
-                                      Rotation3d{135_deg, 0_deg, 90_deg}},
+                                      Rotation3d{0_deg, 45_deg, 0_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NED());
 
   // 45° pitch from EDN to NED
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{0_deg, 45_deg, 0_deg}},
                           Transform3d{Translation3d{3_m, 1_m, 2_m},
-                                      Rotation3d{90_deg, 0_deg, 135_deg}},
+                                      Rotation3d{0_deg, 0_deg, 45_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NED());
 
   // 45° yaw from EDN to NED
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{0_deg, 0_deg, 45_deg}},
                           Transform3d{Translation3d{3_m, 1_m, 2_m},
-                                      Rotation3d{90_deg, -45_deg, 90_deg}},
+                                      Rotation3d{45_deg, 0_deg, 0_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NED());
 }

diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
index 5ce6819..bd5a9aa 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp

@@ -3,12 +3,32 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
+#include <cstdlib>
+
+#include <gtest/gtest.h>
 
 #include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
+TEST(Pose2dTest, RotateBy) {
+  constexpr auto x = 1_m;
+  constexpr auto y = 2_m;
+  const Pose2d initial{x, y, 45_deg};
+
+  const Rotation2d rotation{5_deg};
+  const auto rotated = initial.RotateBy(rotation);
+
+  // Translation is rotated by CCW rotation matrix
+  double c = rotation.Cos();
+  double s = rotation.Sin();
+  EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
+  EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
+  EXPECT_DOUBLE_EQ(
+      initial.Rotation().Degrees().value() + rotation.Degrees().value(),
+      rotated.Rotation().Degrees().value());
+}
+
 TEST(Pose2dTest, TransformBy) {
   const Pose2d initial{1_m, 2_m, 45_deg};
   const Transform2d transform{Translation2d{5_m, 0_m}, 5_deg};
@@ -26,9 +46,9 @@
 
   const auto finalRelativeToInitial = final.RelativeTo(initial);
 
-  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
+  EXPECT_NEAR(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value(), 1e-9);
   EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9);
-  EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value());
+  EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Degrees().value(), 1e-9);
 }
 
 TEST(Pose2dTest, Equality) {
@@ -49,9 +69,76 @@
 
   const auto transform = final - initial;
 
-  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
+  EXPECT_NEAR(5.0 * std::sqrt(2.0), transform.X().value(), 1e-9);
   EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
-  EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
+  EXPECT_NEAR(0.0, transform.Rotation().Degrees().value(), 1e-9);
+}
+
+TEST(Pose2dTest, Nearest) {
+  const Pose2d origin{0_m, 0_m, 0_deg};
+
+  const Pose2d pose1{Translation2d{1_m, Rotation2d{45_deg}}, 0_deg};
+  const Pose2d pose2{Translation2d{2_m, Rotation2d{90_deg}}, 0_deg};
+  const Pose2d pose3{Translation2d{3_m, Rotation2d{135_deg}}, 0_deg};
+  const Pose2d pose4{Translation2d{4_m, Rotation2d{180_deg}}, 0_deg};
+  const Pose2d pose5{Translation2d{5_m, Rotation2d{270_deg}}, 0_deg};
+
+  EXPECT_DOUBLE_EQ(pose3.X().value(),
+                   origin.Nearest({pose5, pose3, pose4}).X().value());
+  EXPECT_DOUBLE_EQ(pose3.Y().value(),
+                   origin.Nearest({pose5, pose3, pose4}).Y().value());
+
+  EXPECT_DOUBLE_EQ(pose1.X().value(),
+                   origin.Nearest({pose1, pose2, pose3}).X().value());
+  EXPECT_DOUBLE_EQ(pose1.Y().value(),
+                   origin.Nearest({pose1, pose2, pose3}).Y().value());
+
+  EXPECT_DOUBLE_EQ(pose2.X().value(),
+                   origin.Nearest({pose4, pose2, pose3}).X().value());
+  EXPECT_DOUBLE_EQ(pose2.Y().value(),
+                   origin.Nearest({pose4, pose2, pose3}).Y().value());
+
+  // Rotation component sort (when distance is the same)
+  // Use the same translation because using different angles at the same
+  // distance can cause rounding error.
+  const Translation2d translation{1_m, Rotation2d{0_deg}};
+
+  const Pose2d poseA{translation, 0_deg};
+  const Pose2d poseB{translation, Rotation2d{30_deg}};
+  const Pose2d poseC{translation, Rotation2d{120_deg}};
+  const Pose2d poseD{translation, Rotation2d{90_deg}};
+  const Pose2d poseE{translation, Rotation2d{-180_deg}};
+
+  EXPECT_DOUBLE_EQ(poseA.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{360_deg})
+                       .Nearest({poseA, poseB, poseD})
+                       .Rotation()
+                       .Degrees()
+                       .value());
+  EXPECT_DOUBLE_EQ(poseB.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{-335_deg})
+                       .Nearest({poseB, poseC, poseD})
+                       .Rotation()
+                       .Degrees()
+                       .value());
+  EXPECT_DOUBLE_EQ(poseC.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{-120_deg})
+                       .Nearest({poseB, poseC, poseD})
+                       .Rotation()
+                       .Degrees()
+                       .value());
+  EXPECT_DOUBLE_EQ(poseD.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{85_deg})
+                       .Nearest({poseA, poseC, poseD})
+                       .Rotation()
+                       .Degrees()
+                       .value());
+  EXPECT_DOUBLE_EQ(poseE.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{170_deg})
+                       .Nearest({poseA, poseD, poseE})
+                       .Rotation()
+                       .Degrees()
+                       .value());
 }
 
 TEST(Pose2dTest, Constexpr) {

diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
index 8c4452c..6d689db 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp

@@ -4,11 +4,34 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+#include <wpi/array.h>
+
 #include "frc/geometry/Pose3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
+TEST(Pose3dTest, RotateBy) {
+  constexpr auto x = 1_m;
+  constexpr auto y = 2_m;
+  const Pose3d initial{x, y, 0_m, Rotation3d{0_deg, 0_deg, 45_deg}};
+
+  constexpr units::radian_t yaw = 5_deg;
+  const Rotation3d rotation{0_deg, 0_deg, yaw};
+  const auto rotated = initial.RotateBy(rotation);
+
+  // Translation is rotated by CCW rotation matrix
+  double c = std::cos(yaw.value());
+  double s = std::sin(yaw.value());
+  EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
+  EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Z().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(initial.Rotation().Z().value() + rotation.Z().value(),
+                   rotated.Rotation().Z().value());
+}
+
 TEST(Pose3dTest, TestTransformByRotations) {
   const double kEpsilon = 1E-9;
 
@@ -95,3 +118,86 @@
 
   EXPECT_EQ(expected, pose.ToPose2d());
 }
+
+TEST(Pose3dTest, ComplexTwists) {
+  wpi::array<Pose3d, 5> initial_poses{
+      Pose3d{0.698303_m, -0.959096_m, 0.271076_m,
+             Rotation3d{Quaternion{0.86403, -0.076866, 0.147234, 0.475254}}},
+      Pose3d{0.634892_m, -0.765209_m, 0.117543_m,
+             Rotation3d{Quaternion{0.84987, -0.070829, 0.162097, 0.496415}}},
+      Pose3d{0.584827_m, -0.590303_m, -0.02557_m,
+             Rotation3d{Quaternion{0.832743, -0.041991, 0.202188, 0.513708}}},
+      Pose3d{0.505038_m, -0.451479_m, -0.112835_m,
+             Rotation3d{Quaternion{0.816515, -0.002673, 0.226182, 0.531166}}},
+      Pose3d{0.428178_m, -0.329692_m, -0.189707_m,
+             Rotation3d{Quaternion{0.807886, 0.029298, 0.257788, 0.529157}}},
+  };
+
+  wpi::array<Pose3d, 5> final_poses{
+      Pose3d{-0.230448_m, -0.511957_m, 0.198406_m,
+             Rotation3d{Quaternion{0.753984, 0.347016, 0.409105, 0.379106}}},
+      Pose3d{-0.088932_m, -0.343253_m, 0.095018_m,
+             Rotation3d{Quaternion{0.638738, 0.413016, 0.536281, 0.365833}}},
+      Pose3d{-0.107908_m, -0.317552_m, 0.133946_m,
+             Rotation3d{Quaternion{0.653444, 0.417069, 0.465505, 0.427046}}},
+      Pose3d{-0.123383_m, -0.156411_m, -0.047435_m,
+             Rotation3d{Quaternion{0.652983, 0.40644, 0.431566, 0.47135}}},
+      Pose3d{-0.084654_m, -0.019305_m, -0.030022_m,
+             Rotation3d{Quaternion{0.620243, 0.429104, 0.479384, 0.44873}}},
+  };
+
+  for (size_t i = 0; i < initial_poses.size(); i++) {
+    auto start = initial_poses[i];
+    auto end = final_poses[i];
+
+    auto twist = start.Log(end);
+    auto start_exp = start.Exp(twist);
+
+    auto eps = 1E-5;
+
+    EXPECT_NEAR(start_exp.X().value(), end.X().value(), eps);
+    EXPECT_NEAR(start_exp.Y().value(), end.Y().value(), eps);
+    EXPECT_NEAR(start_exp.Z().value(), end.Z().value(), eps);
+    EXPECT_NEAR(start_exp.Rotation().GetQuaternion().W(),
+                end.Rotation().GetQuaternion().W(), eps);
+    EXPECT_NEAR(start_exp.Rotation().GetQuaternion().X(),
+                end.Rotation().GetQuaternion().X(), eps);
+    EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Y(),
+                end.Rotation().GetQuaternion().Y(), eps);
+    EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Z(),
+                end.Rotation().GetQuaternion().Z(), eps);
+  }
+}
+
+TEST(Pose3dTest, TwistNaN) {
+  wpi::array<Pose3d, 2> initial_poses{
+      Pose3d{6.32_m, 4.12_m, 0.00_m,
+             Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
+                                   1.9208309264993548E-8}}},
+      Pose3d{3.75_m, 2.95_m, 0.00_m,
+             Rotation3d{Quaternion{0.9999999999999793, 0.0, 0.0,
+                                   2.0352360299846772E-7}}},
+  };
+
+  wpi::array<Pose3d, 2> final_poses{
+      Pose3d{6.33_m, 4.15_m, 0.00_m,
+             Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
+                                   2.416890209039172E-8}}},
+      Pose3d{3.66_m, 2.93_m, 0.00_m,
+             Rotation3d{Quaternion{0.9999999999999782, 0.0, 0.0,
+                                   2.0859477994905617E-7}}},
+  };
+
+  for (size_t i = 0; i < initial_poses.size(); i++) {
+    auto start = initial_poses[i];
+    auto end = final_poses[i];
+    auto twist = start.Log(end);
+
+    EXPECT_FALSE(std::isnan(twist.dx.value()));
+    EXPECT_FALSE(std::isnan(twist.dy.value()));
+    EXPECT_FALSE(std::isnan(twist.dz.value()));
+    EXPECT_FALSE(std::isnan(twist.rx.value()));
+    EXPECT_FALSE(std::isnan(twist.ry.value()));
+    EXPECT_FALSE(std::isnan(twist.rz.value()));
+  }
+}

diff --git a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
index 5b95abb..ef0e95e 100644
--- a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp

@@ -4,8 +4,9 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Quaternion.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/math.h"
 
@@ -43,6 +44,54 @@
                             q3.Z() * q3.Z());
 }
 
+TEST(QuaternionTest, Addition) {
+  Quaternion q{0.1, 0.2, 0.3, 0.4};
+  Quaternion p{0.5, 0.6, 0.7, 0.8};
+
+  auto sum = q + p;
+
+  EXPECT_DOUBLE_EQ(q.W() + p.W(), sum.W());
+  EXPECT_DOUBLE_EQ(q.X() + p.X(), sum.X());
+  EXPECT_DOUBLE_EQ(q.Y() + p.Y(), sum.Y());
+  EXPECT_DOUBLE_EQ(q.Z() + p.Z(), sum.Z());
+}
+
+TEST(QuaternionTest, Subtraction) {
+  Quaternion q{0.1, 0.2, 0.3, 0.4};
+  Quaternion p{0.5, 0.6, 0.7, 0.8};
+
+  auto difference = q - p;
+
+  EXPECT_DOUBLE_EQ(q.W() - p.W(), difference.W());
+  EXPECT_DOUBLE_EQ(q.X() - p.X(), difference.X());
+  EXPECT_DOUBLE_EQ(q.Y() - p.Y(), difference.Y());
+  EXPECT_DOUBLE_EQ(q.Z() - p.Z(), difference.Z());
+}
+
+TEST(QuaternionTest, ScalarMultiplication) {
+  Quaternion q{0.1, 0.2, 0.3, 0.4};
+  auto scalar = 2;
+
+  auto product = q * scalar;
+
+  EXPECT_DOUBLE_EQ(q.W() * scalar, product.W());
+  EXPECT_DOUBLE_EQ(q.X() * scalar, product.X());
+  EXPECT_DOUBLE_EQ(q.Y() * scalar, product.Y());
+  EXPECT_DOUBLE_EQ(q.Z() * scalar, product.Z());
+}
+
+TEST(QuaternionTest, ScalarDivision) {
+  Quaternion q{0.1, 0.2, 0.3, 0.4};
+  auto scalar = 2;
+
+  auto product = q / scalar;
+
+  EXPECT_DOUBLE_EQ(q.W() / scalar, product.W());
+  EXPECT_DOUBLE_EQ(q.X() / scalar, product.X());
+  EXPECT_DOUBLE_EQ(q.Y() / scalar, product.Y());
+  EXPECT_DOUBLE_EQ(q.Z() / scalar, product.Z());
+}
+
 TEST(QuaternionTest, Multiply) {
   // 90° CCW rotations around each axis
   double c = units::math::cos(90_deg / 2.0);
@@ -64,19 +113,110 @@
   Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
                0.48507125007266594};
   actual = q * q.Inverse();
-  EXPECT_DOUBLE_EQ(1.0, actual.W());
-  EXPECT_DOUBLE_EQ(0.0, actual.X());
-  EXPECT_DOUBLE_EQ(0.0, actual.Y());
-  EXPECT_DOUBLE_EQ(0.0, actual.Z());
+  EXPECT_NEAR(1.0, actual.W(), 1e-9);
+  EXPECT_NEAR(0.0, actual.X(), 1e-9);
+  EXPECT_NEAR(0.0, actual.Y(), 1e-9);
+  EXPECT_NEAR(0.0, actual.Z(), 1e-9);
+}
+
+TEST(QuaternionTest, Conjugate) {
+  Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
+               0.48507125007266594};
+  auto conj = q.Conjugate();
+
+  EXPECT_DOUBLE_EQ(q.W(), conj.W());
+  EXPECT_DOUBLE_EQ(-q.X(), conj.X());
+  EXPECT_DOUBLE_EQ(-q.Y(), conj.Y());
+  EXPECT_DOUBLE_EQ(-q.Z(), conj.Z());
 }
 
 TEST(QuaternionTest, Inverse) {
   Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
                0.48507125007266594};
+  auto norm = q.Norm();
+
   auto inv = q.Inverse();
 
-  EXPECT_DOUBLE_EQ(q.W(), inv.W());
-  EXPECT_DOUBLE_EQ(-q.X(), inv.X());
-  EXPECT_DOUBLE_EQ(-q.Y(), inv.Y());
-  EXPECT_DOUBLE_EQ(-q.Z(), inv.Z());
+  EXPECT_DOUBLE_EQ(q.W() / (norm * norm), inv.W());
+  EXPECT_DOUBLE_EQ(-q.X() / (norm * norm), inv.X());
+  EXPECT_DOUBLE_EQ(-q.Y() / (norm * norm), inv.Y());
+  EXPECT_DOUBLE_EQ(-q.Z() / (norm * norm), inv.Z());
+}
+
+TEST(QuaternionTest, Norm) {
+  Quaternion q{3, 4, 12, 84};
+  auto norm = q.Norm();
+
+  EXPECT_NEAR(85, norm, 1e-9);
+}
+
+TEST(QuaternionTest, Exponential) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+  Quaternion expect{2.81211398529184, -0.392521193481878, -0.588781790222817,
+                    -0.785042386963756};
+
+  auto q_exp = q.Exp();
+
+  EXPECT_EQ(expect, q_exp);
+}
+
+TEST(QuaternionTest, Logarithm) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+  Quaternion expect{1.7959088706354, 0.515190292664085, 0.772785438996128,
+                    1.03038058532817};
+
+  auto q_log = q.Log();
+
+  EXPECT_EQ(expect, q_log);
+
+  Quaternion zero{0, 0, 0, 0};
+  Quaternion one{1, 0, 0, 0};
+  Quaternion i{0, 1, 0, 0};
+  Quaternion j{0, 0, 1, 0};
+  Quaternion k{0, 0, 0, 1};
+  Quaternion ln_half{std::log(0.5), -std::numbers::pi, 0, 0};
+
+  EXPECT_EQ(zero, one.Log());
+  EXPECT_EQ(i * std::numbers::pi / 2, i.Log());
+  EXPECT_EQ(j * std::numbers::pi / 2, j.Log());
+  EXPECT_EQ(k * std::numbers::pi / 2, k.Log());
+
+  EXPECT_EQ(i * -std::numbers::pi, (one * -1).Log());
+  EXPECT_EQ(ln_half, (one * -0.5).Log());
+}
+
+TEST(QuaternionTest, LogarithmAndExponentialInverse) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+
+  // These operations are order-dependent: ln(exp(q)) is congruent but not
+  // necessarily equal to exp(ln(q)) due to the multi-valued nature of the
+  // complex logarithm.
+
+  auto q_log_exp = q.Log().Exp();
+
+  EXPECT_EQ(q, q_log_exp);
+
+  Quaternion start{1, 2, 3, 4};
+  Quaternion expect{5, 6, 7, 8};
+
+  auto twist = start.Log(expect);
+  auto actual = start.Exp(twist);
+
+  EXPECT_EQ(expect, actual);
+}
+
+TEST(QuaternionTest, DotProduct) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+  Quaternion p{5.5, 6.6, 7.7, 8.8};
+
+  EXPECT_NEAR(q.W() * p.W() + q.X() * p.X() + q.Y() * p.Y() + q.Z() * p.Z(),
+              q.Dot(p), 1e-9);
+}
+
+TEST(QuaternionTest, DotProductAsEquality) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+  auto q_conj = q.Conjugate();
+
+  EXPECT_NEAR(q.Dot(q), q.Norm() * q.Norm(), 1e-9);
+  EXPECT_GT(std::abs(q.Dot(q_conj) - q.Norm() * q_conj.Norm()), 1e-9);
 }

diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index 4e2e683..de2fc17 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp

@@ -5,8 +5,9 @@
 #include <cmath>
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Rotation2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
index 4709ed0..01b37e5 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp

@@ -5,52 +5,93 @@
 #include <cmath>
 #include <numbers>
 
+#include <Eigen/Core>
+#include <gtest/gtest.h>
 #include <wpi/MathExtras.h>
 
-#include "frc/EigenCore.h"
 #include "frc/geometry/Rotation3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
+TEST(Rotation3dTest, GimbalLockAccuracy) {
+  auto rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+  auto rot2 = Rotation3d{units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
+  auto rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+  const auto result1 = rot1 + rot2 + rot3;
+  const auto expected1 =
+      Rotation3d{0_rad, -units::radian_t{std::numbers::pi / 2},
+                 units::radian_t{std::numbers::pi / 2}};
+  EXPECT_EQ(expected1, result1);
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result1.X() + result1.Z()).value());
+  EXPECT_DOUBLE_EQ(-std::numbers::pi / 2, result1.Y().value());
+
+  rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+  rot2 = Rotation3d{units::radian_t{-std::numbers::pi}, 0_rad, 0_rad};
+  rot3 = Rotation3d{units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+  const auto result2 = rot1 + rot2 + rot3;
+  const auto expected2 =
+      Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
+                 units::radian_t{std::numbers::pi / 2}};
+  EXPECT_EQ(expected2, result2);
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result2.Z() - result2.X()).value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result2.Y().value());
+
+  rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+  rot2 = Rotation3d{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
+  rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+  const auto result3 = rot1 + rot2 + rot3;
+  const auto expected3 =
+      Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
+                 units::radian_t{std::numbers::pi / 6}};
+  EXPECT_EQ(expected3, result3);
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 6, (result3.Z() - result3.X()).value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result3.Y().value());
+}
+
 TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
   const Rotation3d rot1{xAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot2{units::radian_t{std::numbers::pi / 3}, 0_rad, 0_rad};
+  const Rotation3d rvec1{Eigen::Vector3d{xAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot1, rot2);
+  EXPECT_EQ(rot1, rvec1);
 
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
   const Rotation3d rot3{yAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot4{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
+  const Rotation3d rvec2{Eigen::Vector3d{yAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot3, rot4);
+  EXPECT_EQ(rot3, rvec2);
 
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
   const Rotation3d rot5{zAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot6{0_rad, 0_rad, units::radian_t{std::numbers::pi / 3}};
+  const Rotation3d rvec3{Eigen::Vector3d{zAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot5, rot6);
+  EXPECT_EQ(rot5, rvec3);
 }
 
 TEST(Rotation3dTest, InitRotationMatrix) {
   // No rotation
-  const Matrixd<3, 3> R1 = Matrixd<3, 3>::Identity();
+  const Eigen::Matrix3d R1 = Eigen::Matrix3d::Identity();
   const Rotation3d rot1{R1};
   EXPECT_EQ(Rotation3d{}, rot1);
 
   // 90 degree CCW rotation around z-axis
-  Matrixd<3, 3> R2;
-  R2.block<3, 1>(0, 0) = Vectord<3>{0.0, 1.0, 0.0};
-  R2.block<3, 1>(0, 1) = Vectord<3>{-1.0, 0.0, 0.0};
-  R2.block<3, 1>(0, 2) = Vectord<3>{0.0, 0.0, 1.0};
+  Eigen::Matrix3d R2;
+  R2.block<3, 1>(0, 0) = Eigen::Vector3d{0.0, 1.0, 0.0};
+  R2.block<3, 1>(0, 1) = Eigen::Vector3d{-1.0, 0.0, 0.0};
+  R2.block<3, 1>(0, 2) = Eigen::Vector3d{0.0, 0.0, 1.0};
   const Rotation3d rot2{R2};
   const Rotation3d expected2{0_deg, 0_deg, 90_deg};
   EXPECT_EQ(expected2, rot2);
 
   // Matrix that isn't orthogonal
-  const Matrixd<3, 3> R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
+  const Eigen::Matrix3d R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
   EXPECT_THROW(Rotation3d{R3}, std::domain_error);
 
   // Matrix that's orthogonal but not special orthogonal
-  const Matrixd<3, 3> R4 = Matrixd<3, 3>::Identity() * 2.0;
+  const Eigen::Matrix3d R4 = Eigen::Matrix3d::Identity() * 2.0;
   EXPECT_THROW(Rotation3d{R4}, std::domain_error);
 }
 

diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
index 1b0934d..49c9466 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp

@@ -4,11 +4,12 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Transform2d.h"
 #include "frc/geometry/Translation2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
index 904ec9c..8a6bbc0 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp

@@ -4,11 +4,12 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Rotation3d.h"
 #include "frc/geometry/Transform3d.h"
 #include "frc/geometry/Translation3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index 5493c43..1c92c0a 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp

@@ -4,8 +4,9 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Translation2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
@@ -94,6 +95,37 @@
   EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
 }
 
+TEST(Translation2dTest, Nearest) {
+  const Translation2d origin{0_m, 0_m};
+
+  const Translation2d translation1{1_m, Rotation2d{45_deg}};
+  const Translation2d translation2{2_m, Rotation2d{90_deg}};
+  const Translation2d translation3{3_m, Rotation2d{135_deg}};
+  const Translation2d translation4{4_m, Rotation2d{180_deg}};
+  const Translation2d translation5{5_m, Rotation2d{270_deg}};
+
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation5, translation3, translation4}).X().value(),
+      translation3.X().value());
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation5, translation3, translation4}).Y().value(),
+      translation3.Y().value());
+
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation1, translation2, translation3}).X().value(),
+      translation1.X().value());
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation1, translation2, translation3}).Y().value(),
+      translation1.Y().value());
+
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation4, translation2, translation3}).X().value(),
+      translation2.X().value());
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation4, translation2, translation3}).Y().value(),
+      translation2.Y().value());
+}
+
 TEST(Translation2dTest, Constexpr) {
   constexpr Translation2d defaultCtor;
   constexpr Translation2d componentCtor{1_m, 2_m};

diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
index 58c611e..b7aa8ce 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp

@@ -4,8 +4,9 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Translation3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
index 33970cd..7f121c0 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp

@@ -5,8 +5,9 @@
 #include <cmath>
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

diff --git a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
index 0d8a8f4..245ec17 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp

@@ -5,8 +5,9 @@
 #include <cmath>
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

diff --git a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
index 8a920f6..e448db2 100644
--- a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
+++ b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp

@@ -4,10 +4,11 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/interpolation/TimeInterpolatableBuffer.h"
-#include "gtest/gtest.h"
 #include "units/time.h"
 
 TEST(TimeInterpolatableBufferTest, Interpolation) {

diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
index 4e0b3e5..97de03b 100644
--- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp

@@ -2,12 +2,34 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <cmath>
+
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/ChassisSpeeds.h"
-#include "gtest/gtest.h"
 
 static constexpr double kEpsilon = 1E-9;
 
-TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
+TEST(ChassisSpeedsTest, Discretize) {
+  constexpr frc::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
+  constexpr units::second_t duration = 1_s;
+  constexpr units::second_t dt = 10_ms;
+
+  const auto speeds = frc::ChassisSpeeds::Discretize(target, duration);
+  const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
+
+  frc::Pose2d pose;
+  for (units::second_t time = 0_s; time < duration; time += dt) {
+    pose = pose.Exp(twist);
+  }
+
+  EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon);
+  EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon);
+  EXPECT_NEAR((target.omega * duration).value(),
+              pose.Rotation().Radians().value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) {
   const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
       1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);
 
@@ -15,3 +37,64 @@
   EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
   EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
 }
+
+TEST(ChassisSpeedsTest, FromRobotRelativeSpeeds) {
+  const auto chassisSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds(
+      1.0_mps, 0.0_mps, 0.5_rad_per_s, 45.0_deg);
+
+  EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
+  EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);
+  EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Plus) {
+  const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+  const frc::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
+
+  const frc::ChassisSpeeds result = left + right;
+
+  EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(1.0, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Minus) {
+  const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+  const frc::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
+
+  const frc::ChassisSpeeds result = left - right;
+
+  EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(0, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(0.5, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, UnaryMinus) {
+  const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+  const frc::ChassisSpeeds result = -speeds;
+
+  EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(-0.75, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Multiplication) {
+  const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+  const frc::ChassisSpeeds result = speeds * 2;
+
+  EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(1.5, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Division) {
+  const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+  const frc::ChassisSpeeds result = speeds / 2;
+
+  EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(0.375, result.omega.value(), kEpsilon);
+}

diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
index 4af5ac8..782fed0 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp

@@ -4,9 +4,10 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "gtest/gtest.h"
 #include "units/angular_velocity.h"
 #include "units/length.h"
 #include "units/velocity.h"

diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index e480941..a228357 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp

@@ -4,9 +4,10 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/kinematics/DifferentialDriveOdometry.h"
-#include "gtest/gtest.h"
 
 static constexpr double kEpsilon = 1E-9;
 

diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp
new file mode 100644
index 0000000..92cc583
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp

@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+TEST(DifferentialDriveWheelSpeedsTest, Plus) {
+  const frc::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
+  const frc::DifferentialDriveWheelSpeeds right{2.0_mps, 1.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = left + right;
+
+  EXPECT_EQ(3.0, result.left.value());
+  EXPECT_EQ(2.0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Minus) {
+  const frc::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
+  const frc::DifferentialDriveWheelSpeeds right{2.0_mps, 0.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = left - right;
+
+  EXPECT_EQ(-1.0, result.left.value());
+  EXPECT_EQ(0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, UnaryMinus) {
+  const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = -speeds;
+
+  EXPECT_EQ(-1.0, result.left.value());
+  EXPECT_EQ(-0.5, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Multiplication) {
+  const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = speeds * 2;
+
+  EXPECT_EQ(2.0, result.left.value());
+  EXPECT_EQ(1.0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Division) {
+  const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = speeds / 2;
+
+  EXPECT_EQ(0.5, result.left.value());
+  EXPECT_EQ(0.25, result.right.value());
+}

diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index 364163e..04a3f1f 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp

@@ -4,9 +4,10 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Translation2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
-#include "gtest/gtest.h"
 #include "units/angular_velocity.h"
 
 using namespace frc;

diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index bfaf91b..be20655 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp

@@ -5,9 +5,10 @@
 #include <limits>
 #include <random>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/MecanumDriveOdometry.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp
new file mode 100644
index 0000000..c10cfb5
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+TEST(MecanumDriveWheelSpeedsTest, Plus) {
+  const frc::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+  const frc::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = left + right;
+
+  EXPECT_EQ(3.0, result.frontLeft.value());
+  EXPECT_EQ(2.0, result.frontRight.value());
+  EXPECT_EQ(2.5, result.rearLeft.value());
+  EXPECT_EQ(2.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Minus) {
+  const frc::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+  const frc::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = left - right;
+
+  EXPECT_EQ(-1.0, result.frontLeft.value());
+  EXPECT_EQ(-1.0, result.frontRight.value());
+  EXPECT_EQ(1.5, result.rearLeft.value());
+  EXPECT_EQ(0.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
+  const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = -speeds;
+
+  EXPECT_EQ(-1.0, result.frontLeft.value());
+  EXPECT_EQ(-0.5, result.frontRight.value());
+  EXPECT_EQ(-2.0, result.rearLeft.value());
+  EXPECT_EQ(-1.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
+  const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = speeds * 2;
+
+  EXPECT_EQ(2.0, result.frontLeft.value());
+  EXPECT_EQ(1.0, result.frontRight.value());
+  EXPECT_EQ(4.0, result.rearLeft.value());
+  EXPECT_EQ(3.0, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Division) {
+  const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = speeds / 2;
+
+  EXPECT_EQ(0.5, result.frontLeft.value());
+  EXPECT_EQ(0.25, result.frontRight.value());
+  EXPECT_EQ(1.0, result.rearLeft.value());
+  EXPECT_EQ(0.75, result.rearRight.value());
+}

diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index 8e0dc8f..e7726e6 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp

@@ -4,9 +4,10 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Translation2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
-#include "gtest/gtest.h"
 #include "units/angular_velocity.h"
 
 using namespace frc;
@@ -125,6 +126,25 @@
   EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
   EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
 }
+TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) {
+  Rotation2d fl = {0_deg};
+  Rotation2d fr = {90_deg};
+  Rotation2d bl = {180_deg};
+  Rotation2d br = {270_deg};
+  m_kinematics.ResetHeadings(fl, fr, bl, br);
+  auto [flMod, frMod, blMod, brMod] =
+      m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
+
+  EXPECT_NEAR(flMod.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(frMod.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(blMod.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(brMod.speed.value(), 0.0, kEpsilon);
+
+  EXPECT_NEAR(flMod.angle.Degrees().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(frMod.angle.Degrees().value(), 90.0, kEpsilon);
+  EXPECT_NEAR(blMod.angle.Degrees().value(), 180.0, kEpsilon);
+  EXPECT_NEAR(brMod.angle.Degrees().value(), 270.0, kEpsilon);
+}
 
 TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
   SwerveModuleState fl{106.629_mps, 135_deg};
@@ -274,3 +294,18 @@
   EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
   EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
 }
+
+TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
+  SwerveModuleState state1{1.0_mps, 0_deg};
+  SwerveModuleState state2{1.0_mps, 0_deg};
+  SwerveModuleState state3{-2.0_mps, 0_deg};
+  SwerveModuleState state4{-2.0_mps, 0_deg};
+
+  wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+  SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 1.0_mps);
+
+  EXPECT_NEAR(arr[0].speed.value(), 0.5, kEpsilon);
+  EXPECT_NEAR(arr[1].speed.value(), 0.5, kEpsilon);
+  EXPECT_NEAR(arr[2].speed.value(), -1.0, kEpsilon);
+  EXPECT_NEAR(arr[3].speed.value(), -1.0, kEpsilon);
+}

diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index 8c67ab5..265b0cc 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp

@@ -5,12 +5,13 @@
 #include <limits>
 #include <random>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
 #include "frc/trajectory/Trajectory.h"
 #include "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp
new file mode 100644
index 0000000..14155f1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveModulePosition.h"
+
+TEST(SwerveModulePositionTest, Equality) {
+  frc::SwerveModulePosition position1{2_m, 90_deg};
+  frc::SwerveModulePosition position2{2_m, 90_deg};
+
+  EXPECT_EQ(position1, position2);
+}
+
+TEST(SwerveModulePositionTest, Inequality) {
+  frc::SwerveModulePosition position1{1_m, 90_deg};
+  frc::SwerveModulePosition position2{2_m, 90_deg};
+  frc::SwerveModulePosition position3{1_m, 89_deg};
+
+  EXPECT_NE(position1, position2);
+  EXPECT_NE(position1, position3);
+}

diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
index 4880bef..efc3975 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp

@@ -2,9 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Rotation2d.h"
 #include "frc/kinematics/SwerveModuleState.h"
-#include "gtest/gtest.h"
 
 static constexpr double kEpsilon = 1E-9;
 
@@ -39,3 +40,19 @@
   EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
   EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
 }
+
+TEST(SwerveModuleStateTest, Equality) {
+  frc::SwerveModuleState state1{2_mps, 90_deg};
+  frc::SwerveModuleState state2{2_mps, 90_deg};
+
+  EXPECT_EQ(state1, state2);
+}
+
+TEST(SwerveModuleStateTest, Inequality) {
+  frc::SwerveModuleState state1{1_mps, 90_deg};
+  frc::SwerveModuleState state2{2_mps, 90_deg};
+  frc::SwerveModuleState state3{1_mps, 89_deg};
+
+  EXPECT_NE(state1, state2);
+  EXPECT_NE(state1, state3);
+}

diff --git a/wpimath/src/test/native/cpp/main.cpp b/wpimath/src/test/native/cpp/main.cpp
index 09072ee..e993c1f 100644
--- a/wpimath/src/test/native/cpp/main.cpp
+++ b/wpimath/src/test/native/cpp/main.cpp

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
 
 int main(int argc, char** argv) {
   ::testing::InitGoogleTest(&argc, argv);

diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 7422a7f..c98d190 100644
--- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp

@@ -5,12 +5,13 @@
 #include <chrono>
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/spline/QuinticHermiteSpline.h"
 #include "frc/spline/SplineHelper.h"
 #include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
 #include "units/length.h"
 
 using namespace frc;

diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index e45df7b..a0df9bb 100644
--- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp

@@ -4,12 +4,13 @@
 
 #include <chrono>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/spline/QuinticHermiteSpline.h"
 #include "frc/spline/SplineHelper.h"
 #include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/length.h"
 

diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index d735338..c42fcf0 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp

@@ -2,11 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <functional>
 
-#include "Eigen/Eigenvalues"
+#include <Eigen/Eigenvalues>
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/NumericalIntegration.h"
@@ -114,98 +114,6 @@
       << discQIntegrated;
 }
 
-// Test that the Taylor series discretization produces nearly identical results.
-TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
-  frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
-  frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
-
-  constexpr auto dt = 1_s;
-
-  frc::Matrixd<2, 2> discQTaylor;
-  frc::Matrixd<2, 2> discA;
-  frc::Matrixd<2, 2> discATaylor;
-
-  // Continuous Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
-  for (int i = 0; i < contQ.rows(); ++i) {
-    EXPECT_GE(esCont.eigenvalues()[i], 0);
-  }
-
-  //       T
-  // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-  //       0
-  frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
-      std::function<frc::Matrixd<2, 2>(units::second_t,
-                                       const frc::Matrixd<2, 2>&)>,
-      frc::Matrixd<2, 2>>(
-      [&](units::second_t t, const frc::Matrixd<2, 2>&) {
-        return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
-                                  (contA.transpose() * t.value()).exp());
-      },
-      0_s, frc::Matrixd<2, 2>::Zero(), dt);
-
-  frc::DiscretizeA<2>(contA, dt, &discA);
-  frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
-
-  EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
-      << "Expected these to be nearly equal:\ndiscQTaylor:\n"
-      << discQTaylor << "\ndiscQIntegrated:\n"
-      << discQIntegrated;
-  EXPECT_LT((discA - discATaylor).norm(), 1e-10);
-
-  // Discrete Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor};
-  for (int i = 0; i < discQTaylor.rows(); ++i) {
-    EXPECT_GE(esDisc.eigenvalues()[i], 0);
-  }
-}
-
-// Test that the Taylor series discretization produces nearly identical results.
-TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
-  frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}};
-  frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
-
-  constexpr auto dt = 5_ms;
-
-  frc::Matrixd<2, 2> discQTaylor;
-  frc::Matrixd<2, 2> discA;
-  frc::Matrixd<2, 2> discATaylor;
-
-  // Continuous Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
-  for (int i = 0; i < contQ.rows(); ++i) {
-    EXPECT_GE(esCont.eigenvalues()[i], 0);
-  }
-
-  //       T
-  // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-  //       0
-  frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
-      std::function<frc::Matrixd<2, 2>(units::second_t,
-                                       const frc::Matrixd<2, 2>&)>,
-      frc::Matrixd<2, 2>>(
-      [&](units::second_t t, const frc::Matrixd<2, 2>&) {
-        return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
-                                  (contA.transpose() * t.value()).exp());
-      },
-      0_s, frc::Matrixd<2, 2>::Zero(), dt);
-
-  frc::DiscretizeA<2>(contA, dt, &discA);
-  frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
-
-  EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
-      << "Expected these to be nearly equal:\ndiscQTaylor:\n"
-      << discQTaylor << "\ndiscQIntegrated:\n"
-      << discQIntegrated;
-  EXPECT_LT((discA - discATaylor).norm(), 1e-10);
-
-  // Discrete Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
-  for (int i = 0; i < discQTaylor.rows(); ++i) {
-    EXPECT_GE(esDisc.eigenvalues()[i], 0);
-  }
-}
-
 // Test that DiscretizeR() works
 TEST(DiscretizationTest, DiscretizeR) {
   frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};

diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index dbc4284..4dc6263 100644
--- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp

@@ -5,6 +5,7 @@
 #include <frc/system/LinearSystem.h>
 #include <frc/system/plant/DCMotor.h>
 #include <frc/system/plant/LinearSystemId.h>
+
 #include <gtest/gtest.h>
 
 #include "frc/system/plant/LinearSystemId.h"

diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index b1793ad..1c73195 100644
--- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp

@@ -2,10 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
 #include "frc/system/NumericalIntegration.h"
 
 // Tests that integrating dx/dt = e^x works.
@@ -30,6 +31,16 @@
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
 
+// Tests that integrating dx/dt = 0 works with RKDP
+TEST(NumericalIntegrationTest, ZeroRKDP) {
+  frc::Vectord<1> y1 = frc::RKDP(
+      [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return frc::Vectord<1>::Zero();
+      },
+      frc::Vectord<1>{0.0}, frc::Vectord<1>{0.0}, 0.1_s);
+  EXPECT_NEAR(y1(0), 0.0, 1e-3);
+}
+
 // Tests that integrating dx/dt = e^x works with RKDP
 TEST(NumericalIntegrationTest, ExponentialRKDP) {
   frc::Vectord<1> y0{0.0};

diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
index 70f1938..e502e95 100644
--- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp

@@ -2,10 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
 #include "frc/system/RungeKuttaTimeVarying.h"
 
 namespace {

diff --git a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
index e2f7112..fafcbd4 100644
--- a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp

@@ -5,9 +5,10 @@
 #include <memory>
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/acceleration.h"
 #include "units/angle.h"

diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
index e3723b5..09abc76 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp

@@ -5,9 +5,10 @@
 #include <memory>
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/time.h"
 

diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
index e3d6b7f..5282638 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp

@@ -5,11 +5,12 @@
 #include <memory>
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/acceleration.h"
 #include "units/length.h"

diff --git a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
index 8d9e221..c39bb15 100644
--- a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp

@@ -4,10 +4,11 @@
 
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
 #include "frc/trajectory/constraint/MaxVelocityConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/acceleration.h"
 #include "units/angle.h"

diff --git a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
new file mode 100644
index 0000000..8df3aa1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp

@@ -0,0 +1,338 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/trajectory/ExponentialProfile.h"  // NOLINT(build/include_order)
+
+#include <chrono>
+#include <cmath>
+#include <tuple>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/acceleration.h"
+#include "units/frequency.h"
+#include "units/length.h"
+#include "units/math.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+static constexpr auto kDt = 10_ms;
+static constexpr auto kV = 2.5629_V / 1_mps;
+static constexpr auto kA = 0.43277_V / 1_mps_sq;
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
+  if (val1 <= val2) {                            \
+    EXPECT_LE(val1, val2);                       \
+  } else {                                       \
+    EXPECT_NEAR_UNITS(val1, val2, eps);          \
+  }
+
+frc::ExponentialProfile<units::meter, units::volts>::State CheckDynamics(
+    frc::ExponentialProfile<units::meter, units::volts> profile,
+    frc::ExponentialProfile<units::meter, units::volts>::Constraints
+        constraints,
+    frc::SimpleMotorFeedforward<units::meter> feedforward,
+    frc::ExponentialProfile<units::meter, units::volts>::State current,
+    frc::ExponentialProfile<units::meter, units::volts>::State goal) {
+  auto next = profile.Calculate(kDt, current, goal);
+  auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt);
+
+  EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V);
+
+  return next;
+}
+
+TEST(ExponentialProfileTest, ReachesGoal) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 450; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(ExponentialProfileTest, PosContinousUnderVelChange) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 300; ++i) {
+    if (i == 150) {
+      constraints.maxInput = 9_V;
+      profile =
+          frc::ExponentialProfile<units::meter, units::volts>{constraints};
+    }
+
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 300; ++i) {
+    if (i == 150) {
+      constraints.maxInput = 9_V;
+      profile =
+          frc::ExponentialProfile<units::meter, units::volts>{constraints};
+    }
+
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// There is some somewhat tricky code for dealing with going backwards
+TEST(ExponentialProfileTest, Backwards) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+  for (int i = 0; i < 400; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 50; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_NE(state, goal);
+
+  goal = {0.0_m, 0.0_mps};
+  for (int i = 0; i < 100; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, TopSpeed) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+  units::meters_per_second_t maxSpeed = 0_mps;
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    maxSpeed = units::math::max(state.velocity, maxSpeed);
+  }
+
+  EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, TopSpeedBackward) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+  units::meters_per_second_t maxSpeed = 0_mps;
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    maxSpeed = units::math::min(state.velocity, maxSpeed);
+  }
+
+  EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, HighInitialSpeed) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 8_mps};
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, -8_mps};
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+
+  EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TestHeuristic) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  std::vector<std::tuple<
+      frc::ExponentialProfile<units::meter, units::volts>::State,  // initial
+      frc::ExponentialProfile<units::meter, units::volts>::State,  // goal
+      frc::ExponentialProfile<units::meter, units::volts>::State>  // inflection
+                                                                   // point
+              >
+      testCases{
+          // red > green and purple => always positive => false
+          {{0_m, -4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}},
+          {{0_m, -4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}},
+          {{0.6603_m, 4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}},
+          {{0.6603_m, 4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}},
+
+          // purple > red > green => positive if v0 < 0 => c && !d && a
+          {{0_m, -4_mps}, {0.5_m, -2_mps}, {0.4367_m, 3.7217_mps}},
+          {{0_m, -4_mps}, {0.546_m, 2_mps}, {0.4367_m, 3.7217_mps}},
+          {{0.6603_m, 4_mps}, {0.5_m, -2_mps}, {0.5560_m, -2.9616_mps}},
+          {{0.6603_m, 4_mps}, {0.546_m, 2_mps}, {0.5560_m, -2.9616_mps}},
+
+          // red < green and purple => always negative => true => !c && !d
+          {{0_m, -4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}},
+          {{0_m, -4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}},
+          {{0.6603_m, 4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}},
+          {{0.6603_m, 4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}},
+
+          // green > red > purple => positive if vf < 0 => !c && d && b
+          {{0_m, -4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}},
+          {{0_m, -4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}},
+          {{0.6603_m, 4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}},
+          {{0.6603_m, 4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}},
+
+          // tests for initial velocity > V/kV
+          {{0_m, -8_mps}, {0_m, 0_mps}, {-0.1384_m, 3.342_mps}},
+          {{0_m, -8_mps}, {-1_m, 0_mps}, {-0.562_m, -6.792_mps}},
+          {{0_m, 8_mps}, {1_m, 0_mps}, {0.562_m, 6.792_mps}},
+          {{0_m, 8_mps}, {-1_m, 0_mps}, {-0.785_m, -4.346_mps}},
+      };
+
+  for (auto& testCase : testCases) {
+    auto state = profile.CalculateInflectionPoint(std::get<0>(testCase),
+                                                  std::get<1>(testCase));
+    EXPECT_NEAR_UNITS(std::get<2>(testCase).position / 1_m,
+                      state.position / 1_m, 1e-3);
+    EXPECT_NEAR_UNITS(std::get<2>(testCase).velocity / 1_mps,
+                      state.velocity / 1_mps, 1e-3);
+  }
+}
+
+TEST(ExponentialProfileTest, TimingToCurrent) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state, state), 0_s, 2e-2_s);
+  }
+
+  EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TimingToGoal) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  auto prediction = profile.TimeLeftUntil(state, goal);
+  auto reachedGoal = false;
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    if (!reachedGoal && state == goal) {
+      EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s);
+      reachedGoal = true;
+    }
+  }
+
+  EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TimingToNegativeGoal) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-2_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  auto prediction = profile.TimeLeftUntil(state, goal);
+  auto reachedGoal = false;
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    if (!reachedGoal && state == goal) {
+      EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s);
+      reachedGoal = true;
+    }
+  }
+
+  EXPECT_EQ(state, goal);
+}

diff --git a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
index 0bf6b1a..8ec3a70 100644
--- a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp

@@ -4,10 +4,11 @@
 
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/constraint/MaxVelocityConstraint.h"
 #include "frc/trajectory/constraint/RectangularRegionConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/acceleration.h"
 #include "units/length.h"

diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
index 2b733a8..2bfd541 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp

@@ -2,9 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 TEST(TrajectoryConcatenateTest, States) {
   auto t1 = frc::TrajectoryGenerator::GenerateTrajectory(

diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 5892461..8b50160 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp

@@ -4,11 +4,12 @@
 
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/trajectory/Trajectory.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/math.h"
 

diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
index 90c6dc0..9411696 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp

@@ -2,9 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <gtest/gtest.h>
+
 #include "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryUtil.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 
 using namespace frc;

diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
index 5c77a5b..f088fa3 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp

@@ -4,10 +4,11 @@
 
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/trajectory/Trajectory.h"
 #include "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
                               std::vector<frc::Trajectory::State> statesB) {

diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index 6a35261..b2e5b18 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp

@@ -7,7 +7,8 @@
 #include <chrono>
 #include <cmath>
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
 #include "units/acceleration.h"
 #include "units/length.h"
 #include "units/math.h"
@@ -31,9 +32,9 @@
   frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 450; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_EQ(state, goal);
 }
@@ -45,17 +46,18 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto lastPos = state.position;
   for (int i = 0; i < 1600; ++i) {
     if (i == 400) {
       constraints.maxVelocity = 0.75_mps;
+      profile = frc::TrapezoidProfile<units::meter>{constraints};
     }
 
-    profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     auto estimatedVel = (state.position - lastPos) / kDt;
 
     if (i >= 400) {
@@ -79,9 +81,9 @@
   frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 400; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_EQ(state, goal);
 }
@@ -92,16 +94,16 @@
   frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_NE(state, goal);
 
   goal = {0.0_m, 0.0_mps};
+  profile = frc::TrapezoidProfile<units::meter>{constraints};
   for (int i = 0; i < 550; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_EQ(state, goal);
 }
@@ -113,15 +115,15 @@
   frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
 
+  profile = frc::TrapezoidProfile<units::meter>{constraints};
   for (int i = 0; i < 2000; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_EQ(state, goal);
 }
@@ -132,9 +134,9 @@
   frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 400; i++) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
   }
 }
@@ -146,14 +148,14 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
       // time left in the profile doesn't increase linearly at the endpoints
@@ -170,14 +172,14 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     if (!reachedGoal &&
         (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
       EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
@@ -193,14 +195,14 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
       // time left in the profile doesn't increase linearly at the endpoints
@@ -217,14 +219,14 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     if (!reachedGoal &&
         (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
       EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);

diff --git a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
deleted file mode 100644
index d6bcbb8..0000000
--- a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
+++ /dev/null

@@ -1,116 +0,0 @@
-#pragma once
-
-#include <algorithm>
-#include <cmath>
-#include <limits>
-
-#include <Eigen/Core>
-#include <gtest/gtest.h>
-
-// #include "drake/common/text_logging.h"
-
-namespace drake {
-
-enum class MatrixCompareType { absolute, relative };
-
-/**
- * Compares two matrices to determine whether they are equal to within a certain
- * threshold.
- *
- * @param m1 The first matrix to compare.
- * @param m2 The second matrix to compare.
- * @param tolerance The tolerance for determining equivalence.
- * @param compare_type Whether the tolereance is absolute or relative.
- * @param explanation A pointer to a string variable for saving an explanation
- * of why @p m1 and @p m2 are unequal. This parameter is optional and defaults
- * to `nullptr`. If this is `nullptr` and @p m1 and @p m2 are not equal, an
- * explanation is logged as an error message.
- * @return true if the two matrices are equal based on the specified tolerance.
- */
-template <typename DerivedA, typename DerivedB>
-[[nodiscard]] ::testing::AssertionResult CompareMatrices(
-    const Eigen::MatrixBase<DerivedA>& m1,
-    const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
-    MatrixCompareType compare_type = MatrixCompareType::absolute) {
-  if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
-    return ::testing::AssertionFailure()
-           << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
-           << " vs. " << m2.rows() << " x " << m2.cols() << ")";
-  }
-
-  for (int ii = 0; ii < m1.rows(); ii++) {
-    for (int jj = 0; jj < m1.cols(); jj++) {
-      // First handle the corner cases of positive infinity, negative infinity,
-      // and NaN
-      const auto both_positive_infinity =
-          m1(ii, jj) == std::numeric_limits<double>::infinity() &&
-          m2(ii, jj) == std::numeric_limits<double>::infinity();
-
-      const auto both_negative_infinity =
-          m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
-          m2(ii, jj) == -std::numeric_limits<double>::infinity();
-
-      using std::isnan;
-      const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
-
-      if (both_positive_infinity || both_negative_infinity || both_nan)
-        continue;
-
-      // Check for case where one value is NaN and the other is not
-      if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
-          (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
-        return ::testing::AssertionFailure() << "NaN mismatch at (" << ii
-                                             << ", " << jj << "):\nm1 =\n"
-                                             << m1 << "\nm2 =\n"
-                                             << m2;
-      }
-
-      // Determine whether the difference between the two matrices is less than
-      // the tolerance.
-      using std::abs;
-      const auto delta = abs(m1(ii, jj) - m2(ii, jj));
-
-      if (compare_type == MatrixCompareType::absolute) {
-        // Perform comparison using absolute tolerance.
-
-        if (delta > tolerance) {
-          return ::testing::AssertionFailure()
-                 << "Values at (" << ii << ", " << jj
-                 << ") exceed tolerance: " << m1(ii, jj) << " vs. "
-                 << m2(ii, jj) << ", diff = " << delta
-                 << ", tolerance = " << tolerance << "\nm1 =\n"
-                 << m1 << "\nm2 =\n"
-                 << m2 << "\ndelta=\n"
-                 << (m1 - m2);
-        }
-      } else {
-        // Perform comparison using relative tolerance, see:
-        // http://realtimecollisiondetection.net/blog/?p=89
-        using std::max;
-        const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj)));
-        const auto relative_tolerance =
-            tolerance * max(decltype(max_value){1}, max_value);
-
-        if (delta > relative_tolerance) {
-          return ::testing::AssertionFailure()
-                 << "Values at (" << ii << ", " << jj
-                 << ") exceed tolerance: " << m1(ii, jj) << " vs. "
-                 << m2(ii, jj) << ", diff = " << delta
-                 << ", tolerance = " << tolerance
-                 << ", relative tolerance = " << relative_tolerance
-                 << "\nm1 =\n"
-                 << m1 << "\nm2 =\n"
-                 << m2 << "\ndelta=\n"
-                 << (m1 - m2);
-        }
-      }
-    }
-  }
-
-  return ::testing::AssertionSuccess() << "m1 =\n"
-                                       << m1
-                                       << "\nis approximately equal to m2 =\n"
-                                       << m2;
-}
-
-}  // namespace drake

diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
index 5e7b165..6273532 100644
--- a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
+++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h

@@ -6,7 +6,6 @@
 
 #include <array>
 
-#include "frc/EigenCore.h"
 #include "units/time.h"
 
 namespace frc {

diff --git a/wpimath/wpimath-config.cmake.in b/wpimath/wpimath-config.cmake.in
index 4769e43..9100d79 100644
--- a/wpimath/wpimath-config.cmake.in
+++ b/wpimath/wpimath-config.cmake.in

@@ -2,5 +2,9 @@
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
 
+if(@USE_SYSTEM_EIGEN@)
+    find_dependency(Eigen3)
+endif()
+
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpimath.cmake)
commit	b13e13f43101a4dd3b9665015eca2d3770eb734a	[log] [tgz]
author	James Kuszmaul <jabukuszmaul+collab@gmail.com>	Wed Nov 22 20:44:04 2023 -0800
committer	James Kuszmaul <jabukuszmaul+collab@gmail.com>	Wed Nov 22 20:44:04 2023 -0800
tree	a8824b99468c742c110c5e1371cbaf04d0f3eccd
parent	cf32412550e07365bb2afb6faed18e0e4c0ff7c1 [diff]