Merge branch 'main' into 2027

2026-06-30 02:31:44 +00:00 · 2025-04-25 23:45:43 -07:00
parent 2f0990e9d2 0695a4db89
commit 36811211be
79 changed files with 2093 additions and 415 deletions
--- a/.bazelrc
+++ b/.bazelrc
@@ -11,14 +11,14 @@ build --tool_java_runtime_version=remotejdk_17
 test --test_output=errors
 test --test_verbose_timeout_warnings

-import shared/bazel/compiler_flags/sanitizers.rc
-import shared/bazel/compiler_flags/base_linux_flags.rc
-import shared/bazel/compiler_flags/linux_flags.rc
-import shared/bazel/compiler_flags/osx_flags.rc
-import shared/bazel/compiler_flags/roborio_flags.rc
-import shared/bazel/compiler_flags/systemcore_flags.rc
-import shared/bazel/compiler_flags/windows_flags.rc
-import shared/bazel/compiler_flags/coverage_flags.rc
+import %workspace%/shared/bazel/compiler_flags/sanitizers.rc
+import %workspace%/shared/bazel/compiler_flags/base_linux_flags.rc
+import %workspace%/shared/bazel/compiler_flags/linux_flags.rc
+import %workspace%/shared/bazel/compiler_flags/osx_flags.rc
+import %workspace%/shared/bazel/compiler_flags/roborio_flags.rc
+import %workspace%/shared/bazel/compiler_flags/systemcore_flags.rc
+import %workspace%/shared/bazel/compiler_flags/windows_flags.rc
+import %workspace%/shared/bazel/compiler_flags/coverage_flags.rc

 # Alias toolchain names to what wpilibsuite uses for CI/Artifact naming
 build:athena --config=roborio
--- a/.github/workflows/bazel.yml
+++ b/.github/workflows/bazel.yml
@@ -31,8 +31,8 @@ jobs:
        with:
          token: ${{ secrets.BUILDBUDDY_API_KEY }}

-      - name: Build Release
-        run: bazel --output_user_root=C:\\bazelroot ${{ matrix.action }} -k ...   --config=ci -c opt ${{ matrix.config }} --verbose_failures
+      - name: bazel ${{ matrix.action }}
+        run: bazel --output_user_root=C:\\bazelroot ${{ matrix.action }} -k ...   --config=ci ${{ matrix.config }} --verbose_failures
        shell: bash

  build-mac:
@@ -47,7 +47,7 @@ jobs:
        with:
          token: ${{ secrets.BUILDBUDDY_API_KEY }}

-      - name: Build Release
+      - name: bazel test (release)
        run: bazel test -k ...   --config=ci -c opt --config=macos --nojava_header_compilation --verbose_failures
        shell: bash

@@ -70,7 +70,7 @@ jobs:
        with:
          token: ${{ secrets.BUILDBUDDY_API_KEY }}

-      - name: Build and Test Release
+      - name: bazel ${{ matrix.action }} (release)
        run: bazel ${{ matrix.action }} ... --config=ci -c opt ${{ matrix.config }} -k --verbose_failures

  buildifier:
--- a/.github/workflows/cmake-android.yml
+++ b/.github/workflows/cmake-android.yml
@@ -39,7 +39,7 @@ jobs:
          java-version: 17

      - name: Install sccache
-        uses: mozilla-actions/sccache-action@v0.0.5
+        uses: mozilla-actions/sccache-action@v0.0.9

      - name: Install dependencies
        run: sudo apt-get update && sudo apt-get install -y ninja-build
--- a/.github/workflows/cmake.yml
+++ b/.github/workflows/cmake.yml
@@ -50,7 +50,7 @@ jobs:
        uses: lukka/get-cmake@v3.29.3

      - name: Install sccache
-        uses: mozilla-actions/sccache-action@v0.0.5
+        uses: mozilla-actions/sccache-action@v0.0.9

      - uses: actions/checkout@v4

--- a/.github/workflows/lint-format.yml
+++ b/.github/workflows/lint-format.yml
@@ -36,7 +36,7 @@ jobs:
      - name: Install wpiformat
        run: |
          python -m venv ${{ runner.temp }}/wpiformat
-          ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.51
+          ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2025.33
      - name: Run
        run: ${{ runner.temp }}/wpiformat/bin/wpiformat
      - name: Check output
@@ -78,7 +78,7 @@ jobs:
      - name: Install wpiformat
        run: |
          python -m venv ${{ runner.temp }}/wpiformat
-          ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.51
+          ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2025.33
      - name: Create compile_commands.json
        run: |
          ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio
--- a/.github/workflows/sanitizers.yml
+++ b/.github/workflows/sanitizers.yml
@@ -36,7 +36,7 @@ jobs:
        run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv-java clang-18 libprotobuf-dev protobuf-compiler ninja-build

      - name: Install sccache
-        uses: mozilla-actions/sccache-action@v0.0.5
+        uses: mozilla-actions/sccache-action@v0.0.9

      - uses: actions/checkout@v4

--- a/CODE_OF_CONDUCT.md
+++ b/CODE_OF_CONDUCT.md
@@ -1,69 +1,47 @@
-# Contributor Covenant Code of Conduct
+# Contributor Community Code of Conduct

 ## Our Pledge

-We as members, contributors, and leaders pledge to make participation in our
-community a harassment-free experience for everyone, regardless of age, body
-size, visible or invisible disability, ethnicity, sex characteristics, gender
-identity and expression, level of experience, education, socio-economic status,
-nationality, personal appearance, race, religion, or sexual identity
-and orientation.
+As members, contributors, and leaders, we commit to fostering a community where everyone feels safe, respected, and valued. We are dedicated to ensuring that participation in this community is harassment-free, inclusive, and welcoming, regardless of age, body type, abilities (visible or invisible), ethnicity, gender identity or expression, sexual orientation, socioeconomic background, education, nationality, personal appearance, race, or religion.
+
+Above all, we pledge to act with integrity, kindness, and empathy—striving to be not only good participants but also good humans.

-We pledge to act and interact in ways that contribute to an open, welcoming,
-diverse, inclusive, and healthy community.

 ## Our Standards

-Examples of behavior that contributes to a positive environment for our
-community include:
+Positive and respectful behavior is essential to creating a thriving community. This includes:

-* Exhibiting Gracious Professionalism® at all times. Gracious Professionalism
+* Practicing **Gracious Professionalism®** at all times. Gracious Professionalism
  is a way of doing things that encourages high-quality work, emphasizes the
  value of others, and respects individuals and the community.
-* Demonstrating empathy and kindness toward other people
-* Being respectful of differing opinions, viewpoints, and experiences
-* Giving and gracefully accepting constructive feedback
-* Accepting responsibility and apologizing to those affected by our mistakes,
-  and learning from the experience
-* Focusing on what is best not just for us as individuals, but for the
-  overall community
+* Showing empathy, kindness, and patience.
+* Respecting diverse perspectives and experiences.
+* Giving and receiving constructive feedback with openness and humility.
+* Owning mistakes, apologizing when necessary, and learning from them.
+* Prioritizing the well-being and success of the entire community over individual interests.

-Examples of unacceptable behavior include:
+Unacceptable behavior include:

-* The use of sexualized language or imagery, and sexual attention or
-  advances of any kind
+* Using sexualized language, imagery, or making inappropriate advances
 * Trolling, insulting or derogatory comments, and personal or political attacks
-* Public or private harassment
-* Publishing others' private information, such as a physical or email
-  address, without their explicit permission
-* Other conduct which could reasonably be considered inappropriate in a
-  professional setting
+* Harassment in any form, whether public or private.
+* Sharing private information (e.g., email or physical addresses) without explicit consent.
+
+* Any behavior that is unprofessional, harmful, or exclusionary.

 ## Enforcement Responsibilities

-Community leaders are responsible for clarifying and enforcing our standards of
-acceptable behavior and will take appropriate and fair corrective action in
-response to any behavior that they deem inappropriate, threatening, offensive,
-or harmful.
-
-Community leaders have the right and responsibility to remove, edit, or reject
-comments, commits, code, wiki edits, issues, and other contributions that are
-not aligned to this Code of Conduct, and will communicate reasons for moderation
-decisions when appropriate.
+Community leaders are responsible for maintaining these standards and will take appropriate action to address any behavior deemed harmful, threatening, or inappropriate. Actions may include removing content, issuing warnings, or, when necessary, banning individuals. Moderation decisions will be communicated transparently where appropriate.

 ## Scope

-This Code of Conduct applies within all community spaces, and also applies when
-an individual is officially representing the community in public spaces.
-Examples of representing our community include using an official e-mail address,
-posting via an official social media account, or acting as an appointed
-representative at an online or offline event.
+This Code of Conduct applies to all community spaces, events, and instances where individuals represent the community (e.g., official email accounts, social media posts, or in-person/virtual events).

 ## Enforcement

 Instances of abusive, harassing, or otherwise unacceptable behavior may be
 reported to the community leaders responsible for enforcement at
-[conduct@wpilib.org](mailto:conduct@wpilib.org).
+[wpilib@wpi.edu](mailto:wpilib@wpi.edu).
 All complaints will be reviewed and investigated promptly and fairly.

 All community leaders are obligated to respect the privacy and security of the
@@ -115,6 +93,9 @@ individual, or aggression toward or disparagement of classes of individuals.
 **Consequence**: A permanent ban from any sort of public interaction within
 the community.

+## A Note on Kindness
+Building a community isn’t just about rules—it’s about connection. Every interaction is an opportunity to be understanding, compassionate, and supportive. Being a good human is the key to our ethos.
+
 ## Attribution

 This Code of Conduct is adapted from the [Contributor Covenant][homepage],
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -80,7 +80,7 @@ xₖ₊₁ = Axₖ + Buₖ

 Changes should be submitted as a Pull Request against the main branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current main branch. We do this to make sure that the git history isn't too cluttered.

