Merge branch 'main' into 2027

.github/workflows/cmake-android.yml

@@ -30,7 +30,7 @@ jobs:
       - uses: nttld/setup-ndk@v1
         id: setup-ndk
         with:
-          ndk-version: r27c
+          ndk-version: r27d
           add-to-path: false
 
       - uses: actions/setup-java@v5

.github/workflows/gradle.yml

@@ -56,8 +56,7 @@ jobs:
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v2027')
       - name: Build with Gradle
-        # main on 2021-05-30, to include addnab/docker-run-action/pull/17
-        uses: addnab/docker-run-action@3e77f186b7a929ef010f183a9e24c0f9955ea609
+        uses: wpilibsuite/docker-run-action@v4
         with:
           image: ${{ matrix.container }}
           options: -v ${{ github.workspace }}:/work -w /work -e ARTIFACTORY_PUBLISH_USERNAME -e ARTIFACTORY_PUBLISH_PASSWORD -e GITHUB_REF -e CI

.github/workflows/sentinel-build.yml

@@ -50,8 +50,7 @@ jobs:
         with:
           fetch-depth: 0
       - name: Build with Gradle
-        # main on 2021-05-30, to include addnab/docker-run-action/pull/17
-        uses: addnab/docker-run-action@3e77f186b7a929ef010f183a9e24c0f9955ea609
+        uses: wpilibsuite/docker-run-action@v4
         with:
           image: ${{ matrix.container }}
           options: -v ${{ github.workspace }}:/work -w /work -e GITHUB_REF -e CI

CMakeLists.txt

@@ -349,7 +349,9 @@ if(WITH_WPILIB)
     set(APRILTAG_DEP_REPLACE "find_dependency(apriltag)")
     set(COMMANDSV3_DEP_REPLACE "find_dependency(commandsv3)")
     set(WPILIBC_DEP_REPLACE "find_dependency(wpilibc)")
-    set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)")
+    if(WITH_JAVA)
+        set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)")
+    endif()
     set(COMMAND_DEP_REPLACE "find_dependency(commandsv2)")
     add_subdirectory(apriltag)
     add_subdirectory(wpilibj)

LICENSE.md

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2025 FIRST and other WPILib contributors
+Copyright (c) 2009-2026 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java

@@ -17,13 +17,17 @@ public enum AprilTagFields {
   /** 2025 Reefscape Welded (see TU 12). */
   k2025ReefscapeWelded("2025-reefscape-welded.json"),
   /** 2025 Reefscape AndyMark (see TU 12). */
-  k2025ReefscapeAndyMark("2025-reefscape-andymark.json");
+  k2025ReefscapeAndyMark("2025-reefscape-andymark.json"),
+  /** 2026 Rebuilt Welded. */
+  k2026RebuiltWelded("2026-rebuilt-welded.json"),
+  /** 2026 Rebuilt AndyMark. */
+  k2026RebuiltAndymark("2026-rebuilt-andymark.json");
 
   /** Base resource directory. */
   public static final String kBaseResourceDir = "/org/wpilib/vision/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2025ReefscapeWelded;
+  public static final AprilTagFields kDefaultField = k2026RebuiltWelded;
 
   /** Resource filename. */
   public final String m_resourceFile;

apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp

@@ -141,6 +141,8 @@ std::string_view GetResource_2023_chargedup_json();
 std::string_view GetResource_2024_crescendo_json();
 std::string_view GetResource_2025_reefscape_welded_json();
 std::string_view GetResource_2025_reefscape_andymark_json();
+std::string_view GetResource_2026_rebuilt_welded_json();
+std::string_view GetResource_2026_rebuilt_andymark_json();
 
 }  // namespace wpi::apriltag
 
@@ -162,6 +164,12 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
     case AprilTagField::k2025ReefscapeAndyMark:
       fieldString = GetResource_2025_reefscape_andymark_json();
       break;
+    case AprilTagField::k2026RebuiltWelded:
+      fieldString = GetResource_2026_rebuilt_welded_json();
+      break;
+    case AprilTagField::k2026RebuiltAndyMark:
+      fieldString = GetResource_2026_rebuilt_andymark_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid Field");
   }

apriltag/src/main/native/include/wpi/apriltag/AprilTagFields.hpp

