Pose estimator psuedocode

.github/workflows/test.yml

@@ -0,0 +1,42 @@
+# This workflow uses actions that are not certified by GitHub.
+# They are provided by a third-party and are governed by
+# separate terms of service, privacy policy, and support
+# documentation.
+# This workflow will build a Java project with Gradle and cache/restore any dependencies to improve the workflow execution time
+# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-java-with-gradle
+
+name: Build and test robot code Gradle
+
+on:
+  push:
+    branches: [ "master" ]
+  pull_request:
+    branches: [ "master" ]
+
+permissions:
+  contents: read
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v3
+    - name: Set up JDK 11
+      uses: actions/setup-java@v3
+      with:
+        java-version: '11'
+        distribution: 'temurin'
+    - name: Setup gradle
+      uses: gradle/gradle-build-action@v2
+    - name: Install roborio toolchain
+      run: ./gradlew installRoboRioToolchain
+    - name: Execute gradle build
+      run: ./gradlew build
+
+
+#    - name: Build with Gradle
+#      uses: gradle/gradle-build-action@67421db6bd0bf253fb4bd25b31ebb98943c375e1
+#      with:
+#        arguments: build

src/main/cpp/DrivePoseEstimator.cpp

@@ -0,0 +1,19 @@
+#include "DrivePoseEstimator.h"
+
+// this should be called periodically, as often as possible
+void DrivePoseEstimator::updateWheels() {
+    // update using wheel odometry:
+    // estimator.Update(navx->GetRotation2d(), swerveDrive->GetRealModuleStates());
+}
+
+// this should be called whenever the camera gets an apriltag reading
+void DrivePoseEstimator::updateCamera() {
+    // pose = get robot pose from vision
+    // timestamp = get image timestamp, or just current timestamp using getFPGATimestamp if that's not avilable
+    // stddevs = calculate based on the distance from the apriltag
+    // estimator.AddVisionMeasurement(pose, timestamp, stddevs)
+}
+
+frc::Pose2d DrivePoseEstimator::getPose() {
+
+}

src/main/include/DrivePoseEstimator.h

@@ -0,0 +1,24 @@
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc/MathUtil.h>
+#include <frc/geometry/Pose2d.h>
+#include "Constants.h"
+
+
+
+class DrivePoseEstimator {
+public:
+  void updateWheels();
+  void updateCamera();
+  frc::Pose2d getPose();
+
+private:
+
+  // TODO tune these hyperparameters
+  // these hyperparameters specify how accurate each measurement is (how much to trust it)
+  const wpi::array<double, 3> stateStdDevs = {0.01, 0.01, 0.01};
+  const wpi::array<double, 1> localMeasurementStdDevs = {0.1};
+  const wpi::array<double, 3> visionMeasurementStdDevs = {0.1, 0.1, 0.1};
+
+  // This is the kalman filter that combines vision and wheel odometry measurements
+  //frc::SwerveDrivePoseEstimator<4> estimator 
+};