-During the build season, breaking changes or other changes intended for the next season can be created as a pull request against the development branch of WPILib. After the season is over, the changes in the development branch will be merged into main.
+Particularly large and/or breaking changes should be targeted to the 2027 branch, which targets the [SystemCore Robot Controller](https://community.firstinspires.org/introducing-the-future-mobile-robot-controller). The intent is minimize changes for 2026, to allow development to focus on preparing for 2027.

 ### Merge Process

--- a/README.md
+++ b/README.md
@@ -50,7 +50,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
 - C++ compiler
    - On Linux, install GCC 11 or greater
    - On Windows, install [Visual Studio Community 2022](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation (Gradle can't use the build tools for Visual Studio)
-    - On macOS, install the Xcode command-line build tools via `xcode-select --install`. Xcode 13 or later is required.
+    - On macOS 13.3 or newer, install Xcode 14 or later (the command-line build tools are insufficient).
 - ARM compiler toolchain
    - Run `./gradlew installRoboRioToolchain` after cloning this repository
    - If the WPILib installer was used, this toolchain is already installed
@@ -136,6 +136,9 @@ If you have installed the FRC Toolchain to a directory other than the default, o

 Once a PR has been submitted, formatting can be run in CI by commenting `/format` on the PR. A new commit will be pushed with the formatting changes.

+> [!NOTE]
+> The `/format` action has been temporarily disabled. The individual formatting commands can be run locally as shown below. Alternately, the Lint and Format action for a PR will upload a patch file that can be downloaded and applied manually.
+
 #### wpiformat

 wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat` on Windows or `python3 -m wpiformat` on other platforms.
--- a/cmake/modules/CompileWarnings.cmake
+++ b/cmake/modules/CompileWarnings.cmake
@@ -46,7 +46,7 @@ macro(wpilib_target_warnings target)

    # Suppress warning "enumeration types with a fixed underlying type are a
    # Clang extension"
-    if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    if(CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT EMSCRIPTEN)
        target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:C>:-Wno-fixed-enum-extension>)
    endif()

--- a/cscore/BUILD.bazel
+++ b/cscore/BUILD.bazel
@@ -42,6 +42,13 @@ objc_library(
        "src/main/native/include",
        "src/main/native/objcpp",
    ],
+    sdk_frameworks = [
+        "CoreFoundation",
+        "AVFoundation",
+        "Foundation",
+        "CoreMedia",
+        "CoreVideo",
+    ],
    tags = ["manual"],
    deps = [
        "//wpinet:wpinet.static",
--- a/cscore/src/main/native/objcpp/UsbCameraImpl.h
+++ b/cscore/src/main/native/objcpp/UsbCameraImpl.h
@@ -91,6 +91,5 @@ class UsbCameraImpl : public SourceImpl {
 private:
  UsbCameraImplObjc* m_objc;
  std::vector<CameraModeStore> m_platformModes;
-  VideoMode m_mode;
 };
 }  // namespace cs
--- a/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm
+++ b/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm
@@ -380,22 +380,27 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) {
             toCheck->height, toCheck->fps);
  std::vector<CameraModeStore>& platformModes =
      sharedThis->objcGetPlatformVideoModes();
-  // Find the matching mode
-  auto match = std::find_if(platformModes.begin(), platformModes.end(),
-                            [&](CameraModeStore& input) {
-                              return input.mode.CompareWithoutFps(*toCheck);
-                            });
+  
+  // Find all matching modes
+  std::vector<CameraModeStore*> matchingModes;
+  for (auto& mode : platformModes) {
+    if (mode.mode.CompareWithoutFps(*toCheck)) {
+      matchingModes.push_back(&mode);
+    }
+  }

-  if (match == platformModes.end()) {
+  if (matchingModes.empty()) {
    return nil;
  }

  // Check FPS
-  for (CameraFPSRange& range : match->fpsRanges) {
-    OBJCDEBUG3("Checking Range {} {}", range.min, range.max);
-    if (range.IsWithinRange(toCheck->fps)) {
-      *fps = toCheck->fps;
-      return match->format;
+  for (auto mode : matchingModes) {
+    for (CameraFPSRange& range : mode->fpsRanges) {
+      OBJCDEBUG3("Checking Range {} {}", range.min, range.max);
+      if (range.IsWithinRange(toCheck->fps)) {
+        *fps = toCheck->fps;
+        return mode->format;
+      }
    }
  }

--- a/datalog/doc/datalog.adoc
+++ b/datalog/doc/datalog.adoc
@@ -190,3 +190,9 @@ Each entry's data type is an arbitrary string. The following data types are stan
 === Metadata

 Each entry has an associated metadata string. If not blank, the metadata should be <<JSON,JSON>>, but may be arbitrary text. Metadata is intended to convey additional information about the entry beyond what the type conveys--for example the source of the data.
+
+[[additional-resources]]
+== Additional Resources
+
+A https://kaitai.io/[Kaitai Struct] definition for the data log format is included in link:./wpilog.ksy[wpilog.ksy].
+Kaitai Struct is a declarative language used to describe various binary data structures.
--- a/glass/src/lib/native/cpp/other/Field2D.cpp
+++ b/glass/src/lib/native/cpp/other/Field2D.cpp
@@ -224,8 +224,8 @@ class ObjectInfo {

 class FieldInfo {
 public:
-  static constexpr auto kDefaultWidth = 16.541052_m;
-  static constexpr auto kDefaultHeight = 8.211_m;
+  static constexpr auto kDefaultWidth = 17.5483_m;
+  static constexpr auto kDefaultHeight = 8.0519_m;

  explicit FieldInfo(Storage& storage);

@@ -345,7 +345,7 @@ static bool InputPose(frc::Pose2d* pose) {
 }

 FieldInfo::FieldInfo(Storage& storage)
-    : m_builtin{storage.GetString("builtin", "2024 Crescendo")},
+    : m_builtin{storage.GetString("builtin", "2025 Reefscape")},
      m_filename{storage.GetString("image")},
      m_width{storage.GetFloat("width", kDefaultWidth.to<float>())},
      m_height{storage.GetFloat("height", kDefaultHeight.to<float>())},
--- a/glass/src/libnt/native/cpp/NetworkTables.cpp
+++ b/glass/src/libnt/native/cpp/NetworkTables.cpp
@@ -359,7 +359,7 @@ static void UpdateProtobufValueSource(NetworkTablesModel& model,
                                      const google::protobuf::Message& msg,
                                      std::string_view name, int64_t time) {
  auto desc = msg.GetDescriptor();
-  out->typeStr = "proto:" + desc->full_name();
+  out->typeStr = fmt::format("proto:{}", desc->full_name());
  if (!out->valueChildrenMap ||
      desc->field_count() != static_cast<int>(out->valueChildren.size())) {
    out->valueChildren.clear();
--- a/ntcore/src/test/native/cpp/ValueTest.cpp
+++ b/ntcore/src/test/native/cpp/ValueTest.cpp
@@ -271,6 +271,8 @@ TEST_F(ValueTest, StringArray) {
  NT_DisposeValue(&cv);
 }

+// Google Test doesn't have ASSERT_DEATH when compiled with emscripten
+#ifndef __EMSCRIPTEN__
 #ifdef NDEBUG
 TEST_F(ValueDeathTest, DISABLED_GetAssertions) {
 #else
@@ -285,6 +287,7 @@ TEST_F(ValueDeathTest, GetAssertions) {
  ASSERT_DEATH((void)v.GetDoubleArray(), "type == NT_DOUBLE_ARRAY");
  ASSERT_DEATH((void)v.GetStringArray(), "type == NT_STRING_ARRAY");
 }
+#endif

 TEST_F(ValueTest, UnassignedComparison) {
  Value v1, v2;
--- a/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java
+++ b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java
@@ -25,6 +25,7 @@ public class RomiMotor extends PWMMotorController {
   * @param channel The PWM channel that the RomiMotor is attached to. 0 is the left motor, 1 is the
   *     right.
   */
+  @SuppressWarnings("this-escape")
  public RomiMotor(final int channel) {
    super("Romi Motor", channel);
    initRomiMotor();
--- a/shared/bazel/compiler_flags/osx_flags.rc
+++ b/shared/bazel/compiler_flags/osx_flags.rc
@@ -26,17 +26,6 @@ build:macos --conlyopt=-Wno-missing-field-initializers
 build:macos --conlyopt=-Wno-unused-private-field
 build:macos --conlyopt=-Wno-fixed-enum-extension"

-
-build:macos --linkopt=-framework
-build:macos --linkopt=CoreFoundation
-build:macos --linkopt=-framework
-build:macos --linkopt=AVFoundation
-build:macos --linkopt=-framework
-build:macos --linkopt=Foundation
-build:macos --linkopt=-framework
-build:macos --linkopt=CoreMedia
-build:macos --linkopt=-framework
-build:macos --linkopt=CoreVideo
 build:macos --linkopt=-headerpad_max_install_names
 build:macos --linkopt=-Wl,-rpath,'@loader_path'"

--- a/shared/examplecheck.gradle
+++ b/shared/examplecheck.gradle
@@ -103,6 +103,24 @@ task checkExamples(type: Task) {
    }
 }

+task checkSnippets(type: Task) {
+    doLast {
+        def parsedJson = new groovy.json.JsonSlurper().parseText(snippetsFile.text)
+        fileCheck(parsedJson, snippetsDirectory)
+        parsedJson.each {
+            assert it.name != null
+            assert it.description != null
+            assert it.tags != null
+            assert it.tags.findAll { !tagList.contains(it) }.empty
+            assert it.foldername != null
+            assert it.gradlebase != null
+            if (it.gradlebase == 'java') {
+                assert it.mainclass != null
+            }
+        }
+    }
+}
+
 task checkCommands(type: Task) {
    doLast {
        def parsedJson = new groovy.json.JsonSlurper().parseText(commandFile.text)
@@ -127,3 +145,4 @@ task checkCommands(type: Task) {
 check.dependsOn checkTemplates
 check.dependsOn checkExamples
 check.dependsOn checkCommands
+check.dependsOn checkSnippets
--- a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
+++ b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
@@ -139,6 +139,10 @@ void XRP::HandleServoSimValueChanged(const wpi::json& data) {
    deviceId = 4;
  } else if (data["device"] == "servo2") {
    deviceId = 5;
+  } else if (data["device"] == "servo3") {
+    deviceId = 6;
+  } else if (data["device"] == "servo4") {
+    deviceId = 7;
  }

  if (deviceId != -1 && servoData.find("<position") != servoData.end()) {
--- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
+++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
@@ -8,6 +8,7 @@

 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/system/LinearSystem.h>
+#include <frc/system/plant/LinearSystemId.h>
 #include <units/acceleration.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
@@ -18,6 +19,7 @@ using namespace sysid;

 using Kv_t = decltype(1_V / 1_mps);
 using Ka_t = decltype(1_V / 1_mps_sq);
+using Matrix1d = Eigen::Matrix<double, 1, 1>;

 FeedbackGains sysid::CalculatePositionFeedbackGains(
    const FeedbackControllerPreset& preset, const LQRParameters& params,
@@ -26,39 +28,32 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
    return {0.0, 0.0};
  }

-  // If acceleration requires no effort, velocity becomes an input for position
-  // control. We choose an appropriate model in this case to avoid numerical
+  // If acceleration for position control requires no effort, velocity becomes
+  // an input. We choose an appropriate model in this case to avoid numerical
  // instabilities in the LQR.
-  if (Ka > 1E-7) {
-    // Create a position system from our feedforward gains.
-    frc::LinearSystem<2, 1, 1> system{
-        frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}},
-        frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0},
-        frc::Matrixd<1, 1>{0.0}};
-    // Create an LQR with 2 states to control -- position and velocity.
-    frc::LinearQuadraticRegulator<2, 1> controller{
-        system, {params.qp, params.qv}, {params.r}, preset.period};
-    // Compensate for any latency from sensor measurements, filtering, etc.
+  if (std::abs(Ka) < 1e-7) {
+    // System has position state and velocity input
+    frc::LinearSystem<1, 1, 1> system{Matrix1d{0.0}, Matrix1d{1.0},
+                                      Matrix1d{1.0}, Matrix1d{0.0}};
+
+    frc::LinearQuadraticRegulator<1, 1> controller{
+        system, {params.qp}, {params.r}, preset.period};
    controller.LatencyCompensate(system, preset.period,
                                 preset.measurementDelay);

-    return {
-        controller.K(0, 0) * preset.outputConversionFactor,
-        controller.K(0, 1) * preset.outputConversionFactor /
-            (preset.normalized ? 1 : units::second_t{preset.period}.value())};
+    return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
  }

-  // This is our special model to avoid instabilities in the LQR.
-  auto system = frc::LinearSystem<1, 1, 1>(
-      Eigen::Matrix<double, 1, 1>{0.0}, Eigen::Matrix<double, 1, 1>{1.0},
-      Eigen::Matrix<double, 1, 1>{1.0}, Eigen::Matrix<double, 1, 1>{0.0});
-  // Create an LQR with one state -- position.
-  frc::LinearQuadraticRegulator<1, 1> controller{
-      system, {params.qp}, {params.r}, preset.period};
-  // Compensate for any latency from sensor measurements, filtering, etc.
+  auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meters>(
+      Kv_t{Kv}, Ka_t{Ka});
+
+  frc::LinearQuadraticRegulator<2, 1> controller{
+      system, {params.qp, params.qv}, {params.r}, preset.period};
  controller.LatencyCompensate(system, preset.period, preset.measurementDelay);

-  return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
+  return {controller.K(0, 0) * preset.outputConversionFactor,
+          controller.K(0, 1) * preset.outputConversionFactor /
+              (preset.normalized ? 1 : units::second_t{preset.period}.value())};
 }

 FeedbackGains sysid::CalculateVelocityFeedbackGains(
@@ -69,20 +64,16 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
  }

  // If acceleration for velocity control requires no effort, the feedback
-  // control gains approach zero. We special-case it here because numerical
-  // instabilities arise in LQR otherwise.
-  if (Ka < 1E-7) {
+  // control gains approach zero. We special-case it here to avoid numerical
+  // instabilities in LQR.
+  if (std::abs(Ka) < 1E-7) {
    return {0.0, 0.0};
  }

-  // Create a velocity system from our feedforward gains.
-  frc::LinearSystem<1, 1, 1> system{
-      frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka},
-      frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}};
-  // Create an LQR controller with 1 state -- velocity.
+  auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
+      Kv_t{Kv}, Ka_t{Ka});
  frc::LinearQuadraticRegulator<1, 1> controller{
      system, {params.qv}, {params.r}, preset.period};
-  // Compensate for any latency from sensor measurements, filtering, etc.
  controller.LatencyCompensate(system, preset.period, preset.measurementDelay);

  return {controller.K(0, 0) * preset.outputConversionFactor /
--- a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
+++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
@@ -7,7 +7,7 @@
 #include "sysid/analysis/FeedbackAnalysis.h"
 #include "sysid/analysis/FeedbackControllerPreset.h"

-TEST(FeedbackAnalysisTest, Velocity1) {
+TEST(FeedbackAnalysisTest, VelocitySystem1) {
  auto Kv = 3.060;
  auto Ka = 0.327;

@@ -20,7 +20,7 @@ TEST(FeedbackAnalysisTest, Velocity1) {
  EXPECT_NEAR(Kd, 0.00, 0.05);
 }

-TEST(FeedbackAnalysisTest, Velocity2) {
+TEST(FeedbackAnalysisTest, VelocitySystem2) {
  auto Kv = 0.0693;
  auto Ka = 0.1170;

@@ -33,6 +33,19 @@ TEST(FeedbackAnalysisTest, Velocity2) {
  EXPECT_NEAR(Kd, 0.00, 0.05);
 }

+TEST(FeedbackAnalysisTest, VelocitySystemWithSmallKa) {
+  auto Kv = 3.060;
+  auto Ka = 0.0;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 0.00, 0.05);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
 TEST(FeedbackAnalysisTest, VelocityConversion) {
  auto Kv = 0.0693;
  auto Ka = 0.1170;
@@ -117,6 +130,19 @@ TEST(FeedbackAnalysisTest, Position) {
  EXPECT_NEAR(Kd, 2.48, 0.05);
 }

+TEST(FeedbackAnalysisTest, PositionWithSmallKa) {
+  auto Kv = 3.060;
+  auto Ka = 1e-10;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 19.97, 0.05);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
 TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
  auto Kv = 3.060;
  auto Ka = 0.327;
--- a/upstream_utils/debugging_patches/0001-Guard-gnu-flatten-attribute.patch
+++ b/upstream_utils/debugging_patches/0001-Guard-gnu-flatten-attribute.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Thu, 21 Nov 2024 17:51:15 -0800
-Subject: [PATCH 1/4] Guard [[gnu::flatten]] attribute
+Subject: [PATCH 1/5] Guard [[gnu::flatten]] attribute

 ---
 include/debugging.hpp | 10 ++++++++--
--- a/upstream_utils/debugging_patches/0002-Remove-debugger_query-argument-from-Windows-and-macO.patch
+++ b/upstream_utils/debugging_patches/0002-Remove-debugger_query-argument-from-Windows-and-macO.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Thu, 21 Nov 2024 17:23:48 -0800
-Subject: [PATCH 2/4] Remove debugger_query argument from Windows and macOS
+Subject: [PATCH 2/5] Remove debugger_query argument from Windows and macOS

 ---
 src/macos.cxx   | 2 +-
--- a/upstream_utils/debugging_patches/0003-Fix-exception-mask-type-typo-on-macOS.patch
+++ b/upstream_utils/debugging_patches/0003-Fix-exception-mask-type-typo-on-macOS.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Thu, 21 Nov 2024 18:09:37 -0800
-Subject: [PATCH 3/4] Fix exception mask type typo on macOS
+Subject: [PATCH 3/5] Fix exception mask type typo on macOS

 ---
 src/macos.cxx | 2 +-
--- a/upstream_utils/debugging_patches/0004-Remove-NOMINMAX-macro-from-Windows.patch
+++ b/upstream_utils/debugging_patches/0004-Remove-NOMINMAX-macro-from-Windows.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Thu, 21 Nov 2024 18:49:53 -0800
-Subject: [PATCH 4/4] Remove NOMINMAX macro from Windows
+Subject: [PATCH 4/5] Remove NOMINMAX macro from Windows

 ---
 src/windows.cxx | 1 -
--- a/upstream_utils/debugging_patches/0005-Add-emscripten-shim.patch
+++ b/upstream_utils/debugging_patches/0005-Add-emscripten-shim.patch
@@ -0,0 +1,34 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Tue, 8 Apr 2025 15:28:38 -0700
+Subject: [PATCH 5/5] Add emscripten shim
+
+---
+ src/emscripten.cxx | 18 ++++++++++++++++++
+ 1 file changed, 18 insertions(+)
+ create mode 100644 src/emscripten.cxx
+
+diff --git a/src/emscripten.cxx b/src/emscripten.cxx
+new file mode 100644
+index 0000000000000000000000000000000000000000..16924894cc0c6085b27b33e6b9f2a6e6d582d116
+--- /dev/null
+++ b/src/emscripten.cxx
+@@ -0,0 +1,18 @@
+#if defined(__EMSCRIPTEN__)
+
+#	include <debugging.hpp>
+
+#	include <atomic>
+#	include <fstream>
+#	include <string>
+
+namespace wpi {
+
+bool is_debugger_present() noexcept
+{
+  return false;
+}
+
+} // namespace wpi
+
+#endif
--- a/upstream_utils/memory_patches/0001-Group-doxygen-into-memory-module.patch
+++ b/upstream_utils/memory_patches/0001-Group-doxygen-into-memory-module.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Ryan Blue <ryanzblue@gmail.com>
 Date: Wed, 21 Aug 2024 20:50:15 -0400
-Subject: [PATCH 1/2] Group doxygen into memory module
+Subject: [PATCH 1/3] Group doxygen into memory module

 ---
 .../foonathan/memory/aligned_allocator.hpp    |  2 +-
--- a/upstream_utils/memory_patches/0002-Remove-conflicting-doxygen-concept-alias.patch
+++ b/upstream_utils/memory_patches/0002-Remove-conflicting-doxygen-concept-alias.patch
@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Ryan Blue <ryanzblue@gmail.com>
 Date: Wed, 21 Aug 2024 21:47:32 -0400
-Subject: [PATCH 2/2] Remove conflicting doxygen 'concept' alias
+Subject: [PATCH 2/3] Remove conflicting doxygen 'concept' alias

 \concept was added as a doxygen command in 1.9.2 and is meant to be applied to concepts. Inserting them into standard comment paragraphs causes doxygen to interpret the following text as a concept name and add it to the documentation, as well as remove the text from the paragraph.
 In the upstream repo, this alias links to markdown documentation, so it's not usable for us anyways.
--- a/upstream_utils/memory_patches/0003-Fix-deprecation-warning-for-UDLs.patch
+++ b/upstream_utils/memory_patches/0003-Fix-deprecation-warning-for-UDLs.patch
@@ -0,0 +1,52 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Tue, 8 Apr 2025 15:30:06 -0700
+Subject: [PATCH 3/3] Fix deprecation warning for UDLs
+
+---
+ include/foonathan/memory/memory_arena.hpp | 12 ++++++------
+ 1 file changed, 6 insertions(+), 6 deletions(-)
+
+diff --git a/include/foonathan/memory/memory_arena.hpp b/include/foonathan/memory/memory_arena.hpp
+index eb969a677329b5b2d536f39f9a15817f040cf79f..d91f2a58cef56278cdb091daf34cebba7ec5b92c 100644
+--- a/include/foonathan/memory/memory_arena.hpp
+++ b/include/foonathan/memory/memory_arena.hpp
+@@ -656,32 +656,32 @@ namespace foonathan
+             /// \returns The number of bytes `value` is in the given unit.
+             /// \ingroup memory_core
+             /// @{
+-            constexpr std::size_t operator"" _KiB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_KiB(unsigned long long value) noexcept
+             {
+                 return std::size_t(value * 1024);
+             }
+ 
+-            constexpr std::size_t operator"" _KB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_KB(unsigned long long value) noexcept
+             {
+                 return std::size_t(value * 1000);
+             }
+ 
+-            constexpr std::size_t operator"" _MiB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_MiB(unsigned long long value) noexcept
+             {
+                 return std::size_t(value * 1024 * 1024);
+             }
+ 
+-            constexpr std::size_t operator"" _MB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_MB(unsigned long long value) noexcept
+             {
+                 return std::size_t(value * 1000 * 1000);
+             }
+ 
+-            constexpr std::size_t operator"" _GiB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_GiB(unsigned long long value) noexcept
+             {
+                 return std::size_t(value * 1024 * 1024 * 1024);
+             }
+ 
+-            constexpr std::size_t operator"" _GB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_GB(unsigned long long value) noexcept
+             {
+                 return std::size_t(value * 1000 * 1000 * 1000);
+             }
--- a/wpilibcExamples/CMakeLists.txt
+++ b/wpilibcExamples/CMakeLists.txt
@@ -5,6 +5,7 @@ include(SubDirList)

 subdir_list(TEMPLATES ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/templates)
 subdir_list(EXAMPLES ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/examples)
+subdir_list(SNIPPETS ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/snippets)

 add_custom_target(wpilibcExamples)
 add_custom_target(wpilibcExamples_test)
@@ -63,3 +64,16 @@ foreach(template ${TEMPLATES})
    target_link_libraries(${template} wpilibc wpilibNewCommands romiVendordep xrpVendordep)
    add_dependencies(wpilibcExamples ${template})
 endforeach()
+
+foreach(snippet ${SNIPPETS})
+    file(
+        GLOB_RECURSE sources
+        src/main/cpp/snippets/${snippet}/cpp/*.cpp
+        src/main/cpp/snippets/${snippet}/c/*.c
+    )
+    add_executable(snippet${snippet} ${sources})
+    wpilib_target_warnings(${snippet})
+    target_include_directories(snippet${snippet} PUBLIC src/main/cpp/snippets/${snippet}/include)
+    target_link_libraries(snippet${snippet} wpilibc wpilibNewCommands romiVendordep xrpVendordep)
+    add_dependencies(wpilibcExamples snippet${snippet})
+endforeach()
--- a/wpilibcExamples/build.gradle
+++ b/wpilibcExamples/build.gradle
@@ -14,9 +14,20 @@ apply from: "${rootDir}/shared/googletest.gradle"

 ext.examplesMap = [:]
 ext.templatesMap = [:]
+ext.snippetsMap = [:]

-File examplesTree = file("$projectDir/src/main/cpp/examples")
-examplesTree.list(new FilenameFilter() {
+ext {
+    templateDirectory = new File("$projectDir/src/main/cpp/templates/")
+    templateFile = new File("$projectDir/src/main/cpp/templates/templates.json")
+    exampleDirectory = new File("$projectDir/src/main/cpp/examples/")
+    exampleFile = new File("$projectDir/src/main/cpp/examples/examples.json")
+    commandDirectory = new File("$projectDir/src/main/cpp/commands/")
+    commandFile = new File("$projectDir/src/main/cpp/commands/commands.json")
+    snippetsDirectory = new File("$projectDir/src/main/cpp/snippets/")
+    snippetsFile = new File("$projectDir/src/main/cpp/snippets/snippets.json")
+}
+
+exampleDirectory.list(new FilenameFilter() {
            @Override
            public boolean accept(File current, String name) {
                return new File(current, name).isDirectory();
@@ -24,8 +35,7 @@ examplesTree.list(new FilenameFilter() {
        }).each {
            examplesMap.put(it, [])
        }
-File templatesTree = file("$projectDir/src/main/cpp/templates")
-templatesTree.list(new FilenameFilter() {
+templateDirectory.list(new FilenameFilter() {
            @Override
            public boolean accept(File current, String name) {
                return new File(current, name).isDirectory();
@@ -33,6 +43,14 @@ templatesTree.list(new FilenameFilter() {
        }).each {
            templatesMap.put(it, [])
        }
+snippetsDirectory.list(new FilenameFilter() {
+            @Override
+            public boolean accept(File current, String name) {
+                return new File(current, name).isDirectory();
+            }
+        }).each {
+            snippetsMap.put(it, [])
+        }

 nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.windowsx64).configure {
    linker.args.remove('/DEBUG:FULL')
@@ -41,7 +59,7 @@ nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.windowsx64).configur
 }

 ext {
-    sharedCvConfigs = examplesMap + templatesMap + [commands: []]
+    sharedCvConfigs = examplesMap + templatesMap + snippetsMap.collectEntries { key, value -> ['snippets' + key, value] } + [commands: []]
    staticCvConfigs = [:]
    useJava = false
    useCpp = true
@@ -165,6 +183,57 @@ model {
                }
            }
        }
+        snippetsMap.each { key, value ->
+            "snippets${key}"(NativeExecutableSpec) {
+                targetBuildTypes 'debug'
+                binaries.all { binary ->
+                    lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
+                    lib project: ':romiVendordep', library: 'romiVendordep', linkage: 'shared'
+                    lib project: ':xrpVendordep', library: 'xrpVendordep', linkage: 'shared'
+                    lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                    lib project: ':apriltag', library: 'apriltag', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                    project(':ntcore').addNtcoreDependency(binary, 'shared')
+                    lib project: ':cscore', library: 'cscore', linkage: 'shared'
+                    project(':hal').addHalDependency(binary, 'shared')
+                    lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+                    lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                    if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                        nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
+                    }
+                    if (binary.targetPlatform.name == getCurrentArch()) {
+                        simModules.each {
+                            lib project: ":simulation:$it", library: it, linkage: 'shared'
+                        }
+                    }
+                }
+                sources {
+                    cpp {
+                        source {
+                            srcDirs 'src/main/cpp/snippets/' + "${key}" + "/cpp"
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/main/cpp/snippets/' + "${key}" + "/include"
+                            include '**/*.h'
+                        }
+                    }
+                }
+                sources {
+                    c {
+                        source {
+                            srcDirs 'src/main/cpp/snippets/' + "${key}" + "/c"
+                            include '**/*.c'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/main/cpp/snippets/' + "${key}" + "/include"
+                            include '**/*.h'
+                        }
+                    }
+                }
+            }
+        }
    }
    testSuites {
        examplesMap.each { key, value ->
@@ -201,6 +270,41 @@ model {
            }
        }
    }
+    testSuites {
+        snippetsMap.each { key, value ->
+            def testFolder = new File("${rootDir}/wpilibcExamples/src/test/cpp/snippets/${key}")
+            if (testFolder.exists()) {
+                "snippets${key}Test"(GoogleTestTestSuiteSpec) {
+                    for (NativeComponentSpec c : $.components) {
+                        if (c.name == key) {
+                            testing c
+                            break
+                        }
+                    }
+                    sources {
+                        cpp {
+                            source {
+                                srcDirs "src/test/cpp/examples/${key}/cpp"
+                                include '**/*.cpp'
+                            }
+                            exportedHeaders {
+                                srcDirs "src/test/cpp/examples/${key}/include"
+                            }
+                        }
+                        c {
+                            source {
+                                srcDirs "src/test/cpp/examples/${key}/c"
+                                include '**/*.c'
+                            }
+                            exportedHeaders {
+                                srcDirs "src/test/cpp/examples/${key}/include"
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
    binaries {
        withType(GoogleTestTestSuiteBinarySpec) {
            lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
@@ -244,15 +348,6 @@ model {
    }
 }

-ext {
-    templateDirectory = new File("$projectDir/src/main/cpp/templates/")
-    templateFile = new File("$projectDir/src/main/cpp/templates/templates.json")
-    exampleDirectory = new File("$projectDir/src/main/cpp/examples/")
-    exampleFile = new File("$projectDir/src/main/cpp/examples/examples.json")
-    commandDirectory = new File("$projectDir/src/main/cpp/commands/")
-    commandFile = new File("$projectDir/src/main/cpp/commands/commands.json")
-}
-
 model {
    // Create run tasks for all examples.
    tasks {
--- a/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp
@@ -0,0 +1,40 @@
+// 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 <numbers>
+
+#include <frc/DutyCycleEncoder.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * DutyCycleEncoder snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  Robot() {}
+
+  void TeleopPeriodic() override {
+    // Gets the rotation
+    m_encoder.Get();
+
+    // Gets if the encoder is connected
+    m_encoder.IsConnected();
+  }
+
+ private:
+  // Initializes a duty cycle encoder on DIO pins 0
+  frc::DutyCycleEncoder m_encoder{0};
+
+  // Initializes a duty cycle encoder on DIO pins 0 to return a value of 4 for
+  // a full rotation, with the encoder reporting 0 half way through rotation (2
+  // out of 4)
+  frc::DutyCycleEncoder m_encoderFR{0, 4.0, 2.0};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
--- a/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp
@@ -0,0 +1,69 @@
+// 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 <numbers>
+
+#include <frc/Encoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <wpi/deprecated.h>
+
+/**
+ * Encoder snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html
+ */
+WPI_IGNORE_DEPRECATED
+class Robot : public frc::TimedRobot {
+ public:
+  Robot() {
+    // Configures the encoder to return a distance of 4 for every 256 pulses
+    // Also changes the units of getRate
+    m_encoder.SetDistancePerPulse(4.0 / 256.0);
+    // Configures the encoder to consider itself stopped after .1 seconds
+    m_encoder.SetMaxPeriod(0.1_s);
+    // Configures the encoder to consider itself stopped when its rate is below
+    // 10
+    m_encoder.SetMinRate(10);
+    // Reverses the direction of the encoder
+    m_encoder.SetReverseDirection(true);
+    // Configures an encoder to average its period measurement over 5 samples
+    // Can be between 1 and 127 samples
+    m_encoder.SetSamplesToAverage(5);
+  }
+
+  void TeleopPeriodic() override {
+    // Gets the distance traveled
+    m_encoder.GetDistance();
+
+    // Gets the current rate of the encoder
+    m_encoder.GetRate();
+
+    // Gets whether the encoder is stopped
+    m_encoder.GetStopped();
+
+    // Gets the last direction in which the encoder moved
+    m_encoder.GetDirection();
+
+    // Gets the current period of the encoder
+    m_encoder.GetPeriod();
+
+    // Resets the encoder to read a distance of zero
+    m_encoder.Reset();
+  }
+
+ private:
+  // Initializes an encoder on DIO pins 0 and 1
+  // Defaults to 4X decoding and non-inverted
+  frc::Encoder m_encoder{0, 1};
+
+  // Initializes an encoder on DIO pins 0 and 1
+  // 2X encoding and non-inverted
+  frc::Encoder m_encoder2x{0, 1, false, frc::Encoder::EncodingType::k2X};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
--- a/wpilibcExamples/src/main/cpp/snippets/snippets.json
+++ b/wpilibcExamples/src/main/cpp/snippets/snippets.json
@@ -0,0 +1,23 @@
+[
+  {
+    "name": "Encoder",
+    "description": "Snippets of Encoder class usage for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Encoder"
+    ],
+    "foldername": "Encoder",
+    "gradlebase": "cpp"
+  },
+  {
+    "name": "DutyCycleEncoder",
+    "description": "Snippets of DutyCycleEncoder class usage for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Encoder",
+      "Duty Cycle"
+    ],
+    "foldername": "DutyCycleEncoder",
+    "gradlebase": "cpp"
+  }
+]
--- a/wpilibjExamples/build.gradle
+++ b/wpilibjExamples/build.gradle
@@ -72,6 +72,8 @@ ext {
    exampleFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/examples.json")
    commandDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/commands/")
    commandFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/commands/commands.json")
+    snippetsDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/snippets/")
+    snippetsFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json")
 }

 apply plugin: 'cpp'
@@ -201,6 +203,47 @@ model {
                                    test.dependsOn(testTask)
                                }
                            }
+                            new groovy.json.JsonSlurper().parseText(snippetsFile.text).each { entry ->
+                                project.tasks.create("runSnippet${entry.foldername}", JavaExec) { run ->
+                                    run.group "run snippets"
+                                    run.mainClass = "edu.wpi.first.wpilibj.snippets." + entry.foldername + "." + entry.mainclass
+                                    run.classpath = sourceSets.main.runtimeClasspath
+                                    run.dependsOn it.tasks.install
+                                    run.systemProperty 'java.library.path', filePath
+                                    doFirst { doFirstTask(run) }
+
+                                    if (org.gradle.internal.os.OperatingSystem.current().isMacOsX()) {
+                                        run.jvmArgs = ['-XstartOnFirstThread']
+                                    }
+                                }
+                                project.tasks.create("testSnippet${entry.foldername}", Test) { testTask ->
+                                    testTask.group "verification"
+                                    testTask.useJUnitPlatform()
+                                    testTask.filter {
+                                        includeTestsMatching("edu.wpi.first.wpilibj.snippets.${entry.foldername}.*")
+                                        setFailOnNoMatchingTests(false)
+                                    }
+                                    test.filter {
+                                        excludeTestsMatching("edu.wpi.first.wpilibj.snippets.${entry.foldername}.*")
+                                        setFailOnNoMatchingTests(false)
+                                    }
+                                    testTask.testClassesDirs = sourceSets.test.output.classesDirs
+                                    testTask.classpath = sourceSets.test.runtimeClasspath
+                                    testTask.dependsOn it.tasks.install
+
+                                    testTask.systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+                                    testTask.testLogging {
+                                        events "failed"
+                                        exceptionFormat "full"
+                                    }
+                                    testTask.systemProperty 'java.library.path', filePath
+
+                                    if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64') || project.hasProperty('onlywindowsarm64')) {
+                                        testTask.enabled = false
+                                    }
+                                    test.dependsOn(testTask)
+                                }
+                            }

                            found = true
                        }
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Main.java
@@ -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.
+
+package edu.wpi.first.wpilibj.snippets.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Robot.java
@@ -0,0 +1,36 @@
+// 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.wpilibj.snippets.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+/**
+ * DutyCycleEncoder snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html
+ */
+public class Robot extends TimedRobot {
+  // Initializes a duty cycle encoder on DIO pins 0
+  DutyCycleEncoder m_encoder = new DutyCycleEncoder(0);
+
+  // Initializes a duty cycle encoder on DIO pins 0 to return a value of 4 for
+  // a full rotation, with the encoder reporting 0 half way through rotation (2
+  // out of 4)
+  DutyCycleEncoder m_encoderFR = new DutyCycleEncoder(0, 4.0, 2.0);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {}
+
+  @Override
+  public void teleopPeriodic() {
+    // Gets the rotation
+    m_encoder.get();
+
+    // Gets if the encoder is connected
+    m_encoder.isConnected();
+
+    m_encoderFR.get();
+  }
+}
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Main.java
@@ -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.
+
+package edu.wpi.first.wpilibj.snippets.encoder;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Robot.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.wpilibj.snippets.encoder;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+/**
+ * Encoder snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html
+ */
+@SuppressWarnings("deprecation")
+public class Robot extends TimedRobot {
+  // Initializes an encoder on DIO pins 0 and 1
+  // Defaults to 4X decoding and non-inverted
+  Encoder m_encoder = new Encoder(0, 1);
+
+  // Initializes an encoder on DIO pins 0 and 1
+  // 2X encoding and non-inverted
+  Encoder m_encoder2x = new Encoder(0, 1, false, Encoder.EncodingType.k2X);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    // Configures the encoder to return a distance of 4 for every 256 pulses
+    // Also changes the units of getRate
+    m_encoder.setDistancePerPulse(4.0 / 256.0);
+    // Configures the encoder to consider itself stopped after .1 seconds
+    m_encoder.setMaxPeriod(0.1);
+    // Configures the encoder to consider itself stopped when its rate is below 10
+    m_encoder.setMinRate(10);
+    // Reverses the direction of the encoder
+    m_encoder.setReverseDirection(true);
+    // Configures an encoder to average its period measurement over 5 samples
+    // Can be between 1 and 127 samples
+    m_encoder.setSamplesToAverage(5);
+
+    m_encoder2x.getRate();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Gets the distance traveled
+    m_encoder.getDistance();
+
+    // Gets the current rate of the encoder
+    m_encoder.getRate();
+
+    // Gets whether the encoder is stopped
+    m_encoder.getStopped();
+
+    // Gets the last direction in which the encoder moved
+    m_encoder.getDirection();
+
+    // Gets the current period of the encoder
+    m_encoder.getPeriod();
+
+    // Resets the encoder to read a distance of zero
+    m_encoder.reset();
+  }
+}
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json
@@ -0,0 +1,25 @@
+[
+  {
+    "name": "Encoder",
+    "description": "Snippets of Encoder class usage for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Encoder"
+    ],
+    "foldername": "encoder",
+    "gradlebase": "java",
+    "mainclass": "Main"
+  },
+  {
+    "name": "DutyCycleEncoder",
+    "description": "Snippets of DutyCycleEncoder class usage for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Encoder",
+      "Duty Cycle"
+    ],
+    "foldername": "dutycycleencoder",
+    "gradlebase": "java",
+    "mainclass": "Main"
+  }
+]
--- a/wpimath/algorithms.md
+++ b/wpimath/algorithms.md
@@ -92,7 +92,7 @@ Substitute these into the feedforward equation.
  uₖ = kₛ sgn(x) + kᵥxₖ₊₁
 ```

-Simplify the model when ka ≠ 0
+Simplify the model when kₐ ≠ 0

 ```
  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
@@ -107,6 +107,21 @@ where
  B_d = A⁻¹(eᴬᵀ - I)B
 ```

+When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method.
+
+```
+ [A  B]
+ [0  0]T   [A_d  B_d]
+e        = [ 0    I ]
+
+ [0  B]
+ [0  0]T   [1  BT]
+e        = [0   1]
+
+A_d = 1
+B_d = BT
+```
+
 ## Elevator feedforward

 ### Derivation
@@ -199,7 +214,7 @@ Substitute these into the feedforward equation.
  uₖ = kₛ sgn(x) + kg + kᵥxₖ₊₁
 ```

-Simplify the model when ka ≠ 0
+Simplify the model when kₐ ≠ 0

 ```
  uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
@@ -214,6 +229,21 @@ where
  B_d = A⁻¹(eᴬᵀ - I)B
 ```

+When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method.
+
+```
+ [A  B]
+ [0  0]T   [A_d  B_d]
+e        = [ 0    I ]
+
+ [0  B]
+ [0  0]T   [1  BT]
+e        = [0   1]
+
+A_d = 1
+B_d = BT
+```
+
 ## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I

 ### Derivation
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -4,6 +4,9 @@

 package edu.wpi.first.math;

+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+
 /** Math utility functions. */
 public final class MathUtil {
  private MathUtil() {
@@ -44,50 +47,51 @@ public final class MathUtil {
   * @return The value after the deadband is applied.
   */
  public static double applyDeadband(double value, double deadband, double maxMagnitude) {
-    if (Math.abs(value) > deadband) {
-      if (maxMagnitude / deadband > 1.0e12) {
-        // If max magnitude is sufficiently large, the implementation encounters
-        // roundoff error.  Implementing the limiting behavior directly avoids
-        // the problem.
-        return value > 0.0 ? value - deadband : value + deadband;
-      }
-      if (value > 0.0) {
-        // Map deadband to 0 and map max to max.
-        //
-        // y - y₁ = m(x - x₁)
-        // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
-        // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
-        //
-        // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
-        // x₁ = deadband
-        // y₁ = 0
-        // x₂ = max
-        // y₂ = max
-        //
-        // y = (max - 0)/(max - deadband) (x - deadband) + 0
-        // y = max/(max - deadband) (x - deadband)
-        // y = max (x - deadband)/(max - deadband)
-        return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
-      } else {
-        // Map -deadband to 0 and map -max to -max.
-        //
-        // y - y₁ = m(x - x₁)
-        // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
-        // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
-        //
-        // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
-        // x₁ = -deadband
-        // y₁ = 0
-        // x₂ = -max
-        // y₂ = -max
-        //
-        // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
-        // y = max/(max - deadband) (x + deadband)
-        // y = max (x + deadband)/(max - deadband)
-        return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
-      }
+    if (Math.abs(value) < deadband) {
+      return 0;
+    }
+    if (value > 0.0) {
+      // Map deadband to 0 and map max to max with a linear relationship.
+      //
+      //   y - y₁ = m(x - x₁)
+      //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+      //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+      //
+      // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
+      //
+      //   x₁ = deadband
+      //   y₁ = 0
+      //   x₂ = max
+      //   y₂ = max
+      //   y = (max - 0)/(max - deadband) (x - deadband) + 0
+      //   y = max/(max - deadband) (x - deadband)
+      //
+      // To handle high values of max, rewrite so that max only appears on the denominator.
+      //
+      //   y = ((max - deadband) + deadband)/(max - deadband) (x - deadband)
+      //   y = (1 + deadband/(max - deadband)) (x - deadband)
+      return (1 + deadband / (maxMagnitude - deadband)) * (value - deadband);
    } else {
-      return 0.0;
+      // Map -deadband to 0 and map -max to -max with a linear relationship.
+      //
+      //   y - y₁ = m(x - x₁)
+      //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+      //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+      //
+      // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
+      //
+      //   x₁ = -deadband
+      //   y₁ = 0
+      //   x₂ = -max
+      //   y₂ = -max
+      //   y = (-max - 0)/(-max + deadband) (x + deadband) + 0
+      //   y = max/(max - deadband) (x + deadband)
+      //
+      // To handle high values of max, rewrite so that max only appears on the denominator.
+      //
+      //   y = ((max - deadband) + deadband)/(max - deadband) (x + deadband)
+      //   y = (1 + deadband/(max - deadband)) (x + deadband)
+      return (1 + deadband / (maxMagnitude - deadband)) * (value + deadband);
    }
  }

@@ -209,4 +213,62 @@ public final class MathUtil {
    double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
    return Math.abs(error) < tolerance;
  }
+
+  /**
+   * Limits translation velocity.
+   *
+   * @param current Translation at current timestep.
+   * @param next Translation at next timestep.
+   * @param dt Timestep duration.
+   * @param maxVelocity Maximum translation velocity.
+   * @return Returns the next Translation2d limited to maxVelocity
+   */
+  public static Translation2d slewRateLimit(
+      Translation2d current, Translation2d next, double dt, double maxVelocity) {
+    if (maxVelocity < 0) {
+      Exception e = new IllegalArgumentException();
+      MathSharedStore.reportError(
+          "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
+      return next;
+    }
+    Translation2d diff = next.minus(current);
+    double dist = diff.getNorm();
+    if (dist < 1e-9) {
+      return next;
+    }
+    if (dist > maxVelocity * dt) {
+      // Move maximum allowed amount in direction of the difference
+      return current.plus(diff.times(maxVelocity * dt / dist));
+    }
+    return next;
+  }
+
+  /**
+   * Limits translation velocity.
+   *
+   * @param current Translation at current timestep.
+   * @param next Translation at next timestep.
+   * @param dt Timestep duration.
+   * @param maxVelocity Maximum translation velocity.
+   * @return Returns the next Translation3d limited to maxVelocity
+   */
+  public static Translation3d slewRateLimit(
+      Translation3d current, Translation3d next, double dt, double maxVelocity) {
+    if (maxVelocity < 0) {
+      Exception e = new IllegalArgumentException();
+      MathSharedStore.reportError(
+          "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
+      return next;
+    }
+    Translation3d diff = next.minus(current);
+    double dist = diff.getNorm();
+    if (dist < 1e-9) {
+      return next;
+    }
+    if (dist > maxVelocity * dt) {
+      // Move maximum allowed amount in direction of the difference
+      return current.plus(diff.times(maxVelocity * dt / dist));
+    }
+    return next;
+  }
 }
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -188,13 +188,13 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
   */
  public double calculate(double currentVelocity, double nextVelocity) {
    // See wpimath/algorithms.md#Elevator_feedforward for derivation
-    if (ka == 0.0) {
+    if (ka < 1e-9) {
      return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
    } else {
      double A = -kv / ka;
      double B = 1.0 / ka;
      double A_d = Math.exp(A * m_dt);
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
      return kg
          + ks * Math.signum(currentVelocity)
          + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -171,13 +171,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
   */
  public double calculate(double currentVelocity, double nextVelocity) {
    // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
-    if (ka == 0.0) {
+    if (ka < 1e-9) {
      return ks * Math.signum(nextVelocity) + kv * nextVelocity;
    } else {
      double A = -kv / ka;
      double B = 1.0 / ka;
      double A_d = Math.exp(A * m_dt);
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
      return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
    }
  }
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
@@ -69,8 +69,8 @@ public class MerweScaledSigmaPoints<S extends Num> {
  }

  /**
-   * Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P)
-   * of the filter.
+   * Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
+   * covariance (s) of the filter.
   *
   * @param x An array of the means.
   * @param s Square-root covariance of the filter.
@@ -86,6 +86,8 @@ public class MerweScaledSigmaPoints<S extends Num> {
    // 2 * states + 1 by states
    Matrix<S, ?> sigmas =
        new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+
+    // equation (17)
    sigmas.setColumn(0, x);
    for (int k = 0; k < m_states.getNum(); k++) {
      var xPlusU = x.plus(U.extractColumnVector(k));
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
@@ -35,9 +35,9 @@ import org.ejml.simple.SimpleMatrix;
 * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
 * chapter 9 "Stochastic control theory".
 *
- * <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). For more
- * information about the SR-UKF, see <a
- * href="https://www.researchgate.net/publication/3908304">https://www.researchgate.net/publication/3908304</a>.
+ * <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). The main reason for
+ * this is to guarantee that the covariance matrix remains positive definite. For more information
+ * about the SR-UKF, see https://www.researchgate.net/publication/3908304.
 *
 * @param <States> Number of states.
 * @param <Inputs> Number of inputs.
@@ -105,7 +105,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
  }

  /**
-   * Constructs an unscented Kalman filter with custom mean, residual, and addition functions. Using
+   * Constructs an Unscented Kalman filter with custom mean, residual, and addition functions. Using
   * 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.
   *
@@ -193,12 +193,21 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
          "Wc must be 2 * states + 1 by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
    }

-    // New mean is usually just the sum of the sigmas * weight:
-    //       n
-    // dot = Σ W[k] Xᵢ[k]
-    //      k=1
+    // New mean is usually just the sum of the sigmas * weights:
+    //
+    //      2n
+    //   x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
+    //      i=0
+    //
+    // equations (19) and (23) in the paper show this,
+    // but we allow a custom function, usually for angle wrapping
    Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);

+    // Form an intermediate matrix S⁻ as:
+    //
+    //   [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
+    //
+    // the part of equations (20) and (24) within the "qr{}"
    Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
    for (int i = 0; i < 2 * s.getNum(); i++) {
      Sbar.setColumn(
@@ -214,8 +223,24 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
      throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage);
    }

-    Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
-    newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false);
+    // Compute the square-root covariance of the sigma points
+    //
+    // We transpose S⁻ first because we formed it by horizontally
+    // concatenating each part; it should be vertical so we can take
+    // the QR decomposition as defined in the "QR Decomposition" passage
+    // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
+    //
+    // The resulting matrix R is the square-root covariance S, but it
+    // is upper triangular, so we need to transpose it.
+    //
+    // equations (20) and (24)
+    Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)).transpose());
+
+    // Update or downdate the square-root covariance with (𝒳₀-x̂)
+    // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
+    //
+    // equations (21) and (25)
+    newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), true);

    return new Pair<>(x, newS);
  }
@@ -256,7 +281,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
   */
  @Override
  public Matrix<States, States> getP() {
-    return m_S.transpose().times(m_S);
+    return m_S.times(m_S.transpose());
  }

  /**
@@ -280,7 +305,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
   */
  @Override
  public void setP(Matrix<States, States> newP) {
-    m_S = newP.lltDecompose(false);
+    m_S = newP.lltDecompose(true);
  }

  /**
@@ -347,14 +372,28 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    var discQ = Discretization.discretizeAQ(contA, m_contQ, dt).getSecond();
    var squareRootDiscQ = discQ.lltDecompose(true);

+    // Generate sigma points around the state mean
+    //
+    // equation (17)
    var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);

+    // Project each sigma point forward in time according to the
+    // dynamics f(x, u)
+    //
+    //   sigmas  = 𝒳ₖ₋₁
+    //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+    //
+    // equation (18)
    for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
      Matrix<States, N1> x = sigmas.extractColumnVector(i);

      m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dt));
    }

+    // Pass the predicted sigmas (𝒳) through the Unscented Transform
+    // to compute the prior state mean and covariance
+    //
+    // equations (18) (19) and (20)
    var ret =
        squareRootUnscentedTransform(
            m_states,
@@ -459,7 +498,15 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    final var discR = Discretization.discretizeR(R, m_dt);
    final var squareRootDiscR = discR.lltDecompose(true);

-    // Transform sigma points into measurement space
+    // Generate new sigma points from the prior mean and covariance
+    // and transform them into measurement space using h(x, u)
+    //
+    //   sigmas  = 𝒳
+    //   sigmasH = 𝒴
+    //
+    // This differs from equation (22) which uses
+    // the prior sigma points, regenerating them allows
+    // multiple measurement updates per time update
    Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
    var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
@@ -467,7 +514,11 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
      sigmasH.setColumn(i, hRet);
    }

-    // Mean and covariance of prediction passed through unscented transform
+    // Pass the predicted measurement sigmas through the Unscented Transform
+    // to compute the mean predicted measurement and square-root innovation
+    // covariance.
+    //
+    // equations (23) (24) and (25)
    var transRet =
        squareRootUnscentedTransform(
            m_states,
@@ -481,30 +532,54 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    var yHat = transRet.getFirst();
    var Sy = transRet.getSecond();

-    // Compute cross covariance of the state and the measurements
+    // Compute cross covariance of the predicted state and measurement sigma
+    // points given as:
+    //
+    //           2n
+    //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+    //           i=0
+    //
+    // equation (26)
    Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
-      // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
      var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat);
      var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose();

      Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
    }

-    // K = (P_{xy} / S_yᵀ) / S_y
-    // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
-    // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+    // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
+    // is equivalent to MATLAB's \ operator, so we need to rearrange to use
+    // that.
+    //
+    //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+    //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+    //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+    //
+    // equation (27)
    Matrix<States, R> K =
        Sy.transpose()
            .solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
            .transpose();

-    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
+    // Compute the posterior state mean
+    //
+    // x̂ = x̂⁻ + K(y − ŷ⁻)
+    //
+    // second part of equation (27)
    m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));

+    // Compute the intermediate matrix U for downdating
+    // the square-root covariance
+    //
+    // equation (28)
    Matrix<States, R> U = K.times(Sy);
+
+    // Downdate the posterior square-root state covariance
+    //
+    // equation (29)
    for (int i = 0; i < rows.getNum(); i++) {
-      m_S.rankUpdate(U.extractColumnVector(i), -1, false);
+      m_S.rankUpdate(U.extractColumnVector(i), -1, true);
    }
  }
 }
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
@@ -21,8 +21,8 @@ public class Debouncer {
    kBoth
  }

-  private final double m_debounceTime;
-  private final DebounceType m_debounceType;
+  private double m_debounceTime;
+  private DebounceType m_debounceType;
  private boolean m_baseline;

  private double m_prevTime;
@@ -40,11 +40,7 @@ public class Debouncer {

    resetTimer();

-    m_baseline =
-        switch (m_debounceType) {
-          case kBoth, kRising -> false;
-          case kFalling -> true;
-        };
+    m_baseline = m_debounceType == DebounceType.kFalling;
  }

  /**
@@ -86,4 +82,44 @@ public class Debouncer {
      return m_baseline;
    }
  }
+
+  /**
+   * Sets the time to debounce.
+   *
+   * @param time The number of seconds the value must change from baseline for the filtered value to
+   *     change.
+   */
+  public void setDebounceTime(double time) {
+    m_debounceTime = time;
+  }
+
+  /**
+   * Gets the time to debounce.
+   *
+   * @return The number of seconds the value must change from baseline for the filtered value to
+   *     change.
+   */
+  public double getDebounceTime() {
+    return m_debounceTime;
+  }
+
+  /**
+   * Sets the debounce type.
+   *
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  public void setDebounceType(DebounceType type) {
+    m_debounceType = type;
+
+    m_baseline = m_debounceType == DebounceType.kFalling;
+  }
+
+  /**
+   * Gets the debounce type.
+   *
+   * @return Which type of state change the debouncing will be performed on.
+   */
+  public DebounceType getDebounceType() {
+    return m_debounceType;
+  }
 }
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -42,7 +42,7 @@ public class TrapezoidProfile {
  private int m_direction;

  private final Constraints m_constraints;
-  private State m_current;
+  private State m_current = new State();

  private double m_endAccel;
  private double m_endFullSpeed;
@@ -186,7 +186,8 @@ public class TrapezoidProfile {
   * 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.
+   * @return The time left until a target distance in the profile is reached, or zero if no goal was
+   *     set.
   */
  public double timeLeftUntil(double target) {
    double position = m_current.position * m_direction;
@@ -252,7 +253,7 @@ public class TrapezoidProfile {
  /**
   * Returns the total time the profile takes to reach the goal.
   *
-   * @return The total time the profile takes to reach the goal.
+   * @return The total time the profile takes to reach the goal, or zero if no goal was set.
   */
  public double totalTime() {
    return m_endDecel;
--- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
+++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
@@ -64,8 +64,14 @@ units::volt_t ArmFeedforward::Calculate(
    sleipnir::Hessian hessianF{cost, xAD};
    Eigen::SparseMatrix<double> H = hessianF.Value();

-    double error = std::numeric_limits<double>::infinity();
-    while (error > 1e-8) {
+    double error_k = std::numeric_limits<double>::infinity();
+    double error_k1 = std::abs(g.coeff(0));
+
+    // Loop until error stops decreasing or max iterations is reached
+    for (size_t iteration = 0;
+         iteration < 50 && error_k1 < (1.0 - 1e-10) * error_k; ++iteration) {
+      error_k = error_k1;
+
      // Iterate via Newton's method.
      //
      //   xₖ₊₁ = xₖ − H⁻¹g
@@ -97,7 +103,7 @@ units::volt_t ArmFeedforward::Calculate(
      g = gradientF.Value();
      H = hessianF.Value();

-      error = std::abs(g.coeff(0));
+      error_k1 = std::abs(g.coeff(0));
    }
  }

--- a/wpimath/src/main/native/cpp/filter/Debouncer.cpp
+++ b/wpimath/src/main/native/cpp/filter/Debouncer.cpp
@@ -10,15 +10,7 @@ using namespace frc;

 Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
    : m_debounceTime(debounceTime), m_debounceType(type) {
-  switch (type) {
-    case DebounceType::kBoth:  // fall-through
-    case DebounceType::kRising:
-      m_baseline = false;
-      break;
-    case DebounceType::kFalling:
-      m_baseline = true;
-      break;
-  }
+  m_baseline = m_debounceType == DebounceType::kFalling;
  ResetTimer();
 }

--- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp
@@ -34,11 +34,12 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
                     slp::pow(y - rotPoint.Y().value(), 2));

    // (x − x_c)²/a² + (y − y_c)²/b² = 1
-    problem.SubjectTo(slp::pow(x - m_center.X().value(), 2) /
-                              (m_xSemiAxis.value() * m_xSemiAxis.value()) +
-                          slp::pow(y - m_center.Y().value(), 2) /
-                              (m_ySemiAxis.value() * m_ySemiAxis.value()) ==
-                      1);
+    // b²(x − x_c)² + a²(y − y_c)² = a²b²
+    double a2 = m_xSemiAxis.value() * m_xSemiAxis.value();
+    double b2 = m_ySemiAxis.value() * m_ySemiAxis.value();
+    problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) +
+                          a2 * slp::pow(y - m_center.Y().value(), 2) ==
+                      a2 * b2);

    problem.Solve();

--- a/wpimath/src/main/native/include/frc/MathUtil.h
+++ b/wpimath/src/main/native/include/frc/MathUtil.h
@@ -10,9 +10,15 @@
 #include <gcem.hpp>
 #include <wpi/SymbolExports.h>

+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
 #include "units/angle.h"
 #include "units/base.h"
+#include "units/length.h"
 #include "units/math.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "wpimath/MathShared.h"

 namespace frc {

@@ -37,51 +43,55 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
    magnitude = units::math::abs(value);
  }

-  if (magnitude > deadband) {
-    if (maxMagnitude / deadband > 1.0E12) {
-      // If max magnitude is sufficiently large, the implementation encounters
-      // roundoff error.  Implementing the limiting behavior directly avoids
-      // the problem.
-      return value > T{0.0} ? value - deadband : value + deadband;
-    }
-    if (value > T{0.0}) {
-      // Map deadband to 0 and map max to max.
-      //
-      // y - y₁ = m(x - x₁)
-      // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
-      // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
-      //
-      // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
-      // x₁ = deadband
-      // y₁ = 0
-      // x₂ = max
-      // y₂ = max
-      //
-      // y = (max - 0)/(max - deadband) (x - deadband) + 0
-      // y = max/(max - deadband) (x - deadband)
-      // y = max (x - deadband)/(max - deadband)
-      return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
-    } else {
-      // Map -deadband to 0 and map -max to -max.
-      //
-      // y - y₁ = m(x - x₁)
-      // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
-      // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
-      //
-      // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
-      // x₁ = -deadband
-      // y₁ = 0
-      // x₂ = -max
-      // y₂ = -max
-      //
-      // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
-      // y = max/(max - deadband) (x + deadband)
-      // y = max (x + deadband)/(max - deadband)
-      return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
-    }
-  } else {
+  if (magnitude < deadband) {
    return T{0.0};
  }
+
+  if (value > T{0.0}) {
+    // Map deadband to 0 and map max to max with a linear relationship.
+    //
+    //   y - y₁ = m(x - x₁)
+    //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+    //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+    //
+    // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
+    //
+    //   x₁ = deadband
+    //   y₁ = 0
+    //   x₂ = max
+    //   y₂ = max
+    //   y = (max - 0)/(max - deadband) (x - deadband) + 0
+    //   y = max/(max - deadband) (x - deadband)
+    //
+    // To handle high values of max, rewrite so that max only appears on the
+    // denominator.
+    //
+    //   y = ((max - deadband) + deadband)/(max - deadband) (x - deadband)
+    //   y = (1 + deadband/(max - deadband)) (x - deadband)
+    return (1.0 + deadband / (maxMagnitude - deadband)) * (value - deadband);
+  } else {
+    // Map -deadband to 0 and map -max to -max with a linear relationship.
+    //
+    //   y - y₁ = m(x - x₁)
+    //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+    //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+    //
+    // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
+    //
+    //   x₁ = -deadband
+    //   y₁ = 0
+    //   x₂ = -max
+    //   y₂ = -max
+    //   y = (-max - 0)/(-max + deadband) (x + deadband) + 0
+    //   y = max/(max - deadband) (x + deadband)
+    //
+    // To handle high values of max, rewrite so that max only appears on the
+    // denominator.
+    //
+    //   y = ((max - deadband) + deadband)/(max - deadband) (x + deadband)
+    //   y = (1 + deadband/(max - deadband)) (x + deadband)
+    return (1.0 + deadband / (maxMagnitude - deadband)) * (value + deadband);
+  }
 }

 /**
@@ -207,4 +217,65 @@ constexpr std::signed_integral auto FloorMod(std::signed_integral auto x,
                                             std::signed_integral auto y) {
  return x - FloorDiv(x, y) * y;
 }
+
+/**
+ * Limits translation velocity.
+ *
+ * @param current Translation at current timestep.
+ * @param next Translation at next timestep.
+ * @param dt Timestep duration.
+ * @param maxVelocity Maximum translation velocity.
+ * @return Returns the next Translation2d limited to maxVelocity
+ */
+constexpr Translation2d SlewRateLimit(const Translation2d& current,
+                                      const Translation2d& next,
+                                      units::second_t dt,
+                                      units::meters_per_second_t maxVelocity) {
+  if (maxVelocity < 0_mps) {
+    wpi::math::MathSharedStore::ReportError(
+        "maxVelocity must be a non-negative number, got {}!", maxVelocity);
+    return next;
+  }
+  Translation2d diff = next - current;
+  units::meter_t dist = diff.Norm();
+  if (dist < 1e-9_m) {
+    return next;
+  }
+  if (dist > maxVelocity * dt) {
+    // Move maximum allowed amount in direction of the difference
+    return current + diff * (maxVelocity * dt / dist);
+  }
+  return next;
+}
+
+/**
+ * Limits translation velocity.
+ *
+ * @param current Translation at current timestep.
+ * @param next Translation at next timestep.
+ * @param dt Timestep duration.
+ * @param maxVelocity Maximum translation velocity.
+ * @return Returns the next Translation3d limited to maxVelocity
+ */
+constexpr Translation3d SlewRateLimit(const Translation3d& current,
+                                      const Translation3d& next,
+                                      units::second_t dt,
+                                      units::meters_per_second_t maxVelocity) {
+  if (maxVelocity < 0_mps) {
+    wpi::math::MathSharedStore::ReportError(
+        "maxVelocity must be a non-negative number, got {}!", maxVelocity);
+    return next;
+  }
+  Translation3d diff = next - current;
+  units::meter_t dist = diff.Norm();
+  if (dist < 1e-9_m) {
+    return next;
+  }
+  if (dist > maxVelocity * dt) {
+    // Move maximum allowed amount in direction of the difference
+    return current + diff * (maxVelocity * dt / dist);
+  }
+  return next;
+}
+
 }  // namespace frc
--- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -133,13 +133,13 @@ class ElevatorFeedforward {
      units::unit_t<Velocity> currentVelocity,
      units::unit_t<Velocity> nextVelocity) const {
    // See wpimath/algorithms.md#Elevator_feedforward for derivation
-    if (kA == decltype(kA)(0)) {
+    if (kA < decltype(kA)(1e-9)) {
      return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
    } else {
      double A = -kV.value() / kA.value();
      double B = 1.0 / kA.value();
      double A_d = gcem::exp(A * m_dt.value());
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
      return kG + kS * wpi::sgn(currentVelocity) +
             units::volt_t{
                 1.0 / B_d *
--- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -93,13 +93,13 @@ class SimpleMotorFeedforward {
      units::unit_t<Velocity> currentVelocity,
      units::unit_t<Velocity> nextVelocity) const {
    // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
-    if (kA == decltype(kA)(0)) {
+    if (kA < decltype(kA)(1e-9)) {
      return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
    } else {
      double A = -kV.value() / kA.value();
      double B = 1.0 / kA.value();
      double A_d = gcem::exp(A * m_dt.value());
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
      return kS * wpi::sgn(currentVelocity) +
             units::volt_t{
                 1.0 / B_d *
--- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
+++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
@@ -51,7 +51,7 @@ class MerweScaledSigmaPoints {

  /**
   * Computes the sigma points for an unscented Kalman filter given the mean
-   * (x) and square-root covariance(S) of the filter.
+   * (x) and square-root covariance (S) of the filter.
   *
   * @param x An array of the means.
   * @param S Square-root covariance of the filter.
@@ -68,6 +68,8 @@ class MerweScaledSigmaPoints {
    Matrixd<States, States> U = eta * S;

    Matrixd<States, 2 * States + 1> sigmas;
+
+    // equation (17)
    sigmas.template block<States, 1>(0, 0) = x;
    for (int k = 0; k < States; ++k) {
      sigmas.template block<States, 1>(0, k + 1) =
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -43,7 +43,9 @@ namespace frc {
 * "Stochastic control theory".
 *
 * <p> This class implements a square-root-form unscented Kalman filter
- * (SR-UKF). For more information about the SR-UKF, see
+ * (SR-UKF). The main reason for this is to guarantee that the covariance
+ * matrix remains positive definite.
+ * For more information about the SR-UKF, see
 * https://www.researchgate.net/publication/3908304.
 *
 * @tparam States Number of states.
@@ -108,7 +110,7 @@ class UnscentedKalmanFilter {
  }

  /**
-   * Constructs an unscented Kalman filter with custom mean, residual, and
+   * Constructs an Unscented Kalman filter with custom mean, residual, and
   * addition functions. Using 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.
@@ -189,7 +191,7 @@ class UnscentedKalmanFilter {
  /**
   * Returns the reconstructed error covariance matrix P.
   */
-  StateMatrix P() const { return m_S.transpose() * m_S; }
+  StateMatrix P() const { return m_S * m_S.transpose(); }

  /**
   * Set the current square-root error covariance matrix S by taking the square
@@ -197,7 +199,7 @@ class UnscentedKalmanFilter {
   *
   * @param P The error covariance matrix P.
   */
-  void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
+  void SetP(const StateMatrix& P) { m_S = P.llt().matrixL(); }

  /**
   * Returns the state estimate x-hat.
@@ -252,14 +254,28 @@ class UnscentedKalmanFilter {
    DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
    Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);

+    // Generate sigma points around the state mean
+    //
+    // equation (17)
    Matrixd<States, 2 * States + 1> sigmas =
        m_pts.SquareRootSigmaPoints(m_xHat, m_S);

+    // Project each sigma point forward in time according to the
+    // dynamics f(x, u)
+    //
+    //   sigmas  = 𝒳ₖ₋₁
+    //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+    //
+    // equation (18)
    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
      StateVector x = sigmas.template block<States, 1>(0, i);
      m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
    }

+    // Pass the predicted sigmas (𝒳) through the Unscented Transform
+    // to compute the prior state mean and covariance
+    //
+    // equations (18) (19) and (20)
    auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
        m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
        discQ.template triangularView<Eigen::Lower>());
@@ -367,7 +383,15 @@ class UnscentedKalmanFilter {
    Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
    Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);

-    // Transform sigma points into measurement space
+    // Generate new sigma points from the prior mean and covariance
+    // and transform them into measurement space using h(x, u)
+    //
+    //   sigmas  = 𝒳
+    //   sigmasH = 𝒴
+    //
+    // This differs from equation (22) which uses
+    // the prior sigma points, regenerating them allows
+    // multiple measurement updates per time update
    Matrixd<Rows, 2 * States + 1> sigmasH;
    Matrixd<States, 2 * States + 1> sigmas =
        m_pts.SquareRootSigmaPoints(m_xHat, m_S);
@@ -376,16 +400,26 @@ class UnscentedKalmanFilter {
          h(sigmas.template block<States, 1>(0, i), u);
    }

-    // Mean and covariance of prediction passed through UT
+    // Pass the predicted measurement sigmas through the Unscented Transform
+    // to compute the mean predicted measurement and square-root innovation
+    // covariance.
+    //
+    // equations (23) (24) and (25)
    auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
        sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
        discR.template triangularView<Eigen::Lower>());

-    // Compute cross covariance of the state and the measurements
+    // Compute cross covariance of the predicted state and measurement sigma
+    // points given as:
+    //
+    //           2n
+    //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+    //           i=0
+    //
+    // equation (26)
    Matrixd<States, Rows> Pxy;
    Pxy.setZero();
    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
      Pxy +=
          m_pts.Wc(i) *
          (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
@@ -393,21 +427,39 @@ class UnscentedKalmanFilter {
              .transpose();
    }

-    // K = (P_{xy} / S_yᵀ) / S_y
-    // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
-    // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+    // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
+    // is equivalent to MATLAB's \ operator, so we need to rearrange to use
+    // that.
+    //
+    //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+    //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+    //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+    //
+    // equation (27)
    Matrixd<States, Rows> K =
        Sy.transpose()
            .fullPivHouseholderQr()
            .solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
            .transpose();

-    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
+    // Compute the posterior state mean
+    //
+    //   x̂ = x̂⁻ + K(y − ŷ⁻)
+    //
+    // second part of equation (27)
    m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));

+    // Compute the intermediate matrix U for downdating
+    // the square-root covariance
+    //
+    // equation (28)
    Matrixd<States, Rows> U = K * Sy;
+
+    // Downdate the posterior square-root state covariance
+    //
+    // equation (29)
    for (int i = 0; i < Rows; i++) {
-      Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+      Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
          m_S, U.template block<States, 1>(0, i), -1);
    }
  }
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -47,12 +47,21 @@ SquareRootUnscentedTransform(
                                  const Vectord<CovDim>&)>
        residualFunc,
    const Matrixd<CovDim, CovDim>& squareRootR) {
-  // New mean is usually just the sum of the sigmas * weight:
-  //       n
-  // dot = Σ W[k] Xᵢ[k]
-  //      k=1
+  // New mean is usually just the sum of the sigmas * weights:
+  //
+  //      2n
+  //   x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
+  //      i=0
+  //
+  // equations (19) and (23) in the paper show this,
+  // but we allow a custom function, usually for angle wrapping
  Vectord<CovDim> x = meanFunc(sigmas, Wm);

+  // Form an intermediate matrix S⁻ as:
+  //
+  //   [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
+  //
+  // the part of equations (20) and (24) within the "qr{}"
  Matrixd<CovDim, States * 2 + CovDim> Sbar;
  for (int i = 0; i < States * 2; i++) {
    Sbar.template block<CovDim, 1>(0, i) =
@@ -61,14 +70,29 @@ SquareRootUnscentedTransform(
  }
  Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;

-  // Merwe defines the QR decomposition as Aᵀ = QR
+  // Compute the square-root covariance of the sigma points.
+  //
+  // We transpose S⁻ first because we formed it by horizontally
+  // concatenating each part; it should be vertical so we can take
+  // the QR decomposition as defined in the "QR Decomposition" passage
+  // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
+  //
+  // The resulting matrix R is the square-root covariance S, but it
+  // is upper triangular, so we need to transpose it.
+  //
+  // equations (20) and (24)
  Matrixd<CovDim, CovDim> S = Sbar.transpose()
                                  .householderQr()
                                  .matrixQR()
                                  .template block<CovDim, CovDim>(0, 0)
-                                  .template triangularView<Eigen::Upper>();
+                                  .template triangularView<Eigen::Upper>()
+                                  .transpose();

-  Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+  // Update or downdate the square-root covariance with (𝒳₀-x̂)
+  // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
+  //
+  // equations (21) and (25)
+  Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
      S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);

  return std::make_tuple(x, S);
--- a/wpimath/src/main/native/include/frc/filter/Debouncer.h
+++ b/wpimath/src/main/native/include/frc/filter/Debouncer.h
@@ -48,6 +48,42 @@ class WPILIB_DLLEXPORT Debouncer {
   */
  bool Calculate(bool input);

+  /**
+   * Sets the time to debounce.
+   *
+   * @param time The number of seconds the value must change from baseline
+   *             for the filtered value to change.
+   */
+  constexpr void SetDebounceTime(units::second_t time) {
+    m_debounceTime = time;
+  }
+
+  /**
+   * Gets the time to debounce.
+   *
+   * @return The number of seconds the value must change from baseline
+   *             for the filtered value to change.
+   */
+  constexpr units::second_t GetDebounceTime() const { return m_debounceTime; }
+
+  /**
+   * Set the debounce type.
+   *
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  constexpr void SetDebounceType(DebounceType type) {
+    m_debounceType = type;
+
+    m_baseline = m_debounceType == DebounceType::kFalling;
+  }
+
+  /**
+   * Get the debounce type.
+   *
+   * @return Which type of state change the debouncing will be performed on.
+   */
+  constexpr DebounceType GetDebounceType() const { return m_debounceType; }
+
 private:
  units::second_t m_debounceTime;
  bool m_baseline;
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -198,7 +198,8 @@ class TrapezoidProfile {
   * 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.
+   * @return The time left until a target distance in the profile is reached, or
+   * zero if no goal was set.
   */
  constexpr units::second_t TimeLeftUntil(Distance_t target) const {
    Distance_t position = m_current.position * m_direction;
@@ -269,7 +270,8 @@ class TrapezoidProfile {
  /**
   * Returns the total time the profile takes to reach the goal.
   *
-   * @return The total time the profile takes to reach the goal.
+   * @return The total time the profile takes to reach the goal, or zero if no
+   * goal was set.
   */
  constexpr units::second_t TotalTime() const { return m_endDecel; }

@@ -309,14 +311,14 @@ class TrapezoidProfile {
  }

  // The direction of the profile, either 1 for forwards or -1 for inverted
-  int m_direction;
+  int m_direction = 1;

  Constraints m_constraints;
  State m_current;

-  units::second_t m_endAccel;
-  units::second_t m_endFullSpeed;
-  units::second_t m_endDecel;
+  units::second_t m_endAccel = 0_s;
+  units::second_t m_endFullSpeed = 0_s;
+  units::second_t m_endDecel = 0_s;
 };

 }  // namespace frc
--- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
@@ -8,6 +8,8 @@ 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.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.UtilityClassTest;
 import org.junit.jupiter.api.Test;

@@ -167,4 +169,55 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
    assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
    assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
  }
+
+  @Test
+  void testSlewRateTranslation2dUnchanged() {
+    Translation2d translation1 = new Translation2d(0, 0);
+    Translation2d translation2 = new Translation2d(2, 2);
+
+    Translation2d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50);
+
+    Translation2d expected1 = new Translation2d(2, 2);
+
+    assertEquals(expected1, result1);
+  }
+
+  @Test
+  void testSlewRateTranslation2dChanged() {
+    Translation2d translation3 = new Translation2d(1, 1);
+    Translation2d translation4 = new Translation2d(3, 3);
+
+    Translation2d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2);
+
+    Translation2d expected2 =
+        new Translation2d(1.0 + 1.0 / Math.sqrt(8.0), 1.0 + 1.0 / Math.sqrt(8.0));
+
+    assertEquals(expected2, result2);
+  }
+
+  @Test
+  void testSlewRateTranslation3dUnchanged() {
+    Translation3d translation1 = new Translation3d(0, 0, 0);
+    Translation3d translation2 = new Translation3d(2, 2, 2);
+
+    Translation3d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50);
+
+    Translation3d expected1 = new Translation3d(2, 2, 2);
+
+    assertEquals(expected1, result1);
+  }
+
+  @Test
+  void testSlewRateTranslation3dChanged() {
+    Translation3d translation3 = new Translation3d(1, 1, 1);
+    Translation3d translation4 = new Translation3d(3, 3, 3);
+
+    Translation3d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2);
+
+    Translation3d expected2 =
+        new Translation3d(
+            1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0));
+
+    assertEquals(expected2, result2);
+  }
 }
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
@@ -18,15 +18,13 @@ import java.util.function.BiFunction;
 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);
-
  /**
   * Simulates a single-jointed arm and returns the final state.
   *
+   * @param ks The static gain, in volts.
+   * @param kv The velocity gain, in volt seconds per radian.
+   * @param ka The acceleration gain, in volt seconds² per radian.
+   * @param kg The gravity gain, in volts.
   * @param currentAngle The starting angle in radians.
   * @param currentVelocity The starting angular velocity in radians per second.
   * @param input The input voltage.
@@ -34,7 +32,14 @@ class ArmFeedforwardTest {
   * @return The final state as a 2-vector of angle and angular velocity.
   */
  private Matrix<N2, N1> simulate(
-      double currentAngle, double currentVelocity, double input, double dt) {
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double input,
+      double dt) {
    final Matrix<N2, N2> A =
        new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 1.0, 0.0, -kv / ka});
    final Matrix<N2, N1> B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0.0, 1.0 / ka});
@@ -61,46 +66,115 @@ class ArmFeedforwardTest {
   * Calculates a feedforward voltage using overload taking angle, two angular velocities, and dt;
   * then simulates an arm using that voltage to verify correctness.
   *
+   * @param armFF The feedforward object.
+   * @param ks The static gain, in volts.
+   * @param kv The velocity gain, in volt seconds per radian.
+   * @param ka The acceleration gain, in volt seconds² per radian.
+   * @param kg The gravity gain, in volts.
   * @param currentAngle The starting angle in radians.
   * @param currentVelocity The starting angular velocity in radians per second.
   * @param input The input voltage.
   * @param dt The simulation time in seconds.
   */
  private void calculateAndSimulate(
-      double currentAngle, double currentVelocity, double nextVelocity, double dt) {
-    final double input = m_armFF.calculate(currentAngle, currentVelocity, nextVelocity);
-    assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
+      ArmFeedforward armFF,
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double nextVelocity,
+      double dt) {
+    final double input = armFF.calculate(currentAngle, currentVelocity, nextVelocity);
+    assertEquals(
+        nextVelocity,
+        simulate(ks, kv, ka, kg, currentAngle, currentVelocity, input, dt).get(1, 0),
+        1e-4);
  }

  @Test
  void testCalculate() {
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
    // calculate(angle, angular velocity)
-    assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002);
-    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002);
+    assertEquals(0.5, armFF.calculate(Math.PI / 3, 0.0), 0.002);
+    assertEquals(2.5, armFF.calculate(Math.PI / 3, 1.0), 0.002);

    // calculate(currentAngle, currentVelocity, nextAngle, dt)
-    calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);
-    calculateAndSimulate(Math.PI / 3, 1.0, 0.95, 0.020);
-    calculateAndSimulate(-Math.PI / 3, 1.0, 1.05, 0.020);
-    calculateAndSimulate(-Math.PI / 3, 1.0, 0.95, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 1.05, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 0.95, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 1.05, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 0.95, 0.020);
+  }
+
+  @Test
+  void testCalculateIllConditionedModel() {
+    final double ks = 0.39671;
+    final double kv = 2.7167;
+    final double ka = 1e-2;
+    final double kg = 0.2708;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    final double currentAngle = 1.0;
+    final double currentVelocity = 0.02;
+    final double nextVelocity = 0.0;
+
+    var averageAccel = (nextVelocity - currentVelocity) / 0.02;
+
+    assertEquals(
+        armFF.calculate(currentAngle, currentVelocity, nextVelocity),
+        ks + kv * currentVelocity + ka * averageAccel + kg * Math.cos(currentAngle));
+  }
+
+  @Test
+  void testCalculateIllConditionedGradient() {
+    final double ks = 0.39671;
+    final double kv = 2.7167;
+    final double ka = 0.50799;
+    final double kg = 0.2708;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    calculateAndSimulate(armFF, ks, kv, ka, kg, 1.0, 0.02, 0.0, 0.02);
  }

  @Test
  void testAchievableVelocity() {
-    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);
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    assertEquals(6, armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-9, armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
  }

  @Test
  void testAchievableAcceleration() {
-    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);
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    assertEquals(4.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(6.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+    assertEquals(-7.25, armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-5.25, armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
  }

  @Test
  void testNegativeGains() {
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+
    assertAll(
        () ->
            assertThrows(IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, -kv, ka)),
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
@@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;

 import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -18,6 +19,7 @@ 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.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
@@ -26,11 +28,13 @@ import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import java.util.Collections;
 import java.util.List;
 import org.junit.jupiter.api.Test;

 class UnscentedKalmanFilterTest {
-  private static Matrix<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
    var motors = DCMotor.getCIM(2);

    // var gLow = 15.32;    // Low gear ratio
@@ -60,17 +64,17 @@ class UnscentedKalmanFilterTest {
  }

  @SuppressWarnings("PMD.UnusedFormalParameter")
-  private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N3, N1> driveLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
  }

  @SuppressWarnings("PMD.UnusedFormalParameter")
-  private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
    return x.copy();
  }

  @Test
-  void testInit() {
+  void testDriveInit() {
    var dt = 0.005;
    assertDoesNotThrow(
        () -> {
@@ -78,8 +82,8 @@ class UnscentedKalmanFilterTest {
              new UnscentedKalmanFilter<>(
                  Nat.N5(),
                  Nat.N3(),
-                  UnscentedKalmanFilterTest::getDynamics,
-                  UnscentedKalmanFilterTest::getLocalMeasurementModel,
+                  UnscentedKalmanFilterTest::driveDynamics,
+                  UnscentedKalmanFilterTest::driveLocalMeasurementModel,
                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
                  VecBuilder.fill(0.0001, 0.01, 0.01),
                  AngleStatistics.angleMean(2),
@@ -92,10 +96,10 @@ class UnscentedKalmanFilterTest {
          var u = VecBuilder.fill(12.0, 12.0);
          observer.predict(u, dt);

-          var localY = getLocalMeasurementModel(observer.getXhat(), u);
+          var localY = driveLocalMeasurementModel(observer.getXhat(), u);
          observer.correct(u, localY);

-          var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+          var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
          var R =
              StateSpaceUtil.makeCovarianceMatrix(
                  Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
@@ -103,7 +107,7 @@ class UnscentedKalmanFilterTest {
              Nat.N5(),
              u,
              globalY,
-              UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+              UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
              R,
              AngleStatistics.angleMean(2),
              AngleStatistics.angleResidual(2),
@@ -113,16 +117,16 @@ class UnscentedKalmanFilterTest {
  }

  @Test
-  void testConvergence() {
-    double dt = 0.005;
-    double rb = 0.8382 / 2.0; // Robot radius
+  void testDriveConvergence() {
+    final double dt = 0.005;
+    final double rb = 0.8382 / 2.0; // Robot radius

    UnscentedKalmanFilter<N5, N2, N3> observer =
        new UnscentedKalmanFilter<>(
            Nat.N5(),
            Nat.N3(),
-            UnscentedKalmanFilterTest::getDynamics,
-            UnscentedKalmanFilterTest::getLocalMeasurementModel,
+            UnscentedKalmanFilterTest::driveDynamics,
+            UnscentedKalmanFilterTest::driveLocalMeasurementModel,
            VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
            VecBuilder.fill(0.0001, 0.5, 0.5),
            AngleStatistics.angleMean(2),
@@ -146,7 +150,7 @@ class UnscentedKalmanFilterTest {
        NumericalJacobian.numericalJacobianU(
            Nat.N5(),
            Nat.N2(),
-            UnscentedKalmanFilterTest::getDynamics,
+            UnscentedKalmanFilterTest::driveDynamics,
            new Matrix<>(Nat.N5(), Nat.N1()),
            new Matrix<>(Nat.N2(), Nat.N1()));

@@ -161,7 +165,7 @@ class UnscentedKalmanFilterTest {
    var trueXhat = observer.getXhat();

    double totalTime = trajectory.getTotalTime();
-    for (int i = 0; i < (totalTime / dt); i++) {
+    for (int i = 0; i < (totalTime / dt); ++i) {
      var ref = trajectory.sample(dt * i);
      double vl = ref.velocity * (1 - (ref.curvature * rb));
      double vr = ref.velocity * (1 + (ref.curvature * rb));
@@ -174,24 +178,26 @@ class UnscentedKalmanFilterTest {
              vl,
              vr);

-      Matrix<N3, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+      Matrix<N3, N1> localY =
+          driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
      var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);

      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));

      var rdot = nextR.minus(r).div(dt);
-      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+      u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));

      observer.predict(u, dt);

      r = nextR;
-      trueXhat = NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dt);
+      trueXhat =
+          NumericalIntegration.rk4(UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dt);
    }

-    var localY = getLocalMeasurementModel(trueXhat, u);
+    var localY = driveLocalMeasurementModel(trueXhat, u);
    observer.correct(u, localY);

-    var globalY = getGlobalMeasurementModel(trueXhat, u);
+    var globalY = driveGlobalMeasurementModel(trueXhat, u);
    var R =
        StateSpaceUtil.makeCovarianceMatrix(
            Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
@@ -199,7 +205,7 @@ class UnscentedKalmanFilterTest {
        Nat.N5(),
        u,
        globalY,
-        UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+        UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
        R,
        AngleStatistics.angleMean(2),
        AngleStatistics.angleResidual(2),
@@ -236,7 +242,7 @@ class UnscentedKalmanFilterTest {
    Matrix<N1, N1> ref = VecBuilder.fill(100);
    Matrix<N1, N1> u = VecBuilder.fill(0);

-    for (int i = 0; i < (2.0 / dt); i++) {
+    for (int i = 0; i < (2.0 / dt); ++i) {
      observer.predict(u, dt);

      u = discB.solve(ref.minus(discA.times(ref)));
@@ -264,4 +270,125 @@ class UnscentedKalmanFilterTest {

    assertTrue(observer.getP().isEqual(P, 1e-9));
  }
+
+  // Second system, single motor feedforward estimator
+  private static Matrix<N4, N1> motorDynamics(Matrix<N4, N1> x, Matrix<N1, N1> u) {
+    double v = x.get(1, 0);
+    double kV = x.get(2, 0);
+    double kA = x.get(3, 0);
+
+    double V = u.get(0, 0);
+
+    double a = -kV / kA * v + 1.0 / kA * V;
+    return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0);
+  }
+
+  private static Matrix<N3, N1> motorMeasurementModel(Matrix<N4, N1> x, Matrix<N1, N1> u) {
+    double p = x.get(0, 0);
+    double v = x.get(1, 0);
+    double kV = x.get(2, 0);
+    double kA = x.get(3, 0);
+    double V = u.get(0, 0);
+
+    double a = -kV / kA * v + 1.0 / kA * V;
+    return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a);
+  }
+
+  private static Matrix<N1, N1> motorControlInput(double t) {
+    return MatBuilder.fill(
+        Nat.N1(),
+        Nat.N1(),
+        MathUtil.clamp(
+            8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+                + 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+                + 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),
+            -12.0,
+            12.0));
+  }
+
+  @Test
+  void testMotorConvergence() {
+    final double dt = 0.01;
+    final int steps = 500;
+    final double true_kV = 3;
+    final double true_kA = 0.2;
+
+    final double pos_stddev = 0.02;
+    final double vel_stddev = 0.1;
+    final double accel_stddev = 0.1;
+
+    var states =
+        new ArrayList<>(
+            Collections.nCopies(
+                steps + 1, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 0.0, 0.0)));
+    var inputs =
+        new ArrayList<>(Collections.nCopies(steps, MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0)));
+    var measurements =
+        new ArrayList<>(
+            Collections.nCopies(steps + 1, MatBuilder.fill(Nat.N3(), Nat.N1(), 0.0, 0.0, 0.0)));
+    states.set(0, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, true_kV, true_kA));
+
+    var A =
+        MatBuilder.fill(
+            Nat.N4(),
+            Nat.N4(),
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            -true_kV / true_kA,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0);
+    var B = MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 1.0 / true_kA, 0.0, 0.0);
+
+    var discABPair = Discretization.discretizeAB(A, B, dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    for (int i = 0; i < steps; ++i) {
+      inputs.set(i, motorControlInput(i * dt));
+      states.set(i + 1, discA.times(states.get(i)).plus(discB.times(inputs.get(i))));
+      measurements.set(
+          i,
+          motorMeasurementModel(states.get(i + 1), inputs.get(i))
+              .plus(
+                  StateSpaceUtil.makeWhiteNoiseVector(
+                      VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
+    }
+
+    var P0 =
+        MatBuilder.fill(
+            Nat.N4(), Nat.N4(), 0.001, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0,
+            0.0, 0.0, 10);
+
+    var observer =
+        new UnscentedKalmanFilter<N4, N1, N3>(
+            Nat.N4(),
+            Nat.N3(),
+            UnscentedKalmanFilterTest::motorDynamics,
+            UnscentedKalmanFilterTest::motorMeasurementModel,
+            VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
+            VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
+            dt);
+
+    observer.setXhat(MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 2.0, 2.0));
+    observer.setP(P0);
+
+    for (int i = 0; i < steps; ++i) {
+      observer.predict(inputs.get(i), dt);
+      observer.correct(inputs.get(i), measurements.get(i));
+    }
+
+    assertEquals(true_kV, observer.getXhat(2), true_kV * 0.5);
+    assertEquals(true_kA, observer.getXhat(3), true_kA * 0.5);
+  }
 }
--- a/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java
@@ -4,7 +4,9 @@

 package edu.wpi.first.math.filter;

+import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertSame;
 import static org.junit.jupiter.api.Assertions.assertTrue;

 import edu.wpi.first.util.WPIUtilJNI;
@@ -67,4 +69,21 @@ class DebouncerTest {

    assertFalse(debouncer.calculate(false));
  }
+
+  @Test
+  void debounceParamsTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
+
+    assertEquals(debouncer.getDebounceTime(), 0.02);
+    assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kBoth);
+
+    debouncer.setDebounceTime(0.1);
+
+    assertEquals(debouncer.getDebounceTime(), 0.1);
+
+    debouncer.setDebounceType(Debouncer.DebounceType.kFalling);
+
+    assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kFalling);
+    assertTrue(debouncer.calculate(false));
+  }
 }
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
@@ -246,4 +246,11 @@ class TrapezoidProfileTest {
      }
    }
  }
+
+  @Test
+  void initalizationOfCurrentState() {
+    var profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(1, 1));
+    assertNear(profile.timeLeftUntil(0), 0, 1e-10);
+    assertNear(profile.totalTime(), 0, 1e-10);
+  }
 }
--- a/wpimath/src/test/native/cpp/MathUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp
@@ -3,11 +3,17 @@
 // the WPILib BSD license file in the root directory of this project.

 #include <limits>
+#include <numbers>

 #include <gtest/gtest.h>

 #include "frc/MathUtil.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
 #include "units/angle.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"

 #define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())

@@ -164,3 +170,56 @@ TEST(MathUtilTest, IsNear) {
  EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
  EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
 }
+
+TEST(MathUtilTest, Translation2dSlewRateLimitUnchanged) {
+  const frc::Translation2d translation1{0_m, 0_m};
+  const frc::Translation2d translation2{2_m, 2_m};
+
+  const frc::Translation2d result1 =
+      frc::SlewRateLimit(translation1, translation2, 1_s, 50_mps);
+
+  const frc::Translation2d expected1{2_m, 2_m};
+
+  EXPECT_EQ(result1, expected1);
+}
+
+TEST(MathUtilTest, Translation2dSlewRateLimitChanged) {
+  const frc::Translation2d translation3{1_m, 1_m};
+  const frc::Translation2d translation4{3_m, 3_m};
+
+  const frc::Translation2d result2 =
+      frc::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps);
+
+  const frc::Translation2d expected2{
+      units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)},
+      units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}};
+
+  EXPECT_EQ(result2, expected2);
+}
+
+TEST(MathUtilTest, Translation3dSlewRateLimitUnchanged) {
+  const frc::Translation3d translation1{0_m, 0_m, 0_m};
+  const frc::Translation3d translation2{2_m, 2_m, 2_m};
+
+  const frc::Translation3d result1 =
+      frc::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps);
+
+  const frc::Translation3d expected1{2_m, 2_m, 2_m};
+
+  EXPECT_EQ(result1, expected1);
+}
+
+TEST(MathUtilTest, Translation3dSlewRateLimitChanged) {
+  const frc::Translation3d translation3{1_m, 1_m, 1_m};
+  const frc::Translation3d translation4{3_m, 3_m, 3_m};
+
+  const frc::Translation3d result2 =
+      frc::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps);
+
+  const frc::Translation3d expected2{
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}};
+
+  EXPECT_EQ(result2, expected2);
+}
--- a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
@@ -15,27 +15,32 @@
 #include "units/time.h"
 #include "units/voltage.h"

-static constexpr auto Ks = 0.5_V;
-static constexpr auto Kv = 1.5_V / 1_rad_per_s;
-static constexpr auto Ka = 2_V / 1_rad_per_s_sq;
-static constexpr auto Kg = 1_V;
-
 namespace {

+using Ks_unit = decltype(1_V);
+using Kv_unit = decltype(1_V / 1_rad_per_s);
+using Ka_unit = decltype(1_V / 1_rad_per_s_sq);
+using Kg_unit = decltype(1_V);
+
 /**
 * Simulates a single-jointed arm and returns the final state.
 *
+ * @param Ks The static gain, in volts.
+ * @param Kv The velocity gain, in volt seconds per radian.
+ * @param Ka The acceleration gain, in volt seconds² per radian.
+ * @param Kg The gravity gain, in volts.
 * @param currentAngle The starting angle.
 * @param currentVelocity The starting angular velocity.
 * @param input The input voltage.
 * @param dt The simulation time.
 * @return The final state as a 2-vector of angle and angular velocity.
 */
-frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
+frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
+                            units::radian_t currentAngle,
                            units::radians_per_second_t currentVelocity,
                            units::volt_t input, units::second_t dt) {
-  constexpr frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
-  constexpr frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
+  frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
+  frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};

  return frc::RK4(
      [&](const frc::Matrixd<2, 1>& x,
@@ -52,24 +57,35 @@ frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
 * Simulates a single-jointed arm and returns the final state.
 *
 * @param armFF The feedforward object.
+ * @param Ks The static gain, in volts.
+ * @param Kv The velocity gain, in volt seconds per radian.
+ * @param Ka The acceleration gain, in volt seconds² per radian.
+ * @param Kg The gravity gain, in volts.
 * @param currentAngle The starting angle.
 * @param currentVelocity The starting angular velocity.
 * @param input The input voltage.
 * @param dt The simulation time.
 */
-void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
+void CalculateAndSimulate(const frc::ArmFeedforward& armFF, Ks_unit Ks,
+                          Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
                          units::radian_t currentAngle,
                          units::radians_per_second_t currentVelocity,
                          units::radians_per_second_t nextVelocity,
                          units::second_t dt) {
  auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
-  EXPECT_NEAR(nextVelocity.value(),
-              Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
+  EXPECT_NEAR(
+      nextVelocity.value(),
+      Simulate(Ks, Kv, Ka, Kg, currentAngle, currentVelocity, input, dt)(1),
+      1e-4);
 }

 }  // namespace

 TEST(ArmFeedforwardTest, Calculate) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = 1.5_V / 1_rad_per_s;
+  constexpr auto Ka = 2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 1_V;
  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};

  // Calculate(angle, angular velocity)
@@ -81,18 +97,54 @@ TEST(ArmFeedforwardTest, Calculate) {
      0.002);

  // Calculate(currentAngle, currentVelocity, nextAngle, dt)
-  CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
-                       1.05_rad_per_s, 20_ms);
-  CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
-                       0.95_rad_per_s, 20_ms);
-  CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
-                       1.05_rad_per_s, 20_ms);
-  CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
-                       0.95_rad_per_s, 20_ms);
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
+                       1_rad_per_s, 1.05_rad_per_s, 20_ms);
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
+                       1_rad_per_s, 0.95_rad_per_s, 20_ms);
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
+                       1_rad_per_s, 1.05_rad_per_s, 20_ms);
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
+                       1_rad_per_s, 0.95_rad_per_s, 20_ms);
+}
+
+TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
+  constexpr auto Ks = 0.39671_V;
+  constexpr auto Kv = 2.7167_V / 1_rad_per_s;
+  constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 0.2708_V;
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+
+  constexpr auto currentAngle = 1_rad;
+  constexpr auto currentVelocity = 0.02_rad_per_s;
+  constexpr auto nextVelocity = 0_rad_per_s;
+
+  constexpr auto averageAccel = (nextVelocity - currentVelocity) / 20_ms;
+
+  EXPECT_DOUBLE_EQ(
+      armFF.Calculate(currentAngle, currentVelocity, nextVelocity).value(),
+      (Ks + Kv * currentVelocity + Ka * averageAccel +
+       Kg * units::math::cos(currentAngle))
+          .value());
+}
+
+TEST(ArmFeedforwardTest, CalculateIllConditionedGradient) {
+  constexpr auto Ks = 0.39671_V;
+  constexpr auto Kv = 2.7167_V / 1_rad_per_s;
+  constexpr auto Ka = 0.50799_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 0.2708_V;
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, 1_rad, 0.02_rad_per_s,
+                       0_rad_per_s, 20_ms);
 }

 TEST(ArmFeedforwardTest, AchievableVelocity) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = 1.5_V / 1_rad_per_s;
+  constexpr auto Ka = 2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 1_V;
  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+
  EXPECT_NEAR(armFF
                  .MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad,
                                         1_rad_per_s_sq)
@@ -106,7 +158,12 @@ TEST(ArmFeedforwardTest, AchievableVelocity) {
 }

 TEST(ArmFeedforwardTest, AchievableAcceleration) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = 1.5_V / 1_rad_per_s;
+  constexpr auto Ka = 2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 1_V;
  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+
  EXPECT_NEAR(armFF
                  .MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
                                             1_rad_per_s)
@@ -130,7 +187,12 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) {
 }

 TEST(ArmFeedforwardTest, NegativeGains) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = 1.5_V / 1_rad_per_s;
+  constexpr auto Ka = 2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 1_V;
  frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
+
  EXPECT_EQ(armFF.GetKv().value(), 0);
  EXPECT_EQ(armFF.GetKa().value(), 0);
 }
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -2,7 +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 <algorithm>
 #include <cmath>
+#include <numbers>
 #include <vector>

 #include <Eigen/QR>
@@ -12,15 +14,19 @@
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/system/Discretization.h"
 #include "frc/system/NumericalIntegration.h"
 #include "frc/system/NumericalJacobian.h"
 #include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "units/moment_of_inertia.h"

 namespace {

-frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
+// First test system, differential drive
+frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
+                              const frc::Vectord<2>& u) {
  auto motors = frc::DCMotor::CIM(2);

  // constexpr double Glow = 15.32;    // Low gear ratio
@@ -51,22 +57,21 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
          k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
 }

-frc::Vectord<3> LocalMeasurementModel(
+frc::Vectord<3> DriveLocalMeasurementModel(
    const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
  return frc::Vectord<3>{x(2), x(3), x(4)};
 }

-frc::Vectord<5> GlobalMeasurementModel(
+frc::Vectord<5> DriveGlobalMeasurementModel(
    const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
  return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
-}  // namespace

-TEST(UnscentedKalmanFilterTest, Init) {
+TEST(UnscentedKalmanFilterTest, DriveInit) {
  constexpr auto dt = 5_ms;

-  frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
-                                               LocalMeasurementModel,
+  frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
+                                               DriveLocalMeasurementModel,
                                               {0.5, 0.5, 10.0, 1.0, 1.0},
                                               {0.0001, 0.01, 0.01},
                                               frc::AngleMean<5, 5>(2),
@@ -78,22 +83,22 @@ TEST(UnscentedKalmanFilterTest, Init) {
  frc::Vectord<2> u{12.0, 12.0};
  observer.Predict(u, dt);

-  auto localY = LocalMeasurementModel(observer.Xhat(), u);
+  auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
  observer.Correct(u, localY);

-  auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
+  auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
  auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
-  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+  observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
                      frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
                      frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
 }

-TEST(UnscentedKalmanFilterTest, Convergence) {
+TEST(UnscentedKalmanFilterTest, DriveConvergence) {
  constexpr auto dt = 5_ms;
  constexpr auto rb = 0.8382_m / 2.0;  // Robot radius

-  frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
-                                               LocalMeasurementModel,
+  frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
+                                               DriveLocalMeasurementModel,
                                               {0.5, 0.5, 10.0, 1.0, 1.0},
                                               {0.0001, 0.5, 0.5},
                                               frc::AngleMean<5, 5>(2),
@@ -112,8 +117,8 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
  frc::Vectord<5> r = frc::Vectord<5>::Zero();
  frc::Vectord<2> u = frc::Vectord<2>::Zero();

-  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
-                                            frc::Vectord<2>::Zero());
+  auto B = frc::NumericalJacobianU<5, 5, 2>(
+      DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());

  observer.SetXhat(frc::Vectord<5>{
      trajectory.InitialPose().Translation().X().value(),
@@ -134,24 +139,25 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
        ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
        ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};

-    auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
+    auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
    observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));

    frc::Vectord<5> rdot = (nextR - r) / dt.value();
-    u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
+    u = B.householderQr().solve(rdot -
+                                DriveDynamics(r, frc::Vectord<2>::Zero()));

    observer.Predict(u, dt);

    r = nextR;
-    trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
+    trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
  }

-  auto localY = LocalMeasurementModel(trueXhat, u);
+  auto localY = DriveLocalMeasurementModel(trueXhat, u);
  observer.Correct(u, localY);

-  auto globalY = GlobalMeasurementModel(trueXhat, u);
+  auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
  auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
-  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+  observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
                      frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
                      frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)

@@ -168,6 +174,37 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
  EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
 }

+TEST(UnscentedKalmanFilterTest, LinearUKF) {
+  constexpr units::second_t dt = 20_ms;
+  auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
+      0.02_V / 1_mps, 0.006_V / 1_mps_sq);
+  frc::UnscentedKalmanFilter<1, 1, 1> observer{
+      [&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return plant.A() * x + plant.B() * u;
+      },
+      [&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return plant.CalculateY(x, u);
+      },
+      {0.05},
+      {1.0},
+      dt};
+
+  frc::Matrixd<1, 1> discA;
+  frc::Matrixd<1, 1> discB;
+  frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
+
+  frc::Vectord<1> ref{100.0};
+  frc::Vectord<1> u{0.0};
+
+  for (int i = 0; i < 2.0 / dt.value(); ++i) {
+    observer.Predict(u, dt);
+
+    u = discB.householderQr().solve(ref - discA * ref);
+  }
+
+  EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5);
+}
+
 TEST(UnscentedKalmanFilterTest, RoundTripP) {
  constexpr auto dt = 5_ms;

@@ -183,3 +220,90 @@ TEST(UnscentedKalmanFilterTest, RoundTripP) {

  ASSERT_TRUE(observer.P().isApprox(P));
 }
+
+// Second system, single motor feedforward estimator
+frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
+                              const frc::Vectord<1>& u) {
+  double v = x(1);
+  double kV = x(2);
+  double kA = x(3);
+
+  double V = u(0);
+
+  double a = -kV / kA * v + 1.0 / kA * V;
+  return frc::Vectord<4>{v, a, 0.0, 0.0};
+}
+
+frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
+                                      const frc::Vectord<1>& u) {
+  double p = x(0);
+  double v = x(1);
+  double kV = x(2);
+  double kA = x(3);
+
+  double V = u(0);
+
+  double a = -kV / kA * v + 1.0 / kA * V;
+  return frc::Vectord<3>{p, v, a};
+}
+
+frc::Vectord<1> MotorControlInput(double t) {
+  return frc::Vectord<1>{
+      std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
+                     6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
+                     4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
+                 -12.0, 12.0)};
+}
+
+TEST(UnscentedKalmanFilterTest, MotorConvergence) {
+  constexpr units::second_t dt = 10_ms;
+  constexpr int steps = 500;
+  constexpr double true_kV = 3;
+  constexpr double true_kA = 0.2;
+
+  constexpr double pos_stddev = 0.02;
+  constexpr double vel_stddev = 0.1;
+  constexpr double accel_stddev = 0.1;
+
+  std::vector<frc::Vectord<4>> states(steps + 1);
+  std::vector<frc::Vectord<1>> inputs(steps);
+  std::vector<frc::Vectord<3>> measurements(steps);
+  states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
+
+  constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
+                                 {0.0, -true_kV / true_kA, 0.0, 0.0},
+                                 {0.0, 0.0, 0.0, 0.0},
+                                 {0.0, 0.0, 0.0, 0.0}};
+  constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
+
+  frc::Matrixd<4, 4> discA;
+  frc::Matrixd<4, 1> discB;
+  frc::DiscretizeAB(A, B, dt, &discA, &discB);
+
+  for (int i = 0; i < steps; ++i) {
+    inputs[i] = MotorControlInput(i * dt.value());
+    states[i + 1] = discA * states[i] + discB * inputs[i];
+    measurements[i] =
+        MotorMeasurementModel(states[i + 1], inputs[i]) +
+        frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
+  }
+
+  frc::Vectord<4> P0{0.001, 0.001, 10, 10};
+
+  frc::UnscentedKalmanFilter<4, 1, 3> observer{
+      MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
+      wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
+
+  observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
+  observer.SetP(P0.asDiagonal());
+
+  for (int i = 0; i < steps; ++i) {
+    observer.Predict(inputs[i], dt);
+    observer.Correct(inputs[i], measurements[i]);
+  }
+
+  EXPECT_NEAR(true_kV, observer.Xhat(2), true_kV * 0.5);
+  EXPECT_NEAR(true_kA, observer.Xhat(3), true_kA * 0.5);
+}
+
+}  // namespace
--- a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
@@ -56,3 +56,22 @@ TEST_F(DebouncerTest, DebounceBoth) {

  EXPECT_FALSE(debouncer.Calculate(false));
 }
+
+TEST_F(DebouncerTest, DebounceParams) {
+  frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth};
+
+  EXPECT_TRUE(debouncer.GetDebounceTime() == 20_ms);
+  EXPECT_TRUE(debouncer.GetDebounceType() ==
+              frc::Debouncer::DebounceType::kBoth);
+
+  debouncer.SetDebounceTime(100_ms);
+
+  EXPECT_TRUE(debouncer.GetDebounceTime() == 100_ms);
+
+  debouncer.SetDebounceType(frc::Debouncer::DebounceType::kFalling);
+
+  EXPECT_TRUE(debouncer.GetDebounceType() ==
+              frc::Debouncer::DebounceType::kFalling);
+
+  EXPECT_TRUE(debouncer.Calculate(false));
+}
--- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -234,3 +234,10 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
    }
  }
 }
+
+TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1_mps, 1_mps_sq};
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
+  EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
+}
--- a/wpiunits/src/main/java/edu/wpi/first/units/Units.java
+++ b/wpiunits/src/main/java/edu/wpi/first/units/Units.java
@@ -203,6 +203,12 @@ public final class Units {
   */
  public static final LinearAccelerationUnit FeetPerSecondPerSecond = FeetPerSecond.per(Second);

+  /**
+   * A unit of linear acceleration equivalent to accelerating at a rate of one {@link #Inch Inch}
+   * per {@link #Second} every second.
+   */
+  public static final LinearAccelerationUnit InchesPerSecondPerSecond = InchesPerSecond.per(Second);
+
  /**
   * A unit of angular acceleration equivalent to accelerating at a rate of one {@link #Rotations
   * Rotation} per {@link #Second} every second.
--- a/wpiutil/doc/wpilog.ksy
+++ b/wpiutil/doc/wpilog.ksy
@@ -0,0 +1,130 @@
+meta:
+  id: wpilog
+  title: WPILib Data Log
+  file-extension: wpilog
+  ks-version: "0.10"
+  encoding: UTF-8
+  endian: le
+  bit-endian: le
+seq:
+  - id: header
+    type: header
+  - id: records
+    type: record
+    repeat: eos
+types:
+  version_number:
+    seq:
+      - id: minor
+        type: u1
+      - id: major
+        type: u1
+  header:
+    seq:
+      - id: magic
+        contents: WPILOG
+      - id: version
+        type: version_number
+      - id: extra_header_length
+        type: u4
+      - id: extra_header
+        size: extra_header_length
+        type: str
+  record:
+    seq:
+      - id: header_length
+        type: record_header_length
+      - id: entry_id
+        type:
+          switch-on: header_length.len_entry_id
+          cases:
+            0: u1
+            1: u2
+            2: b24
+            3: u4
+      - id: len_payload
+        type:
+          switch-on: header_length.len_payload_size
+          cases:
+            0: u1
+            1: u2
+            2: b24
+            3: u4
+      - id: timestamp
+        type:
+          switch-on: header_length.len_timestamp
+          cases:
+            0: u1
+            1: u2
+            2: b24
+            3: u4
+            4: b40
+            5: b48
+            6: b56
+            7: u8
+      - id: payload
+        size: len_payload
+        type:
+          switch-on: entry_id
+          cases:
+            # ID 0 is reserved for control records
+            0: control_record_payload
+  record_header_length:
+    seq:
+      - id: len_entry_id
+        type: b2
+      - id: len_payload_size
+        type: b2
+      - id: len_timestamp
+        type: b3
+      - id: spare_bit
+        type: b1
+  control_record_payload:
+    seq:
+      - id: type
+        type: u1
+        enum: control_record_type
+      - id: data
+        type:
+          switch-on: type
+          cases:
+            "control_record_type::start": control_record_start_data
+            "control_record_type::finish": control_record_finish_data
+            "control_record_type::set_metadata": control_record_set_metadata_data
+  control_record_start_data:
+    seq:
+      - id: entry_id
+        type: u4
+      - id: len_entry_name
+        type: u4
+      - id: entry_name
+        size: len_entry_name
+        type: str
+      - id: len_entry_type
+        type: u4
+      - id: entry_type
+        size: len_entry_type
+        type: str
+      - id: len_entry_metadata
+        type: u4
+      - id: entry_metadata
+        size: len_entry_metadata
+        type: str
+  control_record_finish_data:
+    seq:
+      - id: entry_id
+        type: u4
+  control_record_set_metadata_data:
+    seq:
+      - id: entry_id
+        type: u4
+      - id: len_entry_metadata
+        type: u4
+      - id: entry_metadata
+        size: len_entry_metadata
+        type: str
+enums:
+  control_record_type:
+    0: start
+    1: finish
+    2: set_metadata
--- a/wpiutil/src/main/native/thirdparty/debugging/src/emscripten.cpp
+++ b/wpiutil/src/main/native/thirdparty/debugging/src/emscripten.cpp
@@ -0,0 +1,18 @@
+#if defined(__EMSCRIPTEN__)
+
+#	include <debugging.hpp>
+
+#	include <atomic>
+#	include <fstream>
+#	include <string>
+
+namespace wpi {
+
+bool is_debugger_present() noexcept
+{
+  return false;
+}
+
+} // namespace wpi
+
+#endif
--- a/wpiutil/src/main/native/thirdparty/memory/include/wpi/memory/memory_arena.hpp
+++ b/wpiutil/src/main/native/thirdparty/memory/include/wpi/memory/memory_arena.hpp
@@ -656,32 +656,32 @@ namespace wpi
            /// \returns The number of bytes `value` is in the given unit.
            /// \ingroup memory_core
            /// @{
-            constexpr std::size_t operator"" _KiB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_KiB(unsigned long long value) noexcept
            {
                return std::size_t(value * 1024);
            }

-            constexpr std::size_t operator"" _KB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_KB(unsigned long long value) noexcept
            {
                return std::size_t(value * 1000);
            }

-            constexpr std::size_t operator"" _MiB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_MiB(unsigned long long value) noexcept
            {
                return std::size_t(value * 1024 * 1024);
            }

-            constexpr std::size_t operator"" _MB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_MB(unsigned long long value) noexcept
            {
                return std::size_t(value * 1000 * 1000);
            }

-            constexpr std::size_t operator"" _GiB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_GiB(unsigned long long value) noexcept
            {
                return std::size_t(value * 1024 * 1024 * 1024);
            }

-            constexpr std::size_t operator"" _GB(unsigned long long value) noexcept
+            constexpr std::size_t operator""_GB(unsigned long long value) noexcept
            {
                return std::size_t(value * 1000 * 1000 * 1000);
            }
--- a/wpiutil/src/main/native/unix/StackTrace.cpp
+++ b/wpiutil/src/main/native/unix/StackTrace.cpp
@@ -4,7 +4,9 @@

 #include "wpi/StackTrace.h"

+#ifndef __EMSCRIPTEN__
 #include <execinfo.h>
+#endif

 #include <string>

@@ -16,7 +18,7 @@
 namespace wpi {

 std::string GetStackTraceDefault(int offset) {
-#ifndef __ANDROID__
+#if !defined(__ANDROID__) && !defined(__EMSCRIPTEN__)
  void* stackTrace[128];
  int stackSize = backtrace(stackTrace, 128);
  char** mangledSymbols = backtrace_symbols(stackTrace, stackSize);
@@ -40,7 +42,7 @@ std::string GetStackTraceDefault(int offset) {

  return std::string{trace.str()};
 #else
-  // backtrace_symbols not supported on android
+  // backtrace_symbols not supported
  return "";
 #endif
 }
--- a/wpiutil/src/test/native/include/wpi/SpanMatcher.h
+++ b/wpiutil/src/test/native/include/wpi/SpanMatcher.h
@@ -70,6 +70,6 @@ void SpanMatcher<T>::DescribeNegationTo(::std::ostream* os) const {

 }  // namespace wpi

-inline std::span<const uint8_t> operator"" _us(const char* str, size_t len) {
+inline std::span<const uint8_t> operator""_us(const char* str, size_t len) {
  return {reinterpret_cast<const uint8_t*>(str), len};
 }
--- a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java
+++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java
@@ -21,7 +21,7 @@ public class XRPServo {

  private static void checkDeviceAllocation(int deviceNum) {
    if (!s_simDeviceNameMap.containsKey(deviceNum)) {
-      throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
+      throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-7");
    }

    if (s_registeredDevices.contains(deviceNum)) {
@@ -34,6 +34,8 @@ public class XRPServo {
  static {
    s_simDeviceNameMap.put(4, "servo1");
    s_simDeviceNameMap.put(5, "servo2");
+    s_simDeviceNameMap.put(6, "servo3");
+    s_simDeviceNameMap.put(7, "servo4");
  }

  private final SimDouble m_simPosition;
--- a/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp
+++ b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp
@@ -15,8 +15,8 @@

 using namespace frc;

-std::map<int, std::string> XRPServo::s_simDeviceMap = {{4, "servo1"},
-                                                       {5, "servo2"}};
+std::map<int, std::string> XRPServo::s_simDeviceMap = {
+    {4, "servo1"}, {5, "servo2"}, {6, "servo3"}, {7, "servo4"}};

 std::set<int> XRPServo::s_registeredDevices = {};