@@ -24,8 +24,12 @@ enum class AprilTagField {
   k2025ReefscapeAndyMark,
   /// 2025 Reefscape Welded (see TU12).
   k2025ReefscapeWelded,
+  /// 2026 Rebuilt Andymark.
+  k2026RebuiltAndyMark,
+  /// 2026 Rebuilt Welded.
+  k2026RebuiltWelded,
   /// Alias to the current game.
-  kDefaultField = k2025ReefscapeWelded,
+  kDefaultField = k2026RebuiltWelded,
 
   // This is a placeholder for denoting the last supported field. This should
   // always be the last entry in the enum and should not be used by users

apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-andymark.csv

@@ -0,0 +1,33 @@
+ID,X,Y,Z,Z-Rotation,X-Rotation
+1,467.085,291.791,35,180,0
+2,468.559,182.077,44.25,90,0
+3,444.797,172.321,44.25,180,0
+4,444.797,158.321,44.25,180,0
+5,468.559,134.565,44.25,270,0
+6,467.085,24.851,35,180,0
+7,470.034,24.851,35,0,0
+8,482.559,134.565,44.25,270,0
+9,492.329,144.321,44.25,0,0
+10,492.329,158.321,44.25,0,0
+11,482.559,182.077,44.25,90,0
+12,470.034,291.791,35,0,0
+13,649.58,291.02,21.75,180,0
+14,649.58,274.02,21.75,180,0
+15,649.566,169.783,21.75,180,0
+16,649.566,152.783,21.75,180,0
+17,183.034,24.851,35,0,0
+18,181.559,134.565,44.25,270,0
+19,205.321,144.321,44.25,0,0
+20,205.321,158.321,44.25,0,0
+21,181.559,182.077,44.25,90,0
+22,183.034,291.791,35,0,0
+23,180.085,291.791,35,180,0
+24,167.559,182.077,44.25,90,0
+25,157.79,172.321,44.25,180,0
+26,157.79,158.321,44.25,180,0
+27,167.559,134.565,44.25,270,0
+28,180.085,24.851,35,180,0
+29,0.539,25.621,21.75,0,0
+30,0.539,42.621,21.75,0,0
+31,0.553,146.858,21.75,0,0
+32,0.553,163.858,21.75,0,0

apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-andymark.json

@@ -0,0 +1,584 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 11.863959,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 11.9013986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.2978438,
+          "y": 4.3769534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 11.2978438,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 11.9013986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 11.863959,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 11.9388636,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 12.2569986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.5051566,
+          "y": 3.6657534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.5051566,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.2569986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 11.9388636,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 16.499332,
+          "y": 7.391907999999999,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 16.499332,
+          "y": 6.960107999999999,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 16.4989764,
+          "y": 4.3124882,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 16.4989764,
+          "y": 3.8806881999999994,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.6490636,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 4.6115986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 5.2151534,
+          "y": 3.6657534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 5.2151534,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 4.6115986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.6490636,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 23,
+      "pose": {
+        "translation": {
+          "x": 4.574159,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 24,
+      "pose": {
+        "translation": {
+          "x": 4.2559986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 25,
+      "pose": {
+        "translation": {
+          "x": 4.007866,
+          "y": 4.3769534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 26,
+      "pose": {
+        "translation": {
+          "x": 4.007866,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 27,
+      "pose": {
+        "translation": {
+          "x": 4.2559986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 28,
+      "pose": {
+        "translation": {
+          "x": 4.574159,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 29,
+      "pose": {
+        "translation": {
+          "x": 0.0136906,
+          "y": 0.6507734,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 30,
+      "pose": {
+        "translation": {
+          "x": 0.0136906,
+          "y": 1.0825734,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 31,
+      "pose": {
+        "translation": {
+          "x": 0.0140462,
+          "y": 3.7301932,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 32,
+      "pose": {
+        "translation": {
+          "x": 0.0140462,
+          "y": 4.1619931999999995,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.518,
+    "width": 8.043
+  }
+}

apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-welded.csv

@@ -0,0 +1,33 @@
+ID,X,Y,Z,Z-Rotation,X-Rotation
+1,467.637,292.314,35,180,0
+2,469.111,182.6,44.25,90,0
+3,445.349,172.844,44.25,180,0
+4,445.349,158.844,44.25,180,0
+5,469.111,135.088,44.25,270,0
+6,467.637,25.374,35,180,0
+7,470.586,25.374,35,0,0
+8,483.111,135.088,44.25,270,0
+9,492.881,144.844,44.25,0,0
+10,492.881,158.844,44.25,0,0
+11,483.111,182.6,44.25,90,0
+12,470.586,292.314,35,0,0
+13,650.918,291.469,21.75,180,0
+14,650.918,274.469,21.75,180,0
+15,650.904,170.219,21.75,180,0
+16,650.904,153.219,21.75,180,0
+17,183.586,25.374,35,0,0
+18,182.111,135.088,44.25,270,0
+19,205.873,144.844,44.25,0,0
+20,205.873,158.844,44.25,0,0
+21,182.111,182.6,44.25,90,0
+22,183.586,292.314,35,0,0
+23,180.637,292.314,35,180,0
+24,168.111,182.6,44.25,90,0
+25,158.341,172.844,44.25,180,0
+26,158.341,158.844,44.25,180,0
+27,168.111,135.088,44.25,270,0
+28,180.637,25.374,35,180,0
+29,0.305,26.219,21.75,0,0
+30,0.305,43.219,21.75,0,0
+31,0.318,147.469,21.75,0,0
+32,0.318,164.469,21.75,0,0

apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-welded.json

@@ -0,0 +1,584 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 11.8779798,
+          "y": 7.4247756,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 11.9154194,
+          "y": 4.638039999999999,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.3118646,
+          "y": 4.3902376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 11.3118646,
+          "y": 4.0346376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 11.9154194,
+          "y": 3.4312351999999997,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 11.8779798,
+          "y": 0.6444996,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 11.9528844,
+          "y": 0.6444996,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 12.2710194,
+          "y": 3.4312351999999997,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.519177399999998,
+          "y": 3.6790375999999996,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.519177399999998,
+          "y": 4.0346376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.2710194,
+          "y": 4.638039999999999,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 11.9528844,
+          "y": 7.4247756,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 16.5333172,
+          "y": 7.4033126,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 16.5333172,
+          "y": 6.9715126,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 16.5329616,
+          "y": 4.3235626,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 16.5329616,
+          "y": 3.8917626,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.6630844,
+          "y": 0.6444996,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 4.6256194,
+          "y": 3.4312351999999997,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 5.229174199999999,
+          "y": 3.6790375999999996,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 5.229174199999999,
+          "y": 4.0346376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 4.6256194,
+          "y": 4.638039999999999,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.6630844,
+          "y": 7.4247756,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 23,
+      "pose": {
+        "translation": {
+          "x": 4.5881798,
+          "y": 7.4247756,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 24,
+      "pose": {
+        "translation": {
+          "x": 4.2700194,
+          "y": 4.638039999999999,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 25,
+      "pose": {
+        "translation": {
+          "x": 4.0218614,
+          "y": 4.3902376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 26,
+      "pose": {
+        "translation": {
+          "x": 4.0218614,
+          "y": 4.0346376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 27,
+      "pose": {
+        "translation": {
+          "x": 4.2700194,
+          "y": 3.4312351999999997,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 28,
+      "pose": {
+        "translation": {
+          "x": 4.5881798,
+          "y": 0.6444996,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 29,
+      "pose": {
+        "translation": {
+          "x": 0.0077469999999999995,
+          "y": 0.6659626,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 30,
+      "pose": {
+        "translation": {
+          "x": 0.0077469999999999995,
+          "y": 1.0977626,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 31,
+      "pose": {
+        "translation": {
+          "x": 0.0080772,
+          "y": 3.7457125999999996,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 32,
+      "pose": {
+        "translation": {
+          "x": 0.0080772,
+          "y": 4.1775126,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.541,
+    "width": 8.069
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/Trigger.java

@@ -11,6 +11,7 @@ import org.wpilib.command2.Command;
 import org.wpilib.command2.CommandScheduler;
 import org.wpilib.event.EventLoop;
 import org.wpilib.math.filter.Debouncer;
+import org.wpilib.math.filter.EdgeCounterFilter;
 
 /**
  * This class provides an easy way to link commands to conditions.
@@ -287,4 +288,31 @@ public class Trigger implements BooleanSupplier {
           }
         });
   }
+
+  /**
+   * Creates a new multi-press trigger from this trigger - it will become active when this trigger
+   * has been activated the required number of times within the specified time window.
+   *
+   * <p>This is useful for implementing "double-click" style functionality.
+   *
+   * <p>Input for this must be stable, consider using a Debouncer before this to avoid counting
+   * noise as multiple presses.
+   *
+   * @param requiredPresses The number of presses required.
+   * @param windowTime The number of seconds in which the presses must occur.
+   * @return The multi-press trigger.
+   */
+  public Trigger multiPress(int requiredPresses, double windowTime) {
+    return new Trigger(
+        m_loop,
+        new BooleanSupplier() {
+          final EdgeCounterFilter m_edgeCounterFilter =
+              new EdgeCounterFilter(requiredPresses, windowTime);
+
+          @Override
+          public boolean getAsBoolean() {
+            return m_edgeCounterFilter.calculate(m_condition.getAsBoolean());
+          }
+        });
+  }
 }

commandsv2/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -8,6 +8,7 @@
 
 #include "wpi/commands2/CommandPtr.hpp"
 #include "wpi/math/filter/Debouncer.hpp"
+#include "wpi/math/filter/EdgeCounterFilter.hpp"
 
 using namespace wpi;
 using namespace wpi::cmd;
@@ -183,6 +184,14 @@ Trigger Trigger::Debounce(wpi::units::second_t debounceTime,
   });
 }
 
+Trigger Trigger::MultiPress(int requiredPresses, units::second_t windowTime) {
+  return Trigger(m_loop, [filter = wpi::math::EdgeCounterFilter(requiredPresses,
+                                                                windowTime),
+                          condition = m_condition]() mutable {
+    return filter.Calculate(condition());
+  });
+}
+
 bool Trigger::Get() const {
   return m_condition();
 }

commandsv2/src/main/native/include/wpi/commands2/button/Trigger.hpp

@@ -283,6 +283,22 @@ class Trigger {
                    wpi::math::Debouncer::DebounceType type =
                        wpi::math::Debouncer::DebounceType::kRising);
 
+  /**
+   * Creates a new multi-press trigger from this trigger - it will become active
+   * when this trigger has been activated the required number of times within
+   * the specified time window.
+   *
+   * <p>This is useful for implementing "double-click" style functionality.
+   *
+   * <p>Input for this must be stable, consider using a Debouncer before this to
+   * avoid counting noise as multiple presses.
+   *
+   * @param requiredPresses The number of presses required.
+   * @param windowTime The time in which the presses must occur.
+   * @return The multi-press trigger.
+   */
+  Trigger MultiPress(int requiredPresses, units::second_t windowTime);
+
   /**
    * Returns the current state of this trigger.
    *

cscore/src/main/native/objcpp/UvcControlImpl.mm

@@ -484,7 +484,7 @@ const propertyInfo_t propertyInfo[] =
                 UVCERROR("USB control interface not found");
                 break;
             default:
-                UVCERROR("ControlRequest failed (KR=sys:sub:code) = {:02Xh}:{:03Xh}:{:04Xh}", 
+                UVCERROR("ControlRequest failed (KR=sys:sub:code) = {:02X}h:{:03X}h:{:04X}h", 
                     sys, sub, code);
                 break; 
         }

fields/src/main/java/org/wpilib/fields/Fields.java

@@ -17,12 +17,13 @@ public enum Fields {
   k2022RapidReact("2022-rapidreact.json"),
   k2023ChargedUp("2023-chargedup.json"),
   k2024Crescendo("2024-crescendo.json"),
-  k2025Reefscape("2025-reefscape.json");
+  k2025Reefscape("2025-reefscape.json"),
+  k2026Rebuilt("2026-rebuilt.json");
 
   public static final String kBaseResourceDir = "/org/wpilib/fields/";
 
   /** Alias to the current game. */
-  public static final Fields kDefaultField = k2025Reefscape;
+  public static final Fields kDefaultField = k2026Rebuilt;
 
   public final String m_resourceFile;
 

fields/src/main/native/cpp/fields.cpp

@@ -17,10 +17,12 @@
 #include "wpi/fields/2023-chargedup.hpp"
 #include "wpi/fields/2024-crescendo.hpp"
 #include "wpi/fields/2025-reefscape.hpp"
+#include "wpi/fields/2026-rebuilt.hpp"
 
 using namespace wpi::fields;
 
 static const Field kFields[] = {
+    {"2026 Rebuilt", GetResource_2026_rebuilt_json, GetResource_2026_field_png},
     {"2025 Reefscape", GetResource_2025_reefscape_json,
      GetResource_2025_field_png},
     {"2024 Crescendo", GetResource_2024_crescendo_json,

fields/src/main/native/include/wpi/fields/2026-rebuilt.hpp

@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+namespace wpi::fields {
+std::string_view GetResource_2026_rebuilt_json();
+std::string_view GetResource_2026_field_png();
+}  // namespace wpi::fields

fields/src/main/native/resources/org/wpilib/fields/2026-rebuilt.json

@@ -0,0 +1,19 @@
+{
+  "game": "Rebuilt",
+  "field-image": "2026-field.png",
+  "field-corners": {
+    "top-left": [
+      245,
+      118
+    ],
+    "bottom-right": [
+      3942,
+      1914
+    ]
+  },
+  "field-size": [
+    54.269,
+    26.474
+  ],
+  "field-unit": "foot"
+}

glass/src/lib/native/cpp/other/FMS.cpp

@@ -73,11 +73,11 @@ void wpi::glass::DisplayFMS(FMSModel* model, bool editableDsAttached) {
     }
     ImGui::SameLine();
     if (ImGui::Button("Auto") && !enabled) {
-      model->SetMatchTime(15.0);
+      model->SetMatchTime(20.0);
     }
     ImGui::SameLine();
     if (ImGui::Button("Teleop") && !enabled) {
-      model->SetMatchTime(135.0);
+      model->SetMatchTime(140.0);
     }
   }
 

glass/src/lib/native/cpp/other/Field2D.cpp

@@ -348,7 +348,7 @@ static bool InputPose(wpi::math::Pose2d* pose) {
 }
 
 FieldInfo::FieldInfo(Storage& storage)
-    : m_builtin{storage.GetString("builtin", "2025 Reefscape")},
+    : m_builtin{storage.GetString("builtin", "2026 Rebuilt")},
       m_filename{storage.GetString("image")},
       m_width{storage.GetFloat("width", kDefaultWidth.to<float>())},
       m_height{storage.GetFloat("height", kDefaultHeight.to<float>())},

hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java

@@ -241,16 +241,19 @@ public class DriverStationJNI extends JNIWrapper {
   public static native String getJoystickName(byte joystickNum);
 
   /**
-   * Returns the approximate match time.
+   * Return the approximate match time. The FMS does not send an official match time to the robots,
+   * but does send an approximate match time. The value will count down the time remaining in the
+   * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
+   * dispute ref calls or guarantee that a function will trigger before the match ends).
    *
-   * <p>The FMS does not send an official match time to the robots, but does send an approximate
-   * match time. The value will count down the time remaining in the current period (auto or
-   * teleop).
+   * <p>When connected to the real field, this number only changes in full integer increments, and
+   * always counts down.
    *
-   * <p>Warning: This is not an official time (so it cannot be used to dispute ref calls or
-   * guarantee that a function will trigger before the match ends).
+   * <p>When the DS is in practice mode, this number is a floating point number, and counts down.
    *
-   * <p>The Practice Match function of the DS approximates the behavior seen on the field.
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
+   *
+   * <p>Simulation matches DS behavior without an FMS connected.
    *
    * @return time remaining in current match period (auto or teleop)
    * @see "HAL_GetMatchTime"

hal/src/main/native/include/wpi/hal/DriverStation.h

@@ -240,8 +240,7 @@ int32_t HAL_SetJoystickLeds(int32_t joystickNum, int32_t leds);
  * <p>When the DS is in practice mode, this number is a floating point number,
  * and counts down.
  *
- * <p>When the DS is in teleop or autonomous mode, this number is a floating
- * point number, and counts up.
+ * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
  *
  * <p>Simulation matches DS behavior without an FMS connected.
  *

tools/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp

@@ -57,7 +57,8 @@ static double Lerp(wpi::units::second_t time,
  */
 static std::vector<PreparedData> ConvertToPrepared(const MotorData& data) {
   std::vector<PreparedData> prepared;
-  // assume we've selected down to a single contiguous run by this point
+
+  // Assume we've selected down to a single contiguous run by this point
   auto run = data.runs[0];
 
   for (int i = 0; i < static_cast<int>(run.voltage.size()) - 1; ++i) {
@@ -101,7 +102,7 @@ static void CopyRawData(wpi::util::StringMap<MotorData>* dataset) {
 }
 
 /**
- * Assigns the combines the various datasets into a single one for analysis.
+ * Combines the various datasets into a single one for analysis.
  *
  * @param slowForward The slow forward dataset
  * @param slowBackward The slow backward dataset
@@ -127,6 +128,28 @@ void AnalysisManager::PrepareGeneralData() {
   // Convert data to PreparedData structs
   for (auto& it : m_data.motorData) {
     auto key = it.first;
+
+    // Assume we've selected down to a single contiguous run by this point
+    auto run = m_data.motorData[key].runs[0];
+
+    // Ensure data has at least two samples in it or linear interpolation within
+    // ConvertToPrepared() will fail
+    if (run.voltage.size() < 2) {
+      throw sysid::InvalidDataError(fmt::format(
+          "{} data has {} voltage samples and at least 2 are required.", key,
+          run.voltage.size()));
+    }
+    if (run.position.size() < 2) {
+      throw sysid::InvalidDataError(fmt::format(
+          "{} data has {} position samples and at least 2 are required.", key,
+          run.position.size()));
+    }
+    if (run.velocity.size() < 2) {
+      throw sysid::InvalidDataError(fmt::format(
+          "{} data has {} velocity samples and at least 2 are required.", key,
+          run.velocity.size()));
+    }
+
     preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
     WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
   }

wpilibc/src/main/native/cpp/hardware/led/LEDPattern.cpp

@@ -11,6 +11,7 @@
 #include <utility>
 #include <vector>
 
+#include "wpi/hal/UsageReporting.h"
 #include "wpi/math/util/MathUtil.hpp"
 #include "wpi/util/MathExtras.hpp"
 #include "wpi/util/timestamp.h"
@@ -21,7 +22,9 @@ LEDPattern::LEDPattern(
     std::function<void(wpi::LEDPattern::LEDReader,
                        std::function<void(int, wpi::util::Color)>)>
         impl)
-    : m_impl(std::move(impl)) {}
+    : m_impl(std::move(impl)) {
+  HAL_ReportUsage("LEDPattern", "");
+}
 
 void LEDPattern::ApplyTo(
     LEDPattern::LEDReader reader,

wpilibc/src/main/native/include/wpi/drive/DifferentialDrive.hpp

@@ -76,7 +76,7 @@ class DifferentialDrive : public RobotDriveBase,
    * Construct a DifferentialDrive.
    *
    * To pass multiple motors per side, use CAN motor controller followers or
-   * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so
+   * PWMMotorController::AddFollower(). If a motor needs to be inverted, do so
    * before passing it in.
    *
    * @param leftMotor Left motor.
@@ -90,7 +90,7 @@ class DifferentialDrive : public RobotDriveBase,
    * Construct a DifferentialDrive.
    *
    * To pass multiple motors per side, use CAN motor controller followers or
-   * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so
+   * PWMMotorController::AddFollower(). If a motor needs to be inverted, do so
    * before passing it in.
    *
    * @param leftMotor Left motor setter.

wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp

@@ -587,8 +587,7 @@ class DriverStation final {
    * <p>When the DS is in practice mode, this number is a floating point number,
    * and counts down.
    *
-   * <p>When the DS is in teleop or autonomous mode, this number is a floating
-   * point number, and counts up.
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
    *
    * <p>Simulation matches DS behavior without an FMS connected.
    *

wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp

@@ -64,7 +64,7 @@ class TimedRobot : public IterativeRobotBase {
   ~TimedRobot() override;
 
   /**
-   * Return the system clock time in micrseconds for the start of the current
+   * Return the system clock time in microseconds for the start of the current
    * periodic loop. This is in the same time base as Timer.GetFPGATimestamp(),
    * but is stable through a loop. It is updated at the beginning of every
    * periodic callback (including the normal periodic loop).

wpilibc/src/main/native/include/wpi/system/Timer.hpp

@@ -139,19 +139,23 @@ class Timer {
   static wpi::units::second_t GetFPGATimestamp();
 
   /**
-   * Return the approximate match time.
-   *
-   * The FMS does not send an official match time to the robots, but does send
-   * an approximate match time. The value will count down the time remaining in
-   * the current period (auto or teleop).
-   *
+   * Return the approximate match time. The FMS does not send an official match
+   * time to the robots, but does send an approximate match time. The value will
+   * count down the time remaining in the current period (auto or teleop).
    * Warning: This is not an official time (so it cannot be used to dispute ref
    * calls or guarantee that a function will trigger before the match ends).
    *
-   * The Practice Match function of the DS approximates the behavior seen on the
-   * field.
+   * <p>When connected to the real field, this number only changes in full
+   * integer increments, and always counts down.
    *
-   * @return Time remaining in current match period (auto or teleop)
+   * <p>When the DS is in practice mode, this number is a floating point number,
+   * and counts down.
+   *
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
+   *
+   * <p>Simulation matches DS behavior without an FMS connected.
+   *
+   * @return Time remaining in current match period (auto or teleop) in seconds
    */
   static wpi::units::second_t GetMatchTime();
 

wpilibcExamples/src/main/cpp/examples/Xrptimed/cpp/Robot.cpp

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Robot.hpp"
+
+Robot::Robot() {
+  wpi::util::SendableRegistry::AddChild(&m_drive, &m_leftMotor);
+  wpi::util::SendableRegistry::AddChild(&m_drive, &m_rightMotor);
+
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotor.SetInverted(true);
+}
+
+/**
+ * This function is called every 20 ms, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {}
+
+// This function is only once each time Autonomous is enabled
+void Robot::AutonomousInit() {
+  m_timer.Restart();
+}
+
+// This function is called periodically during autonomous mode
+void Robot::AutonomousPeriodic() {
+  // Drive for 2 seconds
+  if (m_timer.Get() < 2_s) {
+    // Drive forwards half speed, make sure to turn input squaring off
+    m_drive.ArcadeDrive(0.5, 0.0, false);
+  } else {
+    // Stop robot
+    m_drive.ArcadeDrive(0.0, 0.0, false);
+  }
+}
+
+// This function is only once each time telop is enabled
+void Robot::TeleopInit() {}
+
+// This function is called periodically during teleop mode
+void Robot::TeleopPeriodic() {
+  m_drive.ArcadeDrive(-m_controller.GetRawAxis(2), -m_controller.GetRawAxis(1));
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return wpi::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/Xrptimed/include/Robot.hpp

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "wpi/drive/DifferentialDrive.hpp"
+#include "wpi/driverstation/Joystick.hpp"
+#include "wpi/framework/TimedRobot.hpp"
+#include "wpi/system/Timer.hpp"
+#include "wpi/xrp/XRPMotor.hpp"
+
+class Robot : public wpi::TimedRobot {
+ public:
+  Robot();
+  void RobotPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+
+ private:
+  wpi::xrp::XRPMotor m_leftMotor{0};
+  wpi::xrp::XRPMotor m_rightMotor{1};
+  // Assumes a gamepad plugged into channel 0
+  wpi::Joystick m_controller{0};
+  wpi::Timer m_timer;
+
+  wpi::DifferentialDrive m_drive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
+};

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -510,6 +510,22 @@
       "xrp"
     ]
   },
+  {
+    "name": "XRP Timed",
+    "description": "An very basic timed robot program that can be used with the XRP reference robot design.",
+    "tags": [
+      "XRP",
+      "Differential Drive",
+      "Joystick"
+    ],
+    "foldername": "Xrptimed",
+    "gradlebase": "cppxrp",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "extravendordeps": [
+      "xrp"
+    ]
+  },
   {
     "name": "StateSpaceFlywheel",
     "description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",

wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java

@@ -1733,8 +1733,7 @@ public final class DriverStation {
    *
    * <p>When the DS is in practice mode, this number is a floating point number, and counts down.
    *
-   * <p>When the DS is in teleop or autonomous mode, this number is a floating point number, and
-   * counts up.
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
    *
    * <p>Simulation matches DS behavior without an FMS connected.
    *

wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java

@@ -181,9 +181,9 @@ public class TimedRobot extends IterativeRobotBase {
   }
 
   /**
-   * Return the system clock time in micrseconds for the start of the current periodic loop. This is
-   * in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is updated
-   * at the beginning of every periodic callback (including the normal periodic loop).
+   * Return the system clock time in microseconds for the start of the current periodic loop. This
+   * is in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is
+   * updated at the beginning of every periodic callback (including the normal periodic loop).
    *
    * @return Robot running time in microseconds, as of the start of the current periodic function.
    */

wpilibj/src/main/java/org/wpilib/hardware/led/LEDPattern.java

@@ -14,6 +14,7 @@ import java.util.Objects;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import org.wpilib.driverstation.DriverStation;
+import org.wpilib.hardware.hal.HAL;
 import org.wpilib.system.RobotController;
 import org.wpilib.units.collections.LongToObjectHashMap;
 import org.wpilib.units.measure.Dimensionless;
@@ -150,6 +151,7 @@ public interface LEDPattern {
    * @return the mapped pattern
    */
   default LEDPattern mapIndex(IndexMapper indexMapper) {
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       int bufLen = reader.getLength();
       applyTo(
@@ -295,6 +297,7 @@ public interface LEDPattern {
     final long totalTimeMicros = (long) (onTime.in(Microseconds) + offTime.in(Microseconds));
     final long onTimeMicros = (long) onTime.in(Microseconds);
 
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       if (RobotController.getTime() % totalTimeMicros < onTimeMicros) {
         applyTo(reader, writer);
@@ -324,6 +327,7 @@ public interface LEDPattern {
    * @return the blinking pattern
    */
   default LEDPattern synchronizedBlink(BooleanSupplier signal) {
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       if (signal.getAsBoolean()) {
         applyTo(reader, writer);
@@ -343,6 +347,7 @@ public interface LEDPattern {
   default LEDPattern breathe(Time period) {
     final long periodMicros = (long) period.in(Microseconds);
 
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       applyTo(
           reader,
@@ -374,6 +379,7 @@ public interface LEDPattern {
    * @return the combined overlay pattern
    */
   default LEDPattern overlayOn(LEDPattern base) {
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       // write the base pattern down first...
       base.applyTo(reader, writer);
@@ -401,6 +407,7 @@ public interface LEDPattern {
    * @return the blended pattern
    */
   default LEDPattern blend(LEDPattern other) {
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       applyTo(reader, writer);
 
@@ -432,6 +439,7 @@ public interface LEDPattern {
    * @return the masked pattern
    */
   default LEDPattern mask(LEDPattern mask) {
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       // Apply the current pattern down as normal...
       applyTo(reader, writer);
@@ -471,6 +479,7 @@ public interface LEDPattern {
   default LEDPattern atBrightness(Dimensionless relativeBrightness) {
     double multiplier = relativeBrightness.in(Value);
 
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       applyTo(
           reader,
@@ -497,6 +506,7 @@ public interface LEDPattern {
    * @return the pattern
    */
   static LEDPattern solid(Color color) {
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       int bufLen = reader.getLength();
       for (int led = 0; led < bufLen; led++) {
@@ -526,6 +536,7 @@ public interface LEDPattern {
    * @return the mask pattern
    */
   static LEDPattern progressMaskLayer(DoubleSupplier progressSupplier) {
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       double progress = Math.clamp(progressSupplier.getAsDouble(), 0, 1);
 
@@ -562,6 +573,7 @@ public interface LEDPattern {
    * @return a motionless step pattern
    */
   static LEDPattern steps(Map<? extends Number, Color> steps) {
+    HAL.reportUsage("LEDPattern", "");
     if (steps.isEmpty()) {
       // no colors specified
       DriverStation.reportWarning("Creating LED steps with no colors!", false);
@@ -623,6 +635,7 @@ public interface LEDPattern {
    * @return a motionless gradient pattern
    */
   static LEDPattern gradient(GradientType type, Color... colors) {
+    HAL.reportUsage("LEDPattern", "");
     if (colors.length == 0) {
       // Nothing to display
       DriverStation.reportWarning("Creating a gradient with no colors!", false);
@@ -680,6 +693,7 @@ public interface LEDPattern {
    * @return the rainbow pattern
    */
   static LEDPattern rainbow(int saturation, int value) {
+    HAL.reportUsage("LEDPattern", "");
     return (reader, writer) -> {
       int bufLen = reader.getLength();
       for (int i = 0; i < bufLen; i++) {

wpilibj/src/main/java/org/wpilib/system/Timer.java

@@ -41,8 +41,16 @@ public class Timer {
    * Return the approximate match time. The FMS does not send an official match time to the robots,
    * but does send an approximate match time. The value will count down the time remaining in the
    * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
-   * dispute ref calls or guarantee that a function will trigger before the match ends) The Practice
-   * Match function of the DS approximates the behavior seen on the field.
+   * dispute ref calls or guarantee that a function will trigger before the match ends).
+   *
+   * <p>When connected to the real field, this number only changes in full integer increments, and
+   * always counts down.
+   *
+   * <p>When the DS is in practice mode, this number is a floating point number, and counts down.
+   *
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
+   *
+   * <p>Simulation matches DS behavior without an FMS connected.
    *
    * @return Time remaining in current match period (auto or teleop) in seconds
    */

wpilibjExamples/src/main/java/org/wpilib/examples/examples.json

@@ -706,6 +706,22 @@
       "xrp"
     ]
   },
+  {
+    "name": "XRP Timed",
+    "description": "An very basic timed robot program that can be used with the XRP reference robot design.",
+    "tags": [
+      "XRP",
+      "Differential Drive",
+      "Joystick"
+    ],
+    "foldername": "xrptimed",
+    "gradlebase": "javaxrp",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "extravendordeps": [
+      "xrp"
+    ]
+  },
   {
     "name": "Digital Communication Sample",
     "description": "Communicates with external devices (such as an Arduino) using the roboRIO's DIO.",

wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/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 org.wpilib.examples.xrptimed;
+
+import org.wpilib.framework.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);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Robot.java

@@ -0,0 +1,74 @@
+// 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 org.wpilib.examples.xrptimed;
+
+import org.wpilib.drive.DifferentialDrive;
+import org.wpilib.driverstation.Joystick;
+import org.wpilib.framework.TimedRobot;
+import org.wpilib.system.Timer;
+import org.wpilib.util.sendable.SendableRegistry;
+import org.wpilib.xrp.XRPMotor;
+
+/**
+ * The methods in this class are called automatically corresponding to each mode, as described in
+ * the TimedRobot documentation. If you change the name of this class or the package after creating
+ * this project, you must also update the manifest file in the resource directory.
+ */
+public class Robot extends TimedRobot {
+  private final XRPMotor m_leftDrive = new XRPMotor(0);
+  private final XRPMotor m_rightDrive = new XRPMotor(1);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
+  // Assumes a gamepad plugged into channel 0
+  private final Joystick m_controller = new Joystick(0);
+  private final Timer m_timer = new Timer();
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftDrive);
+    SendableRegistry.addChild(m_robotDrive, m_rightDrive);
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightDrive.setInverted(true);
+  }
+
+  /** This function is run once each time the robot enters autonomous mode. */
+  @Override
+  public void autonomousInit() {
+    m_timer.restart();
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {
+    // Drive for 2 seconds
+    if (m_timer.get() < 2.0) {
+      // Drive forwards half speed, make sure to turn input squaring off
+      m_robotDrive.arcadeDrive(0.5, 0.0, false);
+    } else {
+      m_robotDrive.stopMotor(); // stop robot
+    }
+  }
+
+  /** This function is called once each time the robot enters teleoperated mode. */
+  @Override
+  public void teleopInit() {}
+
+  /** This function is called periodically during teleoperated mode. */
+  @Override
+  public void teleopPeriodic() {
+    m_robotDrive.arcadeDrive(-m_controller.getRawAxis(2), -m_controller.getRawAxis(1));
+  }
+
+  /** This function is called once each time the robot enters test mode. */
+  @Override
+  public void testInit() {}
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java

@@ -11,6 +11,7 @@ import org.wpilib.math.linalg.Vector;
 import org.wpilib.math.numbers.N1;
 import org.wpilib.math.system.Discretization;
 import org.wpilib.math.system.LinearSystem;
+import org.wpilib.math.util.MathSharedStore;
 import org.wpilib.math.util.Num;
 import org.wpilib.math.util.StateSpaceUtil;
 
@@ -119,6 +120,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
     m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
 
     reset();
+    MathSharedStore.getMathShared().reportUsage("LinearQuadraticRegulator", "");
   }
 
   /**
@@ -158,6 +160,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
     m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
 
     reset();
+    MathSharedStore.getMathShared().reportUsage("LinearQuadraticRegulator", "");
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java

@@ -12,6 +12,7 @@ import org.wpilib.math.system.Discretization;
 import org.wpilib.math.system.LinearSystemUtil;
 import org.wpilib.math.system.NumericalIntegration;
 import org.wpilib.math.system.NumericalJacobian;
+import org.wpilib.math.util.MathSharedStore;
 import org.wpilib.math.util.Nat;
 import org.wpilib.math.util.Num;
 import org.wpilib.math.util.StateSpaceUtil;
@@ -164,6 +165,8 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
     }
 
     m_P = m_initP;
+
+    MathSharedStore.getMathShared().reportUsage("ExtendedKalmanFilter", "");
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilter.java

@@ -9,6 +9,7 @@ import org.wpilib.math.linalg.Matrix;
 import org.wpilib.math.numbers.N1;
 import org.wpilib.math.system.Discretization;
 import org.wpilib.math.system.LinearSystem;
+import org.wpilib.math.util.MathSharedStore;
 import org.wpilib.math.util.Nat;
 import org.wpilib.math.util.Num;
 import org.wpilib.math.util.StateSpaceUtil;
@@ -87,6 +88,8 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
     m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
 
     reset();
+
+    MathSharedStore.getMathShared().reportUsage("KalmanFilter", "");
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java

@@ -15,11 +15,9 @@ import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
 import org.wpilib.math.kinematics.Kinematics;
 import org.wpilib.math.kinematics.Odometry;
 import org.wpilib.math.linalg.Matrix;
-import org.wpilib.math.linalg.VecBuilder;
 import org.wpilib.math.numbers.N1;
 import org.wpilib.math.numbers.N3;
 import org.wpilib.math.util.MathSharedStore;
-import org.wpilib.math.util.Nat;
 
 /**
  * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder
@@ -38,8 +36,12 @@ import org.wpilib.math.util.Nat;
  */
 public class PoseEstimator<T> {
   private final Odometry<T> m_odometry;
-  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
-  private final Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
+
+  // Diagonal of process noise covariance matrix Q
+  private final double[] m_q = new double[] {0.0, 0.0, 0.0};
+
+  // Diagonal of Kalman gain matrix K
+  private final double[] m_vision_k = new double[] {0.0, 0.0, 0.0};
 
   private static final double kBufferDuration = 1.5;
   // Maps timestamps to odometry-only pose estimates
@@ -47,7 +49,9 @@ public class PoseEstimator<T> {
       TimeInterpolatableBuffer.createBuffer(kBufferDuration);
   // Maps timestamps to vision updates
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
-  // been no vision measurements after the last reset
+  // been no vision measurements after the last reset. May contain one entry while
+  // m_odometryPoseBuffer is empty to correct for translation/rotation after a call to
+  // ResetRotation/ResetTranslation.
   private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();
 
   private Pose2d m_poseEstimate;
@@ -75,9 +79,10 @@ public class PoseEstimator<T> {
     m_poseEstimate = m_odometry.getPose();
 
     for (int i = 0; i < 3; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+      m_q[i] = stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0);
     }
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    MathSharedStore.getMathShared().reportUsage("PoseEstimator", "");
   }
 
   /**
@@ -90,6 +95,7 @@ public class PoseEstimator<T> {
    *     theta]ᵀ, with units in meters and radians.
    */
   public final void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    // Diagonal of measurement noise covariance matrix R
     var r = new double[3];
     for (int i = 0; i < 3; ++i) {
       r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
@@ -98,11 +104,10 @@ public class PoseEstimator<T> {
     // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
     // and C = I. See wpimath/algorithms.md.
     for (int row = 0; row < 3; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
+      if (m_q[row] == 0.0) {
+        m_vision_k[row] = 0.0;
       } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+        m_vision_k[row] = m_q[row] / (m_q[row] + Math.sqrt(m_q[row] * r[row]));
       }
     }
   }
@@ -144,9 +149,22 @@ public class PoseEstimator<T> {
    */
   public void resetTranslation(Translation2d translation) {
     m_odometry.resetTranslation(translation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPose();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose rotation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose2d(translation, latestVisionUpdate.getValue().visionPose.getRotation()),
+              new Pose2d(translation, latestVisionUpdate.getValue().odometryPose.getRotation()));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
+    } else {
+      m_poseEstimate = m_odometry.getPose();
+    }
   }
 
   /**
@@ -156,9 +174,22 @@ public class PoseEstimator<T> {
    */
   public void resetRotation(Rotation2d rotation) {
     m_odometry.resetRotation(rotation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPose();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose translation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose2d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation),
+              new Pose2d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
+    } else {
+      m_poseEstimate = m_odometry.getPose();
+    }
   }
 
   /**
@@ -274,29 +305,23 @@ public class PoseEstimator<T> {
     var transform = visionRobotPose.minus(visionSample.get());
 
     // Step 5: We should not trust the transform entirely, so instead we scale this transform by a
-    // Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_transform =
-        m_visionK.times(
-            VecBuilder.fill(
-                transform.getX(), transform.getY(), transform.getRotation().getRadians()));
-
-    // Step 6: Convert back to Transform2d.
+    // Kalman gain matrix representing how much we trust vision measurements compared to our current
+    // pose. Then, we convert the result back to a Transform2d.
     var scaledTransform =
         new Transform2d(
-            k_times_transform.get(0, 0),
-            k_times_transform.get(1, 0),
-            Rotation2d.fromRadians(k_times_transform.get(2, 0)));
+            m_vision_k[0] * transform.getX(),
+            m_vision_k[1] * transform.getY(),
+            Rotation2d.fromRadians(m_vision_k[2] * transform.getRotation().getRadians()));
 
-    // Step 7: Calculate and record the vision update.
+    // Step 6: Calculate and record the vision update.
     var visionUpdate =
         new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
     m_visionUpdates.put(timestamp, visionUpdate);
 
-    // Step 8: Remove later vision measurements. (Matches previous behavior)
+    // Step 7: Remove later vision measurements. (Matches previous behavior)
     m_visionUpdates.tailMap(timestamp, false).entrySet().clear();
 
-    // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
+    // Step 8: Update latest pose estimate. Since we cleared all updates after this vision update,
     // it's guaranteed to be the latest vision update.
     m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
   }

wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java

@@ -18,12 +18,9 @@ import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
 import org.wpilib.math.kinematics.Kinematics;
 import org.wpilib.math.kinematics.Odometry3d;
 import org.wpilib.math.linalg.Matrix;
-import org.wpilib.math.linalg.VecBuilder;
 import org.wpilib.math.numbers.N1;
 import org.wpilib.math.numbers.N4;
-import org.wpilib.math.numbers.N6;
 import org.wpilib.math.util.MathSharedStore;
-import org.wpilib.math.util.Nat;
 
 /**
  * This class wraps {@link Odometry3d} to fuse latency-compensated vision measurements with encoder
@@ -46,8 +43,12 @@ import org.wpilib.math.util.Nat;
  */
 public class PoseEstimator3d<T> {
   private final Odometry3d<T> m_odometry;
-  private final Matrix<N4, N1> m_q = new Matrix<>(Nat.N4(), Nat.N1());
-  private final Matrix<N6, N6> m_visionK = new Matrix<>(Nat.N6(), Nat.N6());
+
+  // Diagonal of process noise covariance matrix Q
+  private final double[] m_q = new double[] {0.0, 0.0, 0.0, 0.0};
+
+  // Diagonal of Kalman gain matrix K
+  private final double[] m_vision_k = new double[] {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
   private static final double kBufferDuration = 1.5;
   // Maps timestamps to odometry-only pose estimates
@@ -55,7 +56,9 @@ public class PoseEstimator3d<T> {
       TimeInterpolatableBuffer.createBuffer(kBufferDuration);
   // Maps timestamps to vision updates
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
-  // been no vision measurements after the last reset
+  // been no vision measurements after the last reset. May contain one entry while
+  // m_odometryPoseBuffer is empty to correct for translation/rotation after a call to
+  // ResetRotation/ResetTranslation.
   private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();
 
   private Pose3d m_poseEstimate;
@@ -83,9 +86,10 @@ public class PoseEstimator3d<T> {
     m_poseEstimate = m_odometry.getPose();
 
     for (int i = 0; i < 4; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+      m_q[i] = stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0);
     }
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    MathSharedStore.getMathShared().reportUsage("PoseEstimator3d", "");
   }
 
   /**
@@ -98,6 +102,7 @@ public class PoseEstimator3d<T> {
    *     theta]ᵀ, with units in meters and radians.
    */
   public final void setVisionMeasurementStdDevs(Matrix<N4, N1> visionMeasurementStdDevs) {
+    // Diagonal of measurement covariance matrix R
     var r = new double[4];
     for (int i = 0; i < 4; ++i) {
       r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
@@ -106,17 +111,16 @@ public class PoseEstimator3d<T> {
     // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
     // and C = I. See wpimath/algorithms.md.
     for (int row = 0; row < 4; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
+      if (m_q[row] == 0.0) {
+        m_vision_k[row] = 0.0;
       } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+        m_vision_k[row] = m_q[row] / (m_q[row] + Math.sqrt(m_q[row] * r[row]));
       }
     }
     // Fill in the gains for the other components of the rotation vector
-    double angle_gain = m_visionK.get(3, 3);
-    m_visionK.set(4, 4, angle_gain);
-    m_visionK.set(5, 5, angle_gain);
+    double angle_gain = m_vision_k[3];
+    m_vision_k[4] = angle_gain;
+    m_vision_k[5] = angle_gain;
   }
 
   /**
@@ -156,9 +160,22 @@ public class PoseEstimator3d<T> {
    */
   public void resetTranslation(Translation3d translation) {
     m_odometry.resetTranslation(translation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPose();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose rotation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose3d(translation, latestVisionUpdate.getValue().visionPose.getRotation()),
+              new Pose3d(translation, latestVisionUpdate.getValue().odometryPose.getRotation()));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
+    } else {
+      m_poseEstimate = m_odometry.getPose();
+    }
   }
 
   /**
@@ -168,9 +185,22 @@ public class PoseEstimator3d<T> {
    */
   public void resetRotation(Rotation3d rotation) {
     m_odometry.resetRotation(rotation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPose();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose translation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose3d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation),
+              new Pose3d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
+    } else {
+      m_poseEstimate = m_odometry.getPose();
+    }
   }
 
   /**
@@ -286,38 +316,27 @@ public class PoseEstimator3d<T> {
     var transform = visionRobotPose.minus(visionSample.get());
 
     // Step 5: We should not trust the transform entirely, so instead we scale this transform by a
-    // Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_transform =
-        m_visionK.times(
-            VecBuilder.fill(
-                transform.getX(),
-                transform.getY(),
-                transform.getZ(),
-                transform.getRotation().getX(),
-                transform.getRotation().getY(),
-                transform.getRotation().getZ()));
-
-    // Step 6: Convert back to Transform3d.
+    // Kalman gain matrix representing how much we trust vision measurements compared to our current
+    // pose. Then we convert the result back to a Transform3d.
     var scaledTransform =
         new Transform3d(
-            k_times_transform.get(0, 0),
-            k_times_transform.get(1, 0),
-            k_times_transform.get(2, 0),
+            m_vision_k[0] * transform.getX(),
+            m_vision_k[1] * transform.getY(),
+            m_vision_k[2] * transform.getZ(),
             new Rotation3d(
-                k_times_transform.get(3, 0),
-                k_times_transform.get(4, 0),
-                k_times_transform.get(5, 0)));
+                m_vision_k[3] * transform.getRotation().getX(),
+                m_vision_k[4] * transform.getRotation().getY(),
+                m_vision_k[5] * transform.getRotation().getZ()));
 
-    // Step 7: Calculate and record the vision update.
+    // Step 6: Calculate and record the vision update.
     var visionUpdate =
         new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
     m_visionUpdates.put(timestamp, visionUpdate);
 
-    // Step 8: Remove later vision measurements. (Matches previous behavior)
+    // Step 7: Remove later vision measurements. (Matches previous behavior)
     m_visionUpdates.tailMap(timestamp, false).entrySet().clear();
 
-    // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
+    // Step 8: Update latest pose estimate. Since we cleared all updates after this vision update,
     // it's guaranteed to be the latest vision update.
     m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
   }

wpimath/src/main/java/org/wpilib/math/estimator/SteadyStateKalmanFilter.java

@@ -9,6 +9,7 @@ import org.wpilib.math.linalg.Matrix;
 import org.wpilib.math.numbers.N1;
 import org.wpilib.math.system.Discretization;
 import org.wpilib.math.system.LinearSystem;
+import org.wpilib.math.util.MathSharedStore;
 import org.wpilib.math.util.Nat;
 import org.wpilib.math.util.Num;
 import org.wpilib.math.util.StateSpaceUtil;
@@ -107,6 +108,7 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
     m_K = new Matrix<>(S.getStorage().solve(C.times(P).getStorage()).transpose());
 
     reset();
+    MathSharedStore.getMathShared().reportUsage("SteadyStateKalmanFilter", "");
   }
 
   /** Resets the observer. */

wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java

@@ -12,6 +12,7 @@ import org.wpilib.math.numbers.N1;
 import org.wpilib.math.system.Discretization;
 import org.wpilib.math.system.NumericalIntegration;
 import org.wpilib.math.system.NumericalJacobian;
+import org.wpilib.math.util.MathSharedStore;
 import org.wpilib.math.util.Nat;
 import org.wpilib.math.util.Num;
 import org.wpilib.math.util.Pair;
@@ -173,6 +174,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     m_pts = pts;
 
     reset();
+    MathSharedStore.getMathShared().reportUsage("UnscentedKalmanFilter", "");
   }
 
   static <C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> squareRootUnscentedTransform(

wpimath/src/main/java/org/wpilib/math/filter/EdgeCounterFilter.java

@@ -0,0 +1,120 @@
+// 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 org.wpilib.math.filter;
+
+import org.wpilib.math.util.MathSharedStore;
+
+/**
+ * A rising edge counter for boolean streams. Requires that the boolean change value to true for a
+ * specified number of times within a specified time window after the first rising edge before the
+ * filtered value changes.
+ *
+ * <p>The filter activates when the input has risen (transitioned from false to true) the required
+ * number of times within the time window. Once activated, the output remains true as long as the
+ * input is true. The edge count resets when the time window expires or when the input goes false
+ * after activation.
+ *
+ * <p>Input must be stable; consider using a Debouncer before this filter to avoid counting noise as
+ * multiple edges.
+ */
+public class EdgeCounterFilter {
+  private int m_requiredEdges;
+  private double m_windowTimeSeconds;
+
+  private double m_firstEdgeTimeSeconds;
+  private int m_currentCount;
+
+  private boolean m_lastInput;
+
+  /**
+   * Creates a new EdgeCounterFilter.
+   *
+   * @param requiredEdges The number of rising edges required before the output goes true.
+   * @param windowTime The maximum number of seconds in which all required edges must occur after
+   *     the first rising edge.
+   */
+  public EdgeCounterFilter(int requiredEdges, double windowTime) {
+    m_requiredEdges = requiredEdges;
+    m_windowTimeSeconds = windowTime;
+
+    resetTimer();
+  }
+
+  private void resetTimer() {
+    m_firstEdgeTimeSeconds = MathSharedStore.getTimestamp();
+  }
+
+  private boolean hasElapsed() {
+    return MathSharedStore.getTimestamp() - m_firstEdgeTimeSeconds >= m_windowTimeSeconds;
+  }
+
+  /**
+   * Applies the edge counter filter to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return True if the required number of edges have occurred within the time window and the input
+   *     is currently true; false otherwise.
+   */
+  public boolean calculate(boolean input) {
+    boolean enoughEdges = m_currentCount >= m_requiredEdges;
+
+    boolean expired = hasElapsed() && !enoughEdges;
+    boolean activationEnded = !input && enoughEdges;
+
+    if (expired || activationEnded) {
+      m_currentCount = 0;
+    }
+
+    if (input && !m_lastInput) {
+      if (m_currentCount == 0) {
+        resetTimer(); // Start timer on first rising edge
+      }
+
+      m_currentCount++;
+    }
+
+    m_lastInput = input;
+
+    return input && m_currentCount >= m_requiredEdges;
+  }
+
+  /**
+   * Sets the time window duration.
+   *
+   * @param windowTime The maximum window of seconds in which all required edges must occur after
+   *     the first rising edge.
+   */
+  public void setWindowTime(double windowTime) {
+    m_windowTimeSeconds = windowTime;
+  }
+
+  /**
+   * Gets the time window duration.
+   *
+   * @return The maximum window of seconds in which all required edges must occur after the first
+   *     rising edge.
+   */
+  public double getWindowTime() {
+    return m_windowTimeSeconds;
+  }
+
+  /**
+   * Sets the required number of edges.
+   *
+   * @param requiredEdges The number of rising edges required before the output goes true.
+   */
+  public void setRequiredEdges(int requiredEdges) {
+    m_requiredEdges = requiredEdges;
+  }
+
+  /**
+   * Gets the required number of edges.
+   *
+   * @return The number of rising edges required before the output goes true.
+   */
+  public int getRequiredEdges() {
+    return m_requiredEdges;
+  }
+}

wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java

@@ -16,7 +16,7 @@ public class CoordinateSystem {
   private static final CoordinateSystem m_ned =
       new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
 
-  // Rotation from this coordinate system to NWU coordinate system
+  // Rotation from this coordinate system to NWU coordinate system when applied extrinsically
   private final Rotation3d m_rotation;
 
   /**
@@ -85,7 +85,8 @@ public class CoordinateSystem {
    */
   public static Translation3d convert(
       Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
-    return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
+    // Convert to NWU, then convert to the new coordinate system
+    return translation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus());
   }
 
   /**
@@ -98,7 +99,8 @@ public class CoordinateSystem {
    */
   public static Rotation3d convert(
       Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
-    return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
+    // Convert to NWU, then convert to the new coordinate system
+    return rotation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus());
   }
 
   /**
@@ -124,9 +126,23 @@ public class CoordinateSystem {
    */
   public static Transform3d convert(
       Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
-    var coordRot = from.m_rotation.minus(to.m_rotation);
+    // coordRot is the rotation that converts between the coordinate systems when applied
+    // extrinsically. It first converts to NWU, then converts to the new coordinate system.
+    var coordRot = from.m_rotation.rotateBy(to.m_rotation.unaryMinus());
+    // The new rotation is the extrinsic rotation from convert(zero) to
+    // convert(transformRot). That is, applying convertedRot extrinsically to
+    // convert(zero) produces convert(transformRot). In the below snippet, we
+    // use matrix notation, so rotA rotB applies rotA extrinsically to rotB.
+    //
+    //   convertedRot convert(zero) = convert(transformRot)
+    //   convertedRot = convert(transformRot) convert(zero)⁻¹
+    //                = (coordRot transformRot) (coordRot zero)⁻¹
+    //                = (coordRot transformRot) coordRot⁻¹
+    //
+    // In code, the equivalent for rotA rotB is rotB.rotateBy(rotA) (note the
+    // change in order), and the equivalent for rot⁻¹ is rot.unaryMinus().
     return new Transform3d(
         convert(transform.getTranslation(), from, to),
-        coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot)));
+        coordRot.unaryMinus().rotateBy(transform.getRotation().rotateBy(coordRot)));
   }
 }

wpimath/src/main/java/org/wpilib/math/geometry/Pose2d.java

@@ -220,7 +220,7 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
   public Pose2d transformBy(Transform2d other) {
     return new Pose2d(
         m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
-        other.getRotation().plus(m_rotation));
+        other.getRotation().rotateBy(m_rotation));
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java

@@ -253,9 +253,12 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
    * @return The transformed pose.
    */
   public Pose3d transformBy(Transform3d other) {
+    // Rotating the transform's rotation by the pose's rotation extrinsically is equivalent to
+    // rotating the pose's rotation by the transform's rotation intrinsically. (We define transforms
+    // as being applied intrinsically.)
     return new Pose3d(
         m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
-        other.getRotation().plus(m_rotation));
+        other.getRotation().rotateBy(m_rotation));
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java

@@ -264,6 +264,17 @@ public class Rotation2d
         m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
   }
 
+  /**
+   * Returns the current rotation relative to the given rotation.
+   *
+   * @param other The rotation describing the orientation of the new coordinate frame that the
+   *     current rotation will be converted into.
+   * @return The current rotation relative to the new orientation of the coordinate frame.
+   */
+  public Rotation2d relativeTo(Rotation2d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
   /**
    * Returns matrix representation of this rotation.
    *

wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java

@@ -25,7 +25,49 @@ import org.wpilib.units.measure.Angle;
 import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;
 
-/** A rotation in a 3D coordinate frame represented by a quaternion. */
+/**
+ * A rotation in a 3D coordinate frame, represented by a quaternion. Note that unlike 2D rotations,
+ * 3D rotations are not always commutative, so changing the order of rotations changes the result.
+ *
+ * <p>As an example of the order of rotations mattering, suppose we have a card that looks like
+ * this:
+ *
+ * <pre>
+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ * </pre>
+ *
+ * <p>If we rotate 90º clockwise around the axis into the page, then flip around the left/right
+ * axis, we get one result:
+ *
+ * <pre>
+ *   ┌───┐
+ *   │ X │   ┌───────┐   ┌───────┐
+ *   │ | │ → │------X│ → │······x│
+ *   │ | │   └───────┘   └───────┘
+ *   └───┘
+ * </pre>
+ *
+ * <p>If we flip around the left/right axis, then rotate 90º clockwise around the axis into the
+ * page, we get a different result:
+ *
+ * <pre>
+ *   ┌───┐   ┌───┐
+ *   │ X │   │ · │   ┌───────┐
+ *   │ | │ → │ · │ → │x······│
+ *   │ | │   │ x │   └───────┘
+ *   └───┘   └───┘
+ * </pre>
+ *
+ * <p>Because order matters for 3D rotations, we need to distinguish between <em>extrinsic</em>
+ * rotations and <em>intrinsic</em> rotations. Rotating extrinsically means rotating around the
+ * global axes, while rotating intrinsically means rotating from the perspective of the other
+ * rotation. A neat property is that applying a series of rotations extrinsically is the same as
+ * applying the same series in the opposite order intrinsically.
+ */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d
@@ -285,9 +327,17 @@ public class Rotation3d
   }
 
   /**
-   * Adds two rotations together.
+   * Adds two rotations together. The other rotation is applied extrinsically to this rotation,
+   * which is equivalent to this rotation being applied intrinsically to the other rotation. See the
+   * class comment for definitions of extrinsic and intrinsic rotations.
    *
-   * @param other The rotation to add.
+   * <p>Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))}
+   * sometimes doesn't. To apply a rotation offset, use either {@code offset =
+   * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset =
+   * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected
+   * angle needs to change as the input angle changes.
+   *
+   * @param other The rotation to add (applied extrinsically).
    * @return The sum of the two rotations.
    */
   public Rotation3d plus(Rotation3d other) {
@@ -295,10 +345,19 @@ public class Rotation3d
   }
 
   /**
-   * Subtracts the new rotation from the current rotation and returns the new rotation.
+   * Subtracts the other rotation from the current rotation and returns the new rotation. The new
+   * rotation is from the perspective of the other rotation (like {@link Pose3d#minus}), so it needs
+   * to be applied intrinsically. See the class comment for definitions of extrinsic and intrinsic
+   * rotations.
+   *
+   * <p>Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))}
+   * sometimes doesn't. To apply a rotation offset, use either {@code offset =
+   * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset =
+   * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected
+   * angle needs to change as the input angle changes.
    *
    * @param other The rotation to subtract.
-   * @return The difference between the two rotations.
+   * @return The difference between the two rotations, from the perspective of the other rotation.
    */
   public Rotation3d minus(Rotation3d other) {
     return rotateBy(other.unaryMinus());
@@ -357,6 +416,24 @@ public class Rotation3d
     return new Rotation3d(other.m_q.times(m_q));
   }
 
+  /**
+   * Returns the current rotation relative to the given rotation. Because the result is in the
+   * perspective of the given rotation, it must be applied intrinsically. See the class comment for
+   * definitions of extrinsic and intrinsic rotations.
+   *
+   * @param other The rotation describing the orientation of the new coordinate frame that the
+   *     current rotation will be converted into.
+   * @return The current rotation relative to the new orientation of the coordinate frame.
+   */
+  public Rotation3d relativeTo(Rotation3d other) {
+    // To apply a quaternion intrinsically, we must right-multiply by that quaternion.
+    // Therefore, "this_q relative to other_q" is the q such that other_q q = this_q:
+    //
+    //   other_q q = this_q
+    //   q = other_q⁻¹ this_q
+    return new Rotation3d(other.m_q.inverse().times(m_q));
+  }
+
   /**
    * Returns the quaternion representation of the Rotation3d.
    *
@@ -561,7 +638,7 @@ public class Rotation3d
 
   @Override
   public Rotation3d interpolate(Rotation3d endValue, double t) {
-    return plus(endValue.minus(this).times(Math.clamp(t, 0, 1)));
+    return endValue.minus(this).times(Math.clamp(t, 0, 1)).plus(this);
   }
 
   /** Rotation3d protobuf for serialization. */

wpimath/src/main/java/org/wpilib/math/geometry/Transform2d.java

@@ -36,15 +36,14 @@ public class Transform2d implements ProtobufSerializable, StructSerializable {
    * @param last The final pose for the transformation.
    */
   public Transform2d(Pose2d initial, Pose2d last) {
-    // We are rotating the difference between the translations
-    // using a clockwise rotation matrix. This transforms the global
-    // delta into a local delta (relative to the initial pose).
+    // To transform the global translation delta to be relative to the initial
+    // pose, rotate by the inverse of the initial pose's orientation.
     m_translation =
         last.getTranslation()
             .minus(initial.getTranslation())
             .rotateBy(initial.getRotation().unaryMinus());
 
-    m_rotation = last.getRotation().minus(initial.getRotation());
+    m_rotation = last.getRotation().relativeTo(initial.getRotation());
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java

@@ -18,7 +18,10 @@ import org.wpilib.units.measure.Distance;
 import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;
 
-/** Represents a transformation for a Pose3d in the pose's frame. */
+/**
+ * Represents a transformation for a Pose3d in the pose's frame. Translation is applied before
+ * rotation. (The translation is applied in the pose's original frame, not the transformed frame.)
+ */
 public class Transform3d implements ProtobufSerializable, StructSerializable {
   /**
    * A preallocated Transform3d representing no transformation.
@@ -37,15 +40,14 @@ public class Transform3d implements ProtobufSerializable, StructSerializable {
    * @param last The final pose for the transformation.
    */
   public Transform3d(Pose3d initial, Pose3d last) {
-    // We are rotating the difference between the translations
-    // using a clockwise rotation matrix. This transforms the global
-    // delta into a local delta (relative to the initial pose).
+    // To transform the global translation delta to be relative to the initial
+    // pose, rotate by the inverse of the initial pose's orientation.
     m_translation =
         last.getTranslation()
             .minus(initial.getTranslation())
             .rotateBy(initial.getRotation().unaryMinus());
 
-    m_rotation = last.getRotation().minus(initial.getRotation());
+    m_rotation = last.getRotation().relativeTo(initial.getRotation());
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java

@@ -24,7 +24,10 @@ public class Odometry<T> {
   private Pose2d m_pose;
 
   private Rotation2d m_gyroOffset;
+
+  // Always equal to m_poseMeters.getRotation()
   private Rotation2d m_previousAngle;
+
   private final T m_previousWheelPositions;
 
   /**
@@ -39,7 +42,7 @@ public class Odometry<T> {
       Kinematics<T, ?, ?> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) {
     m_kinematics = kinematics;
     m_pose = initialPose;
-    m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
+    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation());
     m_previousAngle = m_pose.getRotation();
     m_previousWheelPositions = m_kinematics.copy(wheelPositions);
   }
@@ -56,8 +59,8 @@ public class Odometry<T> {
    */
   public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) {
     m_pose = pose;
+    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation());
     m_previousAngle = m_pose.getRotation();
-    m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
     m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
   }
 
@@ -67,7 +70,8 @@ public class Odometry<T> {
    * @param pose The pose to reset to.
    */
   public void resetPose(Pose2d pose) {
-    m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation()));
+    m_gyroOffset =
+        m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(pose.getRotation());
     m_pose = pose;
     m_previousAngle = m_pose.getRotation();
   }
@@ -87,7 +91,7 @@ public class Odometry<T> {
    * @param rotation The rotation to reset to.
    */
   public void resetRotation(Rotation2d rotation) {
-    m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation()));
+    m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(rotation);
     m_pose = new Pose2d(m_pose.getTranslation(), rotation);
     m_previousAngle = m_pose.getRotation();
   }
@@ -112,7 +116,7 @@ public class Odometry<T> {
    * @return The new pose of the robot.
    */
   public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
-    var angle = gyroAngle.plus(m_gyroOffset);
+    var angle = gyroAngle.rotateBy(m_gyroOffset);
 
     var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
     twist.dtheta = angle.minus(m_previousAngle).getRadians();

wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java

@@ -32,8 +32,15 @@ public class Odometry3d<T> {
   private final Kinematics<T, ?, ?> m_kinematics;
   private Pose3d m_pose;
 
+  // Applying a rotation intrinsically to the measured gyro angle should cause the corrected angle
+  // to be rotated intrinsically in the same way, so the measured gyro angle must be applied
+  // intrinsically. This is equivalent to applying the offset extrinsically to the measured gyro
+  // angle.
   private Rotation3d m_gyroOffset;
+
+  // Always equal to m_poseMeters.getRotation()
   private Rotation3d m_previousAngle;
+
   private final T m_previousWheelPositions;
 
   /**
@@ -48,7 +55,9 @@ public class Odometry3d<T> {
       Kinematics<T, ?, ?> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) {
     m_kinematics = kinematics;
     m_pose = initialPose;
-    m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
+    // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and
+    // then rotates to m_poseMeters.getRotation()
+    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation());
     m_previousAngle = m_pose.getRotation();
     m_previousWheelPositions = m_kinematics.copy(wheelPositions);
   }
@@ -65,8 +74,10 @@ public class Odometry3d<T> {
    */
   public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) {
     m_pose = pose;
+    // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and
+    // then rotates to m_poseMeters.getRotation()
+    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation());
     m_previousAngle = m_pose.getRotation();
-    m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
     m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
   }
 
@@ -76,7 +87,9 @@ public class Odometry3d<T> {
    * @param pose The pose to reset to.
    */
   public void resetPose(Pose3d pose) {
-    m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation()));
+    // Cancel the previous m_pose.Rotation() and then rotate to the new angle
+    m_gyroOffset =
+        m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(pose.getRotation());
     m_pose = pose;
     m_previousAngle = m_pose.getRotation();
   }
@@ -96,7 +109,8 @@ public class Odometry3d<T> {
    * @param rotation The rotation to reset to.
    */
   public void resetRotation(Rotation3d rotation) {
-    m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation()));
+    // Cancel the previous m_pose.Rotation() and then rotate to the new angle
+    m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(rotation);
     m_pose = new Pose3d(m_pose.getTranslation(), rotation);
     m_previousAngle = m_pose.getRotation();
   }
@@ -121,7 +135,7 @@ public class Odometry3d<T> {
    * @return The new pose of the robot.
    */
   public Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
-    var angle = gyroAngle.plus(m_gyroOffset);
+    var angle = gyroAngle.rotateBy(m_gyroOffset);
     var angle_difference = angle.minus(m_previousAngle).toVector();
 
     var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);

wpimath/src/main/java/org/wpilib/math/system/LinearSystemLoop.java

@@ -12,6 +12,7 @@ import org.wpilib.math.controller.LinearQuadraticRegulator;
 import org.wpilib.math.estimator.KalmanFilter;
 import org.wpilib.math.linalg.Matrix;
 import org.wpilib.math.numbers.N1;
+import org.wpilib.math.util.MathSharedStore;
 import org.wpilib.math.util.Num;
 import org.wpilib.math.util.StateSpaceUtil;
 
@@ -128,6 +129,7 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs ex
 
     m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
     reset(m_nextR);
+    MathSharedStore.getMathShared().reportUsage("LinearSystemLoop", "");
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java

@@ -7,6 +7,7 @@ package org.wpilib.math.trajectory;
 import java.util.Objects;
 import org.wpilib.math.trajectory.struct.TrapezoidProfileStateStruct;
 import org.wpilib.math.util.MathSharedStore;
+import org.wpilib.util.struct.StructSerializable;
 
 /**
  * A trapezoid-shaped velocity profile.
@@ -75,7 +76,7 @@ public class TrapezoidProfile {
   }
 
   /** Profile state. */
-  public static class State {
+  public static class State implements StructSerializable {
     /** The struct used to serialize this class. */
     public static final TrapezoidProfileStateStruct struct = new TrapezoidProfileStateStruct();
 

wpimath/src/main/native/cpp/filter/EdgeCounterFilter.cpp

@@ -0,0 +1,46 @@
+// 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 "wpi/math/filter/EdgeCounterFilter.hpp"
+
+#include "wpi/math/util/MathShared.hpp"
+
+using namespace wpi::math;
+
+EdgeCounterFilter::EdgeCounterFilter(int requiredEdges, units::second_t window)
+    : m_requiredEdges(requiredEdges), m_windowTime(window) {
+  ResetTimer();
+}
+
+void EdgeCounterFilter::ResetTimer() {
+  m_firstEdgeTime = wpi::math::MathSharedStore::GetTimestamp();
+}
+
+bool EdgeCounterFilter::HasElapsed() const {
+  return wpi::math::MathSharedStore::GetTimestamp() - m_firstEdgeTime >=
+         m_windowTime;
+}
+
+bool EdgeCounterFilter::Calculate(bool input) {
+  bool enoughEdges = m_currentCount >= m_requiredEdges;
+
+  bool expired = HasElapsed() && !enoughEdges;
+  bool activationEnded = !input && enoughEdges;
+
+  if (expired || activationEnded) {
+    m_currentCount = 0;
+  }
+
+  if (input && !m_lastInput) {
+    if (m_currentCount == 0) {
+      ResetTimer();  // Start timer on first rising edge
+    }
+
+    m_currentCount++;
+  }
+
+  m_lastInput = input;
+
+  return input && m_currentCount >= m_requiredEdges;
+}

wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp

@@ -135,6 +135,7 @@ class LinearQuadraticRegulator {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage("LinearQuadraticRegulator", "");
   }
 
   /**
@@ -194,6 +195,7 @@ class LinearQuadraticRegulator {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage("LinearQuadraticRegulator", "");
   }
 
   LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;

wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp

@@ -138,6 +138,7 @@ class ExtendedKalmanFilter {
       m_initP = StateMatrix::Zero();
     }
     m_P = m_initP;
+    wpi::math::MathSharedStore::ReportUsage("ExtendedKalmanFilter", "");
   }
 
   /**
@@ -222,6 +223,7 @@ class ExtendedKalmanFilter {
       m_initP = StateMatrix::Zero();
     }
     m_P = m_initP;
+    wpi::math::MathSharedStore::ReportUsage("ExtendedKalmanFilter", "");
   }
 
   /**

wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp

@@ -117,6 +117,7 @@ class KalmanFilter {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage("KalmanFilter", "");
   }
 
   /**

wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp

@@ -74,6 +74,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
     }
 
     SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    wpi::math::MathSharedStore::ReportUsage("PoseEstimator", "");
   }
 
   /**
@@ -88,6 +89,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
    */
   void SetVisionMeasurementStdDevs(
       const wpi::util::array<double, 3>& visionMeasurementStdDevs) {
+    // Diagonal of measurement covariance matrix R
     wpi::util::array<double, 3> r{wpi::util::empty_array};
     for (size_t i = 0; i < 3; ++i) {
       r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
@@ -97,9 +99,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
     // and C = I. See wpimath/algorithms.md.
     for (size_t row = 0; row < 3; ++row) {
       if (m_q[row] == 0.0) {
-        m_visionK(row, row) = 0.0;
+        m_vision_K.diagonal()[row] = 0.0;
       } else {
-        m_visionK(row, row) =
+        m_vision_K.diagonal()[row] =
             m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
       }
     }
@@ -141,24 +143,70 @@ class WPILIB_DLLEXPORT PoseEstimator {
    *
    * @param translation The pose to translation to.
    */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
   void ResetTranslation(const Translation2d& translation) {
     m_odometry.ResetTranslation(translation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose rotation
+      const VisionUpdate visionUpdate{
+          Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
+          Pose2d{translation,
+                 latestVisionUpdate->second.odometryPose.Rotation()}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Resets the robot's rotation.
    *
    * @param rotation The rotation to reset to.
    */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
   void ResetRotation(const Rotation2d& rotation) {
     m_odometry.ResetRotation(rotation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose translation
+      const VisionUpdate visionUpdate{
+          Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
+          Pose2d{latestVisionUpdate->second.odometryPose.Translation(),
+                 rotation}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Gets the estimated robot pose.
@@ -274,9 +322,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
     // this transform by a Kalman gain matrix representing how much we trust
     // vision measurements compared to our current pose.
     Eigen::Vector3d k_times_transform =
-        m_visionK * Eigen::Vector3d{transform.X().value(),
-                                    transform.Y().value(),
-                                    transform.Rotation().Radians().value()};
+        m_vision_K * Eigen::Vector3d{transform.X().value(),
+                                     transform.Y().value(),
+                                     transform.Rotation().Radians().value()};
 
     // Step 6: Convert back to Transform2d.
     Transform2d scaledTransform{
@@ -431,14 +479,21 @@ class WPILIB_DLLEXPORT PoseEstimator {
   static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
 
   Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
+
+  // Diagonal of process noise covariance matrix Q
   wpi::util::array<double, 3> m_q{wpi::util::empty_array};
-  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+  // Kalman gain matrix K
+  Eigen::DiagonalMatrix<double, 3> m_vision_K =
+      Eigen::DiagonalMatrix<double, 3>::Zero();
 
   // Maps timestamps to odometry-only pose estimates
   TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};
   // Maps timestamps to vision updates
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
-  // unless there have been no vision measurements after the last reset
+  // unless there have been no vision measurements after the last reset. May
+  // contain one entry while m_odometryPoseBuffer is empty to correct for
+  // translation/rotation after a call to ResetRotation/ResetTranslation.
   std::map<wpi::units::second_t, VisionUpdate> m_visionUpdates;
 
   Pose2d m_poseEstimate;

wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp

@@ -79,6 +79,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     }
 
     SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    wpi::math::MathSharedStore::ReportUsage("PoseEstimator3d", "");
   }
 
   /**
@@ -93,6 +94,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
    */
   void SetVisionMeasurementStdDevs(
       const wpi::util::array<double, 4>& visionMeasurementStdDevs) {
+    // Diagonal of measurement noise covariance matrix R
     wpi::util::array<double, 4> r{wpi::util::empty_array};
     for (size_t i = 0; i < 4; ++i) {
       r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
@@ -102,15 +104,15 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     // and C = I. See wpimath/algorithms.md.
     for (size_t row = 0; row < 4; ++row) {
       if (m_q[row] == 0.0) {
-        m_visionK(row, row) = 0.0;
+        m_vision_K.diagonal()[row] = 0.0;
       } else {
-        m_visionK(row, row) =
+        m_vision_K.diagonal()[row] =
             m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
       }
     }
-    double angle_gain = m_visionK(3, 3);
-    m_visionK(4, 4) = angle_gain;
-    m_visionK(5, 5) = angle_gain;
+    double angle_gain = m_vision_K.diagonal()[3];
+    m_vision_K.diagonal()[4] = angle_gain;
+    m_vision_K.diagonal()[5] = angle_gain;
   }
 
   /**
@@ -149,24 +151,70 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
    *
    * @param translation The pose to translation to.
    */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
   void ResetTranslation(const Translation3d& translation) {
     m_odometry.ResetTranslation(translation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose rotation
+      const VisionUpdate visionUpdate{
+          Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
+          Pose3d{translation,
+                 latestVisionUpdate->second.odometryPose.Rotation()}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Resets the robot's rotation.
    *
    * @param rotation The rotation to reset to.
    */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
   void ResetRotation(const Rotation3d& rotation) {
     m_odometry.ResetRotation(rotation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose translation
+      const VisionUpdate visionUpdate{
+          Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
+          Pose3d{latestVisionUpdate->second.odometryPose.Translation(),
+                 rotation}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Gets the estimated robot pose.
@@ -282,12 +330,12 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     // this transform by a Kalman gain matrix representing how much we trust
     // vision measurements compared to our current pose.
     wpi::math::Vectord<6> k_times_transform =
-        m_visionK * wpi::math::Vectord<6>{transform.X().value(),
-                                          transform.Y().value(),
-                                          transform.Z().value(),
-                                          transform.Rotation().X().value(),
-                                          transform.Rotation().Y().value(),
-                                          transform.Rotation().Z().value()};
+        m_vision_K * wpi::math::Vectord<6>{transform.X().value(),
+                                           transform.Y().value(),
+                                           transform.Z().value(),
+                                           transform.Rotation().X().value(),
+                                           transform.Rotation().Y().value(),
+                                           transform.Rotation().Z().value()};
 
     // Step 6: Convert back to Transform3d.
     Transform3d scaledTransform{
@@ -445,14 +493,21 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
   static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
 
   Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
+
+  // Diagonal of process noise covariance matrix Q
   wpi::util::array<double, 4> m_q{wpi::util::empty_array};
-  wpi::math::Matrixd<6, 6> m_visionK = wpi::math::Matrixd<6, 6>::Zero();
+
+  // Kalman gain matrix K
+  Eigen::DiagonalMatrix<double, 6> m_vision_K =
+      Eigen::DiagonalMatrix<double, 6>::Zero();
 
   // Maps timestamps to odometry-only pose estimates
   TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};
   // Maps timestamps to vision updates
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
-  // unless there have been no vision measurements after the last reset
+  // unless there have been no vision measurements after the last reset. May
+  // contain one entry while m_odometryPoseBuffer is empty to correct for
+  // translation/rotation after a call to ResetRotation/ResetTranslation.
   std::map<wpi::units::second_t, VisionUpdate> m_visionUpdates;
 
   Pose3d m_poseEstimate;

wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp

@@ -148,6 +148,7 @@ class SteadyStateKalmanFilter {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage("KalmanFilter", "");
   }
 
   SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default;

wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp

@@ -15,6 +15,7 @@
 #include "wpi/math/system/Discretization.hpp"
 #include "wpi/math/system/NumericalIntegration.hpp"
 #include "wpi/math/system/NumericalJacobian.hpp"
+#include "wpi/math/util/MathShared.hpp"
 #include "wpi/math/util/StateSpaceUtil.hpp"
 #include "wpi/units/time.hpp"
 #include "wpi/util/array.hpp"
@@ -114,6 +115,7 @@ class UnscentedKalmanFilter {
     m_dt = dt;
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage("UnscentedKalmanFilter", "");
   }
 
   /**
@@ -173,6 +175,7 @@ class UnscentedKalmanFilter {
     m_dt = dt;
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage("UnscentedKalmanFilter", "");
   }
 
   /**

wpimath/src/main/native/include/wpi/math/filter/EdgeCounterFilter.hpp

@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "wpi/units/time.hpp"
+#include "wpi/util/SymbolExports.hpp"
+
+namespace wpi::math {
+/**
+ * A rising edge counter for boolean streams. Requires that the boolean change
+ * value to true for a specified number of times within a specified time window
+ * after the first rising edge before the filtered value changes.
+ *
+ * The filter activates when the input has risen (transitioned from false to
+ * true) the required number of times within the time window. Once activated,
+ * the output remains true as long as the input is true. The edge count resets
+ * when the time window expires or when the input goes false after activation.
+ *
+ * Input must be stable; consider using a Debouncer before this filter to avoid
+ * counting noise as multiple edges.
+ */
+class WPILIB_DLLEXPORT EdgeCounterFilter {
+ public:
+  /**
+   * Creates a new EdgeCounterFilter.
+   *
+   * @param requiredEdges The number of rising edges required before the output
+   *                      goes true.
+   * @param windowTime The maximum time window in which all required edges must
+   *               occur after the first rising edge.
+   */
+  explicit EdgeCounterFilter(int requiredEdges,
+                             wpi::units::second_t windowTime);
+
+  /**
+   * Applies the edge counter filter to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return True if the required number of edges have occurred within the time
+   *         window and the input is currently true; false otherwise.
+   */
+  bool Calculate(bool input);
+
+  /**
+   * Sets the time window duration.
+   *
+   * @param windowTime The maximum time window in which all required edges must
+   *               occur after the first rising edge.
+   */
+  constexpr void SetWindowTime(wpi::units::second_t windowTime) {
+    m_windowTime = windowTime;
+  }
+
+  /**
+   * Gets the time window duration.
+   *
+   * @return The maximum time window in which all required edges must occur
+   *         after the first rising edge.
+   */
+  constexpr wpi::units::second_t GetWindowTime() const { return m_windowTime; }
+
+  /**
+   * Sets the required number of edges.
+   *
+   * @param requiredEdges The number of rising edges required before the output
+   *                      goes true.
+   */
+  constexpr void SetRequiredEdges(int requiredEdges) {
+    m_requiredEdges = requiredEdges;
+  }
+
+  /**
+   * Gets the required number of edges.
+   *
+   * @return The number of rising edges required before the output goes true.
+   */
+  constexpr int GetRequiredEdges() const { return m_requiredEdges; }
+
+ private:
+  int m_requiredEdges;
+  wpi::units::second_t m_windowTime;
+
+  wpi::units::second_t m_firstEdgeTime;
+  int m_currentCount = 0;
+
+  bool m_lastInput = false;
+
+  void ResetTimer();
+
+  bool HasElapsed() const;
+};
+}  // namespace wpi::math

wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp

@@ -85,7 +85,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Translation3d Convert(const Translation3d& translation,
                                          const CoordinateSystem& from,
                                          const CoordinateSystem& to) {
-    return translation.RotateBy(from.m_rotation - to.m_rotation);
+    // Convert to NWU, then convert to the new coordinate system
+    return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
   }
 
   /**
@@ -99,7 +100,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Rotation3d Convert(const Rotation3d& rotation,
                                       const CoordinateSystem& from,
                                       const CoordinateSystem& to) {
-    return rotation.RotateBy(from.m_rotation - to.m_rotation);
+    // Convert to NWU, then convert to the new coordinate system
+    return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
   }
 
   /**
@@ -128,14 +130,30 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Transform3d Convert(const Transform3d& transform,
                                        const CoordinateSystem& from,
                                        const CoordinateSystem& to) {
-    const auto coordRot = from.m_rotation - to.m_rotation;
+    // coordRot is the rotation that converts between the coordinate systems
+    // when applied extrinsically. It first converts to NWU, then converts to
+    // the new coordinate system.
+    const auto coordRot = from.m_rotation.RotateBy(-to.m_rotation);
+    // The new rotation is the extrinsic rotation from convert(zero) to
+    // convert(transformRot). That is, applying convertedRot extrinsically to
+    // convert(zero) produces convert(transformRot). In the below snippet, we
+    // use matrix notation, so rotA rotB applies rotA extrinsically to rotB.
+    //
+    //   convertedRot convert(zero) = convert(transformRot)
+    //   convertedRot = convert(transformRot) convert(zero)⁻¹
+    //                = (coordRot transformRot) (coordRot zero)⁻¹
+    //                = (coordRot transformRot) coordRot⁻¹
+    //
+    // In code, the equivalent for rotA rotB is rotB.RotateBy(rotA) (note the
+    // change in order), and the equivalent for rot⁻¹ is -rot.
     return Transform3d{
         Convert(transform.Translation(), from, to),
         (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
   }
 
  private:
-  // Rotation from this coordinate system to NWU coordinate system
+  // Rotation from this coordinate system to NWU coordinate system when applied
+  // extrinsically
   Rotation3d m_rotation;
 };
 

wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp

@@ -286,8 +286,8 @@ constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
 
 constexpr Pose2d Pose2d::TransformBy(
     const wpi::math::Transform2d& other) const {
-  return {m_translation + other.Translation().RotateBy(m_rotation),
-          other.Rotation() + m_rotation};
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          other.Rotation().RotateBy(m_rotation)};
 }
 
 constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {

wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp

@@ -312,8 +312,11 @@ constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
 }
 
 constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
-  return {m_translation + other.Translation().RotateBy(m_rotation),
-          other.Rotation() + m_rotation};
+  // Rotating the transform's rotation by the pose's rotation extrinsically is
+  // equivalent to rotating the pose's rotation by the transform's rotation
+  // intrinsically. (We define transforms as being applied intrinsically.)
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          other.Rotation().RotateBy(m_rotation)};
 }
 
 constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {

wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp

@@ -190,6 +190,19 @@ class WPILIB_DLLEXPORT Rotation2d {
             Cos() * other.Sin() + Sin() * other.Cos()};
   }
 
+  /**
+   * Returns the current rotation relative to the given rotation.
+   *
+   * @param other The rotation describing the orientation of the new coordinate
+   * frame that the current rotation will be converted into.
+   *
+   * @return The current rotation relative to the new orientation of the
+   * coordinate frame.
+   */
+  constexpr Rotation2d RelativeTo(const Rotation2d& other) const {
+    return RotateBy(-other);
+  }
+
   /**
    * Returns matrix representation of this rotation.
    */

wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp

@@ -15,13 +15,56 @@
 #include "wpi/math/linalg/ct_matrix.hpp"
 #include "wpi/units/angle.hpp"
 #include "wpi/units/math.hpp"
+#include "wpi/util/MathExtras.hpp"
 #include "wpi/util/SymbolExports.hpp"
 #include "wpi/util/json_fwd.hpp"
 
 namespace wpi::math {
 
 /**
- * A rotation in a 3D coordinate frame represented by a quaternion.
+ * A rotation in a 3D coordinate frame, represented by a quaternion. Note that
+ * unlike 2D rotations, 3D rotations are not always commutative, so changing the
+ * order of rotations changes the result.
+ *
+ * As an example of the order of rotations mattering, suppose we have a card
+ * that looks like this:
+ *
+ * <pre>
+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ * </pre>
+ *
+ * If we rotate 90º clockwise around the axis into the page, then flip around
+ * the left/right axis, we get one result:
+ *
+ * <pre>
+ *   ┌───┐
+ *   │ X │   ┌───────┐   ┌───────┐
+ *   │ | │ → │------X│ → │······x│
+ *   │ | │   └───────┘   └───────┘
+ *   └───┘
+ * </pre>
+ *
+ * If we flip around the left/right axis, then rotate 90º clockwise around the
+ * axis into the page, we get a different result:
+ *
+ * <pre>
+ *   ┌───┐   ┌───┐
+ *   │ X │   │ · │   ┌───────┐
+ *   │ | │ → │ · │ → │x······│
+ *   │ | │   │ x │   └───────┘
+ *   └───┘   └───┘
+ * </pre>
+ *
+ * Because order matters for 3D rotations, we need to distinguish between
+ * <em>extrinsic</em> rotations and <em>intrinsic</em> rotations. Rotating
+ * extrinsically means rotating around the global axes, while rotating
+ * intrinsically means rotating from the perspective of the other rotation. A
+ * neat property is that applying a series of rotations extrinsically is the
+ * same as applying the same series in the opposite order intrinsically.
  */
 class WPILIB_DLLEXPORT Rotation3d {
  public:
@@ -240,9 +283,18 @@ class WPILIB_DLLEXPORT Rotation3d {
       : Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
 
   /**
-   * Adds two rotations together.
+   * Adds two rotations together. The other rotation is applied extrinsically to
+   * this rotation, which is equivalent to this rotation being applied
+   * intrinsically to the other rotation. See the class comment for definitions
+   * of extrinsic and intrinsic rotations.
    *
-   * @param other The rotation to add.
+   * Note that `a - b + b` always equals `a`, but `b + (a - b)`
+   * sometimes doesn't. To apply a rotation offset, use either `offset =
+   * -measurement + actual; newAngle = angle + offset;` or `offset = actual -
+   * measurement; newAngle = offset + angle;`, depending on how the corrected
+   * angle needs to change as the input angle changes.
+   *
+   * @param other The rotation to add (applied extrinsically).
    *
    * @return The sum of the two rotations.
    */
@@ -252,11 +304,20 @@ class WPILIB_DLLEXPORT Rotation3d {
 
   /**
    * Subtracts the new rotation from the current rotation and returns the new
-   * rotation.
+   * rotation. The new rotation is from the perspective of the other rotation
+   * (like Pose3d::operator-), so it needs to be applied intrinsically. See the
+   * class comment for definitions of extrinsic and intrinsic rotations.
+   *
+   * Note that `a - b + b` always equals `a`, but `b + (a - b)` sometimes
+   * doesn't. To apply a rotation offset, use either `offset = -measurement +
+   * actual; newAngle = angle + offset;` or `offset = actual - measurement;
+   * newAngle = offset + angle;`, depending on how the corrected angle needs to
+   * change as the input angle changes.
    *
    * @param other The rotation to subtract.
    *
-   * @return The difference between the two rotations.
+   * @return The difference between the two rotations, from the perspective of
+   * the other rotation.
    */
   constexpr Rotation3d operator-(const Rotation3d& other) const {
     return *this + -other;
@@ -323,6 +384,28 @@ class WPILIB_DLLEXPORT Rotation3d {
     return Rotation3d{other.m_q * m_q};
   }
 
+  /**
+   * Returns the current rotation relative to the given rotation. Because the
+   * result is in the perspective of the given rotation, it must be applied
+   * intrinsically. See the class comment for definitions of extrinsic and
+   * intrinsic rotations.
+   *
+   * @param other The rotation describing the orientation of the new coordinate
+   * frame that the current rotation will be converted into.
+   *
+   * @return The current rotation relative to the new orientation of the
+   * coordinate frame.
+   */
+  constexpr Rotation3d RelativeTo(const Rotation3d& other) const {
+    // To apply a quaternion intrinsically, we must right-multiply by that
+    // quaternion. Therefore, "this_q relative to other_q" is the q such that
+    // other_q q = this_q:
+    //
+    //   other_q q = this_q
+    //   q = other_q⁻¹ this_q
+    return Rotation3d{other.m_q.Inverse() * m_q};
+  }
+
   /**
    * Returns the quaternion representation of the Rotation3d.
    */
@@ -450,5 +533,12 @@ void from_json(const wpi::util::json& json, Rotation3d& rotation);
 
 }  // namespace wpi::math
 
+template <>
+constexpr wpi::math::Rotation3d wpi::util::Lerp(
+    const wpi::math::Rotation3d& startValue,
+    const wpi::math::Rotation3d& endValue, double t) {
+  return (endValue - startValue) * t + startValue;
+}
+
 #include "wpi/math/geometry/proto/Rotation3dProto.hpp"
 #include "wpi/math/geometry/struct/Rotation3dStruct.hpp"

wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp

@@ -177,13 +177,12 @@ class WPILIB_DLLEXPORT Transform2d {
 namespace wpi::math {
 
 constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) {
-  // We are rotating the difference between the translations
-  // using a clockwise rotation matrix. This transforms the global
-  // delta into a local delta (relative to the initial pose).
+  // To transform the global translation delta to be relative to the initial
+  // pose, rotate by the inverse of the initial pose's orientation.
   m_translation = (final.Translation() - initial.Translation())
                       .RotateBy(-initial.Rotation());
 
-  m_rotation = final.Rotation() - initial.Rotation();
+  m_rotation = final.Rotation().RelativeTo(initial.Rotation());
 }
 
 constexpr Transform2d Transform2d::operator+(const Transform2d& other) const {

wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp

@@ -16,7 +16,9 @@ class Pose3d;
 struct Twist3d;
 
 /**
- * Represents a transformation for a Pose3d in the pose's frame.
+ * Represents a transformation for a Pose3d in the pose's frame. Translation is
+ * applied before rotation. (The translation is applied in the pose's original
+ * frame, not the transformed frame.)
  */
 class WPILIB_DLLEXPORT Transform3d {
  public:
@@ -202,13 +204,12 @@ class WPILIB_DLLEXPORT Transform3d {
 namespace wpi::math {
 
 constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
-  // We are rotating the difference between the translations
-  // using a clockwise rotation matrix. This transforms the global
-  // delta into a local delta (relative to the initial pose).
+  // To transform the global translation delta to be relative to the initial
+  // pose, rotate by the inverse of the initial pose's orientation.
   m_translation = (final.Translation() - initial.Translation())
                       .RotateBy(-initial.Rotation());
 
-  m_rotation = final.Rotation() - initial.Rotation();
+  m_rotation = final.Rotation().RelativeTo(initial.Rotation());
 }
 
 constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {

wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp

@@ -11,6 +11,8 @@
 #include <vector>
 
 #include "wpi/math/geometry/Pose2d.hpp"
+#include "wpi/math/geometry/Pose3d.hpp"
+#include "wpi/math/geometry/Rotation3d.hpp"
 #include "wpi/units/time.hpp"
 #include "wpi/util/MathExtras.hpp"
 
@@ -155,7 +157,8 @@ class TimeInterpolatableBuffer {
   std::function<T(const T&, const T&, double)> m_interpolatingFunc;
 };
 
-// Template specialization to ensure that Pose2d uses pose exponential
+// Template specializations to ensure that Pose2d and Pose3d use pose
+// exponential
 template <>
 inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
     wpi::units::second_t historySize)
@@ -172,4 +175,20 @@ inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
         }
       }) {}
 
+template <>
+inline TimeInterpolatableBuffer<Pose3d>::TimeInterpolatableBuffer(
+    wpi::units::second_t historySize)
+    : m_historySize(historySize),
+      m_interpolatingFunc([](const Pose3d& start, const Pose3d& end, double t) {
+        if (t < 0) {
+          return start;
+        } else if (t >= 1) {
+          return end;
+        } else {
+          Twist3d twist = (end - start).Log();
+          Twist3d scaledTwist = twist * t;
+          return start + scaledTwist.Exp();
+        }
+      }) {}
+
 }  // namespace wpi::math

wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp

@@ -47,7 +47,7 @@ class WPILIB_DLLEXPORT Odometry {
         m_pose(initialPose),
         m_previousWheelPositions(wheelPositions) {
     m_previousAngle = m_pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
   }
 
   /**
@@ -64,7 +64,7 @@ class WPILIB_DLLEXPORT Odometry {
                      const WheelPositions& wheelPositions, const Pose2d& pose) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
     m_previousWheelPositions = wheelPositions;
   }
 
@@ -74,7 +74,8 @@ class WPILIB_DLLEXPORT Odometry {
    * @param pose The pose to reset to.
    */
   void ResetPose(const Pose2d& pose) {
-    m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
+    m_gyroOffset =
+        m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation());
     m_pose = pose;
     m_previousAngle = pose.Rotation();
   }
@@ -94,7 +95,7 @@ class WPILIB_DLLEXPORT Odometry {
    * @param rotation The rotation to reset to.
    */
   void ResetRotation(const Rotation2d& rotation) {
-    m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
+    m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation()).RotateBy(rotation);
     m_pose = Pose2d{m_pose.Translation(), rotation};
     m_previousAngle = rotation;
   }
@@ -118,7 +119,7 @@ class WPILIB_DLLEXPORT Odometry {
    */
   const Pose2d& Update(const Rotation2d& gyroAngle,
                        const WheelPositions& wheelPositions) {
-    auto angle = gyroAngle + m_gyroOffset;
+    auto angle = gyroAngle.RotateBy(m_gyroOffset);
 
     auto twist =
         m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
@@ -139,7 +140,10 @@ class WPILIB_DLLEXPORT Odometry {
   Pose2d m_pose;
 
   WheelPositions m_previousWheelPositions;
+
+  // Always equal to m_pose.Rotation()
   Rotation2d m_previousAngle;
+
   Rotation2d m_gyroOffset;
 };
 

wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp

@@ -47,7 +47,9 @@ class WPILIB_DLLEXPORT Odometry3d {
         m_pose(initialPose),
         m_previousWheelPositions(wheelPositions) {
     m_previousAngle = m_pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    // When applied extrinsically, m_gyroOffset cancels the
+    // current gyroAngle and then rotates to m_pose.Rotation()
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
   }
 
   /**
@@ -64,7 +66,9 @@ class WPILIB_DLLEXPORT Odometry3d {
                      const WheelPositions& wheelPositions, const Pose3d& pose) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    // When applied extrinsically, m_gyroOffset cancels the
+    // current gyroAngle and then rotates to m_pose.Rotation()
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
     m_previousWheelPositions = wheelPositions;
   }
 
@@ -74,7 +78,9 @@ class WPILIB_DLLEXPORT Odometry3d {
    * @param pose The pose to reset to.
    */
   void ResetPose(const Pose3d& pose) {
-    m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
+    // Cancel the previous m_pose.Rotation() and then rotate to the new angle
+    m_gyroOffset =
+        m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation());
     m_pose = pose;
     m_previousAngle = pose.Rotation();
   }
@@ -94,7 +100,8 @@ class WPILIB_DLLEXPORT Odometry3d {
    * @param rotation The rotation to reset to.
    */
   void ResetRotation(const Rotation3d& rotation) {
-    m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
+    // Cancel the previous m_pose.Rotation() and then rotate to the new angle
+    m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation);
     m_pose = Pose3d{m_pose.Translation(), rotation};
     m_previousAngle = rotation;
   }
@@ -118,7 +125,7 @@ class WPILIB_DLLEXPORT Odometry3d {
    */
   const Pose3d& Update(const Rotation3d& gyroAngle,
                        const WheelPositions& wheelPositions) {
-    auto angle = gyroAngle + m_gyroOffset;
+    auto angle = gyroAngle.RotateBy(m_gyroOffset);
     auto angle_difference = (angle - m_previousAngle).ToVector();
 
     auto twist2d =
@@ -145,7 +152,14 @@ class WPILIB_DLLEXPORT Odometry3d {
   Pose3d m_pose;
 
   WheelPositions m_previousWheelPositions;
+
+  // Always equal to m_pose.Rotation()
   Rotation3d m_previousAngle;
+
+  // Applying a rotation intrinsically to the measured gyro angle should cause
+  // the corrected angle to be rotated intrinsically in the same way, so the
+  // measured gyro angle must be applied intrinsically. This is equivalent to
+  // applying the offset extrinsically to the measured gyro angle.
   Rotation3d m_gyroOffset;
 };
 

wpimath/src/main/native/include/wpi/math/system/LinearSystemLoop.hpp

@@ -131,6 +131,7 @@ class LinearSystemLoop {
         m_clampFunc(clampFunction) {
     m_nextR.setZero();
     Reset(m_nextR);
+    wpi::math::MathSharedStore::ReportUsage("LinearSystemLoop", "");
   }
 
   LinearSystemLoop(LinearSystemLoop&&) = default;

wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3dTest.java

@@ -28,6 +28,7 @@ import org.wpilib.math.linalg.VecBuilder;
 import org.wpilib.math.trajectory.Trajectory;
 import org.wpilib.math.trajectory.TrajectoryConfig;
 import org.wpilib.math.trajectory.TrajectoryGenerator;
+import org.wpilib.math.util.MathSharedStore;
 
 class SwerveDrivePoseEstimator3dTest {
   private static final double kEpsilon = 1e-9;
@@ -499,11 +500,23 @@ class SwerveDrivePoseEstimator3dTest {
         () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
         () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
 
+    // Add a vision measurement with a different translation
+    estimator.addVisionMeasurement(
+        new Pose3d(3, 0, 0, Rotation3d.kZero), MathSharedStore.getTimestamp());
+
+    assertAll(
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
     // Test reset rotation
     estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2));
 
     assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
         () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
         () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
         () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
@@ -523,7 +536,7 @@ class SwerveDrivePoseEstimator3dTest {
     }
 
     assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
         () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
         () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
         () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
@@ -532,6 +545,22 @@ class SwerveDrivePoseEstimator3dTest {
             assertEquals(
                 Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
 
+    // Add a vision measurement with a different rotation
+    estimator.addVisionMeasurement(
+        new Pose3d(2.5, 1, 0, new Rotation3d(Rotation2d.kPi)), MathSharedStore.getTimestamp());
+
+    assertAll(
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI * 3.0 / 4,
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                kEpsilon));
+
     // Test reset translation
     estimator.resetTranslation(new Translation3d(-1, -1, -1));
 
@@ -543,7 +572,9 @@ class SwerveDrivePoseEstimator3dTest {
         () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
         () ->
             assertEquals(
-                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+                Math.PI * 3.0 / 4,
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                kEpsilon));
 
     // Test reset pose
     estimator.resetPose(Pose3d.kZero);

wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimatorTest.java

@@ -25,6 +25,7 @@ import org.wpilib.math.linalg.VecBuilder;
 import org.wpilib.math.trajectory.Trajectory;
 import org.wpilib.math.trajectory.TrajectoryConfig;
 import org.wpilib.math.trajectory.TrajectoryGenerator;
+import org.wpilib.math.util.MathSharedStore;
 
 class SwerveDrivePoseEstimatorTest {
   private static final double kEpsilon = 1e-9;
@@ -470,11 +471,21 @@ class SwerveDrivePoseEstimatorTest {
         () ->
             assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
 
+    // Add a vision measurement with a different translation
+    estimator.addVisionMeasurement(
+        new Pose2d(3, 0, Rotation2d.kZero), MathSharedStore.getTimestamp());
+
+    assertAll(
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
     // Test reset rotation
     estimator.resetRotation(Rotation2d.kCCW_Pi_2);
 
     assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
         () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
         () ->
             assertEquals(
@@ -493,7 +504,7 @@ class SwerveDrivePoseEstimatorTest {
     }
 
     assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
         () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
         () ->
             assertEquals(
@@ -501,6 +512,19 @@ class SwerveDrivePoseEstimatorTest {
                 estimator.getEstimatedPosition().getRotation().getRadians(),
                 kEpsilon));
 
+    // Add a vision measurement with a different rotation
+    estimator.addVisionMeasurement(
+        new Pose2d(2.5, 1, Rotation2d.kPi), MathSharedStore.getTimestamp());
+
+    assertAll(
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI * 3.0 / 4,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
     // Test reset translation
     estimator.resetTranslation(new Translation2d(-1, -1));
 
@@ -509,7 +533,7 @@ class SwerveDrivePoseEstimatorTest {
         () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
         () ->
             assertEquals(
-                Math.PI / 2,
+                Math.PI * 3.0 / 4,
                 estimator.getEstimatedPosition().getRotation().getRadians(),
                 kEpsilon));
 

wpimath/src/test/java/org/wpilib/math/filter/EdgeCounterFilterTest.java

@@ -0,0 +1,82 @@
+// 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 org.wpilib.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.assertTrue;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.wpilib.util.WPIUtilJNI;
+
+class EdgeCounterFilterTest {
+  @BeforeEach
+  void setUp() {
+    WPIUtilJNI.enableMockTime();
+    WPIUtilJNI.setMockTime(0L);
+  }
+
+  @AfterEach
+  void tearDown() {
+    WPIUtilJNI.setMockTime(0L);
+    WPIUtilJNI.disableMockTime();
+  }
+
+  @Test
+  void edgeCounterFilterActivatedTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertFalse(filter.calculate(true)); // First edge
+
+    WPIUtilJNI.setMockTime(50_000L);
+    assertFalse(filter.calculate(false)); // First edge ended
+
+    WPIUtilJNI.setMockTime(100_000L);
+    assertTrue(filter.calculate(true)); // Second edge
+
+    WPIUtilJNI.setMockTime(150_000L);
+    assertTrue(filter.calculate(true)); // Still true
+
+    WPIUtilJNI.setMockTime(250_000L);
+    assertTrue(filter.calculate(true)); // Still true
+
+    WPIUtilJNI.setMockTime(300_000L);
+    assertFalse(filter.calculate(false)); // Input false, should reset
+  }
+
+  @Test
+  void edgeCounterFilterExpiredTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertFalse(filter.calculate(true)); // First edge
+
+    WPIUtilJNI.setMockTime(50_000L);
+    filter.calculate(false); // First edge ended
+
+    WPIUtilJNI.setMockTime(250_000L);
+    assertFalse(filter.calculate(true)); // Second edge after window expired
+
+    WPIUtilJNI.setMockTime(300_000L);
+    assertFalse(filter.calculate(true)); // Still false
+  }
+
+  @Test
+  void edgeCounterFilterParamsTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertEquals(filter.getRequiredEdges(), 2);
+    assertEquals(filter.getWindowTime(), 0.2);
+
+    filter.setRequiredEdges(3);
+
+    assertEquals(filter.getRequiredEdges(), 3);
+
+    filter.setWindowTime(0.5);
+
+    assertEquals(filter.getWindowTime(), 0.5);
+  }
+}

wpimath/src/test/java/org/wpilib/math/geometry/Rotation2dTest.java

@@ -59,6 +59,16 @@ class Rotation2dTest {
     assertEquals(120.0, rot.getDegrees(), kEpsilon);
   }
 
+  @Test
+  void testRelativeTo() {
+    var start = Rotation2d.fromDegrees(30.0);
+    var end = Rotation2d.kCCW_Pi_2;
+
+    var result = end.relativeTo(start);
+
+    assertEquals(60.0, result.getDegrees(), kEpsilon);
+  }
+
   @Test
   void testMinus() {
     var rot1 = Rotation2d.fromDegrees(70.0);

wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java

@@ -281,6 +281,22 @@ class Rotation3dTest {
     assertEquals(expected, rot);
   }
 
+  @Test
+  void testRelativeTo() {
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var start = new Rotation3d(yAxis, Units.degreesToRadians(-90.0));
+    var end = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
+
+    final var intrinsicAxis = VecBuilder.fill(1.0, 1.0, 1.0);
+    var expected = new Rotation3d(intrinsicAxis, Units.degreesToRadians(120.0));
+
+    var result = end.relativeTo(start);
+
+    assertEquals(expected, result);
+  }
+
   @Test
   void testMinus() {
     final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
@@ -409,5 +425,32 @@ class Rotation3dTest {
     assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
     assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
     assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon);
+
+    // t value of 0 should always produce the start
+    rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90));
+    interpolated = rot1.interpolate(rot2, 0.0);
+    assertEquals(rot1.getX(), interpolated.getX(), kEpsilon);
+    assertEquals(rot1.getY(), interpolated.getY(), kEpsilon);
+    assertEquals(rot1.getZ(), interpolated.getZ(), kEpsilon);
+
+    // The full rotation from rot1 to rot2 to 120 degrees around extrinsic <-1.0, 1.0, 1.0>
+    var extrinsicAxis = VecBuilder.fill(-1.0, 1.0, 1.0);
+    rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90));
+    assertEquals(rot2, rot1.rotateBy(new Rotation3d(extrinsicAxis, Units.degreesToRadians(120))));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    var expected = rot1.rotateBy(new Rotation3d(extrinsicAxis, Units.degreesToRadians(60)));
+    assertEquals(expected.getX(), interpolated.getX(), kEpsilon);
+    assertEquals(expected.getY(), interpolated.getY(), kEpsilon);
+    assertEquals(expected.getZ(), interpolated.getZ(), kEpsilon);
+
+    // t value of 1 should always produce the end
+    rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90));
+    interpolated = rot1.interpolate(rot2, 1.0);
+    assertEquals(rot2.getX(), interpolated.getX(), kEpsilon);
+    assertEquals(rot2.getY(), interpolated.getY(), kEpsilon);
+    assertEquals(rot2.getZ(), interpolated.getZ(), kEpsilon);
   }
 }

wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveOdometry3dTest.java

@@ -10,6 +10,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import org.junit.jupiter.api.Test;
 import org.wpilib.math.geometry.Pose3d;
 import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Translation3d;
 import org.wpilib.math.util.Units;
 
 class DifferentialDriveOdometry3dTest {
@@ -45,4 +46,22 @@ class DifferentialDriveOdometry3dTest {
         () -> assertEquals(pose.getZ(), 0.0, kEpsilon),
         () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 90.0, kEpsilon));
   }
+
+  @Test
+  void testGyroOffset() {
+    m_odometry.resetPosition(
+        new Rotation3d(0, Units.degreesToRadians(5), 0),
+        0,
+        0,
+        new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
+    var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), 0, 0);
+
+    assertAll(
+        () -> assertEquals(pose.getX(), 0.0, kEpsilon),
+        () -> assertEquals(pose.getY(), 0.0, kEpsilon),
+        () -> assertEquals(pose.getZ(), 0.0, kEpsilon),
+        () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), kEpsilon),
+        () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), kEpsilon),
+        () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), kEpsilon));
+  }
 }

wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveOdometry3dTest.java

@@ -288,4 +288,22 @@ class MecanumDriveOdometry3dTest {
         0.05,
         "Incorrect distance travelled");
   }
+
+  @Test
+  void testGyroOffset() {
+    var wheelPositions = new MecanumDriveWheelPositions();
+    m_odometry.resetPosition(
+        new Rotation3d(0, Units.degreesToRadians(5), 0),
+        wheelPositions,
+        new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
+    var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), wheelPositions);
+
+    assertAll(
+        () -> assertEquals(pose.getX(), 0.0, 1e-9),
+        () -> assertEquals(pose.getY(), 0.0, 1e-9),
+        () -> assertEquals(pose.getZ(), 0.0, 1e-9),
+        () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9),
+        () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9),
+        () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9));
+  }
 }

wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometry3dTest.java

@@ -294,4 +294,22 @@ class SwerveDriveOdometry3dTest {
     assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.06, "Incorrect mean error");
     assertEquals(0.0, maxError, 0.125, "Incorrect max error");
   }
+
+  @Test
+  void testGyroOffset() {
+    SwerveModulePosition[] modulePositions = {zero, zero, zero, zero};
+    m_odometry.resetPosition(
+        new Rotation3d(0, Units.degreesToRadians(5), 0),
+        modulePositions,
+        new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
+    var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), modulePositions);
+
+    assertAll(
+        () -> assertEquals(pose.getX(), 0.0, 1e-9),
+        () -> assertEquals(pose.getY(), 0.0, 1e-9),
+        () -> assertEquals(pose.getZ(), 0.0, 1e-9),
+        () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9),
+        () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9),
+        () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9));
+  }
 }

wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp

@@ -498,10 +498,22 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
 
+  // Add a vision measurement with a different translation
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose3d(3_m, 0_m, 0_m, wpi::math::Rotation3d{}),
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
+
   // Test reset rotation
   estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg});
 
-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -517,7 +529,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
                                                modulePosition, modulePosition});
   }
 
-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -525,6 +537,20 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
   EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
                    estimator.GetEstimatedPosition().Rotation().Z().value());
 
+  // Add a vision measurement with a different rotation
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose3d(2.5_m, 1_m, 0_m,
+                        wpi::math::Rotation3d{wpi::math::Rotation2d{180_deg}}),
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
+                   estimator.GetEstimatedPosition().Rotation().Z().value());
+
   // Test reset translation
   estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
 
@@ -533,7 +559,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
   EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
-  EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
+  EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
                    estimator.GetEstimatedPosition().Rotation().Z().value());
 
   // Test reset pose

wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp

@@ -464,10 +464,20 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
   EXPECT_DOUBLE_EQ(
       0, estimator.GetEstimatedPosition().Rotation().Radians().value());
 
+  // Add a vision measurement with a different translation
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{3_m, 0_m, wpi::math::Rotation2d{}},
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(
+      0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
   // Test reset rotation
   estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
 
-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(
       std::numbers::pi / 2,
@@ -481,19 +491,30 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
                                                modulePosition, modulePosition});
   }
 
-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(
       std::numbers::pi / 2,
       estimator.GetEstimatedPosition().Rotation().Radians().value());
 
+  // Add a vision measurement with a different rotation
+  estimator.AddVisionMeasurement(
+      wpi::math::Pose2d{2.5_m, 1_m, wpi::math::Rotation2d{180_deg}},
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(
+      std::numbers::pi * 3.0 / 4,
+      estimator.GetEstimatedPosition().Rotation().Radians().value());
+
   // Test reset translation
   estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
 
   EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
   EXPECT_DOUBLE_EQ(
-      std::numbers::pi / 2,
+      std::numbers::pi * 3.0 / 4,
       estimator.GetEstimatedPosition().Rotation().Radians().value());
 
   // Test reset pose

wpimath/src/test/native/cpp/filter/EdgeCounterFilterTest.cpp

@@ -0,0 +1,77 @@
+// 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 "wpi/math/filter/EdgeCounterFilter.hpp"
+
+#include <gtest/gtest.h>
+
+#include "wpi/units/time.hpp"
+#include "wpi/util/timestamp.h"
+
+static wpi::units::second_t now = 0_s;
+
+class EdgeCounterFilterTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    WPI_SetNowImpl(
+        [] { return wpi::units::microsecond_t{now}.to<uint64_t>(); });
+    now = 0_ms;
+  }
+
+  void TearDown() override {
+    now = 0_ms;
+    WPI_SetNowImpl(nullptr);
+  }
+};
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterActivated) {
+  wpi::math::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_FALSE(filter.Calculate(true));  // First edge
+
+  now = 50_ms;
+  EXPECT_FALSE(filter.Calculate(false));  // First edge ended
+
+  now = 100_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Second edge
+
+  now = 150_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Still true
+
+  now = 250_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Still true
+
+  now = 300_ms;
+  EXPECT_FALSE(filter.Calculate(false));  // Input false, should reset
+}
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterExpired) {
+  wpi::math::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_FALSE(filter.Calculate(true));  // First edge
+
+  now = 50_ms;
+  filter.Calculate(false);  // First edge ended
+
+  now = 250_ms;
+  EXPECT_FALSE(filter.Calculate(true));  // Second edge after window expired
+
+  now = 300_ms;
+  EXPECT_FALSE(filter.Calculate(true));  // Still false
+}
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterParams) {
+  wpi::math::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_EQ(filter.GetRequiredEdges(), 2);
+  EXPECT_EQ(filter.GetWindowTime(), 0.2_s);
+
+  filter.SetRequiredEdges(3);
+
+  EXPECT_EQ(filter.GetRequiredEdges(), 3);
+
+  filter.SetWindowTime(0.5_s);
+
+  EXPECT_EQ(filter.GetWindowTime(), 0.5_s);
+}

wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp

@@ -41,6 +41,15 @@ TEST(Rotation2dTest, RotateByNonZero) {
   EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value());
 }
 
+TEST(Rotation2dTest, RelativeTo) {
+  auto start = Rotation2d{30_deg};
+  auto end = Rotation2d{90_deg};
+
+  auto result = end.RelativeTo(start);
+
+  EXPECT_DOUBLE_EQ(60.0, result.Degrees().value());
+}
+
 TEST(Rotation2dTest, Minus) {
   const auto rot1 = Rotation2d{70_deg};
   const auto rot2 = Rotation2d{30_deg};

wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp

@@ -261,6 +261,21 @@ TEST(Rotation3dTest, RotateByNonZeroZ) {
   EXPECT_EQ(expected, rot);
 }
 
+TEST(Rotation3dTest, RelativeTo) {
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  auto start = Rotation3d{yAxis, -90_deg};
+  auto end = Rotation3d{zAxis, 90_deg};
+
+  const Eigen::Vector3d intrinsicAxis{1.0, 1.0, 1.0};
+  auto expected = Rotation3d{intrinsicAxis, 120_deg};
+
+  auto result = end.RelativeTo(start);
+
+  EXPECT_EQ(expected, result);
+}
+
 TEST(Rotation3dTest, Minus) {
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
 
@@ -379,4 +394,32 @@ TEST(Rotation3dTest, Interpolate) {
   EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.X()}.value());
   EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Y()}.value());
   EXPECT_DOUBLE_EQ(-175.0, wpi::units::degree_t{interpolated.Z()}.value());
+
+  // t value of 0 should always produce the start
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  interpolated = wpi::util::Lerp(rot1, rot2, 0.0);
+  EXPECT_DOUBLE_EQ(rot1.X().value(), interpolated.X().value());
+  EXPECT_DOUBLE_EQ(rot1.Y().value(), interpolated.Y().value());
+  EXPECT_DOUBLE_EQ(rot1.Z().value(), interpolated.Z().value());
+
+  // The full rotation from rot1 to rot2 is 120 degrees around extrinsic
+  // <-1.0, 1.0, 1.0>
+  const Eigen::Vector3d extrinsicAxis{-1.0, 1.0, 1.0};
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  EXPECT_EQ(rot2, rot1.RotateBy(Rotation3d{extrinsicAxis, 120_deg}));
+  interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
+  auto expected = rot1.RotateBy(Rotation3d{extrinsicAxis, 60_deg});
+  EXPECT_DOUBLE_EQ(expected.X().value(), interpolated.X().value());
+  EXPECT_DOUBLE_EQ(expected.Y().value(), interpolated.Y().value());
+  EXPECT_DOUBLE_EQ(expected.Z().value(), interpolated.Z().value());
+
+  // t value of 1 should always produce the end
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  interpolated = wpi::util::Lerp(rot1, rot2, 1.0);
+  EXPECT_NEAR(rot2.X().value(), interpolated.X().value(), 1e-9);
+  EXPECT_NEAR(rot2.Y().value(), interpolated.Y().value(), 1e-9);
+  EXPECT_NEAR(rot2.Z().value(), interpolated.Z().value(), 1e-9);
 }

wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp

@@ -8,10 +8,10 @@
 
 #include <gtest/gtest.h>
 
-static constexpr double kEpsilon = 1E-9;
-
 using namespace wpi::math;
 
+static constexpr double kEpsilon = 1E-9;
+
 TEST(DifferentialDriveOdometry3dTest, Initialize) {
   DifferentialDriveOdometry3d odometry{
       wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m,
@@ -39,3 +39,20 @@ TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
   EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
   EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon);
 }
+
+TEST(DifferentialDriveOdometry3dTest, GyroOffset) {
+  DifferentialDriveOdometry3d odometry{
+      wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, 0_m, 0_m,
+      wpi::math::Pose3d{wpi::math::Translation3d{},
+                        wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}};
+  const auto& pose =
+      odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg}, 0_m, 0_m);
+
+  EXPECT_NEAR(pose.X().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0,
+              kEpsilon);
+}

wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp

@@ -231,3 +231,20 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
   EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
   EXPECT_LT(maxError, 0.125);
 }
+
+TEST_F(MecanumDriveOdometry3dTest, GyroOffset) {
+  wpi::math::MecanumDriveWheelPositions wheelPositions;
+  odometry.ResetPosition(
+      wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, wheelPositions,
+      wpi::math::Pose3d{wpi::math::Translation3d{},
+                        wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}});
+  auto pose = odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg},
+                              wheelPositions);
+
+  EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
+}

wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp

@@ -225,3 +225,19 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
   EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
   EXPECT_LT(maxError, 0.125);
 }
+
+TEST_F(SwerveDriveOdometry3dTest, GyroOffset) {
+  m_odometry.ResetPosition(
+      wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, {zero, zero, zero, zero},
+      wpi::math::Pose3d{wpi::math::Translation3d{},
+                        wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}});
+  auto pose = m_odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg},
+                                {zero, zero, zero, zero});
+
+  EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
+  EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
+}

wpiunits/src/main/java/org/wpilib/units/Units.java

@@ -166,7 +166,13 @@ public final class Units {
    * A unit of angular velocity equivalent to spinning at a rate of one {@link #Rotations Rotation}
    * per {@link #Minute}. Motor spec sheets often list maximum speeds in terms of RPM.
    */
-  public static final AngularVelocityUnit RPM = Rotations.per(Minute);
+  public static final AngularVelocityUnit RotationsPerMinute = Rotations.per(Minute);
+
+  /**
+   * A unit of angular velocity equivalent to spinning at a rate of one {@link #Rotations Rotation}
+   * per {@link #Minute}. Motor spec sheets often list maximum speeds in terms of RPM.
+   */
+  public static final AngularVelocityUnit RPM = RotationsPerMinute; // alias
 
   /**
    * The standard SI unit of angular velocity, equivalent to spinning at a rate of one {@link

wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp

@@ -189,7 +189,7 @@ class MemoryBufferMMapFile : public MB {
   MappedFileRegion m_mfr;
 
   static uint64_t getLegalMapOffset(uint64_t offset) {
-    return offset & ~(MappedFileRegion::GetAlignment() - 1);
+    return offset & ~(static_cast<uint64_t>(MappedFileRegion::GetAlignment()) - 1);
   }
 
   static uint64_t getLegalMapSize(uint64_t len, uint64_t offset) {