diff --git a/java/sim-pose-estimation/.gitignore b/java/sim-pose-estimation/.gitignore
new file mode 100644
index 0000000..3322773
--- /dev/null
+++ b/java/sim-pose-estimation/.gitignore
@@ -0,0 +1,161 @@
+# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+.classpath
+.project
+.settings/
+bin/
+imgui.ini
+
+
+# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode
diff --git a/java/sim-pose-estimation/.vscode/launch.json b/java/sim-pose-estimation/.vscode/launch.json
new file mode 100644
index 0000000..5b804e8
--- /dev/null
+++ b/java/sim-pose-estimation/.vscode/launch.json
@@ -0,0 +1,21 @@
+{
+  // Use IntelliSense to learn about possible attributes.
+  // Hover to view descriptions of existing attributes.
+  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+  "version": "0.2.0",
+  "configurations": [
+
+    {
+      "type": "wpilib",
+      "name": "WPILib Desktop Debug",
+      "request": "launch",
+      "desktop": true,
+    },
+    {
+      "type": "wpilib",
+      "name": "WPILib roboRIO Debug",
+      "request": "launch",
+      "desktop": false,
+    }
+  ]
+}
diff --git a/java/sim-pose-estimation/.vscode/settings.json b/java/sim-pose-estimation/.vscode/settings.json
new file mode 100644
index 0000000..0a91062
--- /dev/null
+++ b/java/sim-pose-estimation/.vscode/settings.json
@@ -0,0 +1,17 @@
+{
+  "java.configuration.updateBuildConfiguration": "automatic",
+  "java.server.launchMode": "Standard",
+  "files.exclude": {
+    "**/.git": true,
+    "**/.svn": true,
+    "**/.hg": true,
+    "**/CVS": true,
+    "**/.DS_Store": true,
+    "bin/": true,
+    "**/.classpath": true,
+    "**/.project": true,
+    "**/.settings": true,
+    "**/.factorypath": true,
+    "**/*~": true
+  }
+}
diff --git a/java/sim-pose-estimation/.wpilib/wpilib_preferences.json b/java/sim-pose-estimation/.wpilib/wpilib_preferences.json
new file mode 100644
index 0000000..0168953
--- /dev/null
+++ b/java/sim-pose-estimation/.wpilib/wpilib_preferences.json
@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2021",
+    "teamNumber": 1736
+}
\ No newline at end of file
diff --git a/java/sim-pose-estimation/build.gradle b/java/sim-pose-estimation/build.gradle
new file mode 100644
index 0000000..0d4b22f
--- /dev/null
+++ b/java/sim-pose-estimation/build.gradle
@@ -0,0 +1,73 @@
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2021.1.2"
+}
+
+sourceCompatibility = JavaVersion.VERSION_11
+targetCompatibility = JavaVersion.VERSION_11
+
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project EmbeddedTools.
+deploy {
+    targets {
+        roboRIO("roborio") {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = frc.getTeamNumber()
+        }
+    }
+    artifacts {
+        frcJavaArtifact('frcJava') {
+            targets << "roborio"
+            // Debug can be overridden by command line, for use with VSCode
+            debug = frc.getDebugOrDefault(false)
+        }
+        // Built in artifact to deploy arbitrary files to the roboRIO.
+        fileTreeArtifact('frcStaticFileDeploy') {
+            // The directory below is the local directory to deploy
+            files = fileTree(dir: 'src/main/deploy')
+            // Deploy to RoboRIO target, into /home/lvuser/deploy
+            targets << "roborio"
+            directory = '/home/lvuser/deploy'
+        }
+    }
+}
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 4.
+dependencies {
+    implementation wpi.deps.wpilib()
+    nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio)
+    nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop)
+
+
+    implementation wpi.deps.vendor.java()
+    nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
+    nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
+
+    testImplementation 'junit:junit:4.12'
+
+    // Enable simulation gui support. Must check the box in vscode to enable support
+    // upon debugging
+    simulation wpi.deps.sim.gui(wpi.platforms.desktop, false)
+    simulation wpi.deps.sim.driverstation(wpi.platforms.desktop, false)
+
+    // Websocket extensions require additional configuration.
+    // simulation wpi.deps.sim.ws_server(wpi.platforms.desktop, false)
+    // simulation wpi.deps.sim.ws_client(wpi.platforms.desktop, false)
+}
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+}
diff --git a/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.jar b/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 0000000..cc4fdc2
Binary files /dev/null and b/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.properties b/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 0000000..a2c6fac
--- /dev/null
+++ b/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
diff --git a/java/sim-pose-estimation/gradlew b/java/sim-pose-estimation/gradlew
new file mode 100644
index 0000000..2fe81a7
--- /dev/null
+++ b/java/sim-pose-estimation/gradlew
@@ -0,0 +1,183 @@
+#!/usr/bin/env sh
+
+#
+# Copyright 2015 the original author or authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+##############################################################################
+##
+##  Gradle start up script for UN*X
+##
+##############################################################################
+
+# Attempt to set APP_HOME
+# Resolve links: $0 may be a link
+PRG="$0"
+# Need this for relative symlinks.
+while [ -h "$PRG" ] ; do
+    ls=`ls -ld "$PRG"`
+    link=`expr "$ls" : '.*-> \(.*\)$'`
+    if expr "$link" : '/.*' > /dev/null; then
+        PRG="$link"
+    else
+        PRG=`dirname "$PRG"`"/$link"
+    fi
+done
+SAVED="`pwd`"
+cd "`dirname \"$PRG\"`/" >/dev/null
+APP_HOME="`pwd -P`"
+cd "$SAVED" >/dev/null
+
+APP_NAME="Gradle"
+APP_BASE_NAME=`basename "$0"`
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD="maximum"
+
+warn () {
+    echo "$*"
+}
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+}
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "`uname`" in
+  CYGWIN* )
+    cygwin=true
+    ;;
+  Darwin* )
+    darwin=true
+    ;;
+  MINGW* )
+    msys=true
+    ;;
+  NONSTOP* )
+    nonstop=true
+    ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD="$JAVA_HOME/jre/sh/java"
+    else
+        JAVACMD="$JAVA_HOME/bin/java"
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD="java"
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then
+    MAX_FD_LIMIT=`ulimit -H -n`
+    if [ $? -eq 0 ] ; then
+        if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
+            MAX_FD="$MAX_FD_LIMIT"
+        fi
+        ulimit -n $MAX_FD
+        if [ $? -ne 0 ] ; then
+            warn "Could not set maximum file descriptor limit: $MAX_FD"
+        fi
+    else
+        warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
+    fi
+fi
+
+# For Darwin, add options to specify how the application appears in the dock
+if $darwin; then
+    GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
+fi
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then
+    APP_HOME=`cygpath --path --mixed "$APP_HOME"`
+    CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
+    JAVACMD=`cygpath --unix "$JAVACMD"`
+
+    # We build the pattern for arguments to be converted via cygpath
+    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
+    SEP=""
+    for dir in $ROOTDIRSRAW ; do
+        ROOTDIRS="$ROOTDIRS$SEP$dir"
+        SEP="|"
+    done
+    OURCYGPATTERN="(^($ROOTDIRS))"
+    # Add a user-defined pattern to the cygpath arguments
+    if [ "$GRADLE_CYGPATTERN" != "" ] ; then
+        OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
+    fi
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    i=0
+    for arg in "$@" ; do
+        CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
+        CHECK2=`echo "$arg"|egrep -c "^-"`                                 ### Determine if an option
+
+        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition
+            eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
+        else
+            eval `echo args$i`="\"$arg\""
+        fi
+        i=`expr $i + 1`
+    done
+    case $i in
+        0) set -- ;;
+        1) set -- "$args0" ;;
+        2) set -- "$args0" "$args1" ;;
+        3) set -- "$args0" "$args1" "$args2" ;;
+        4) set -- "$args0" "$args1" "$args2" "$args3" ;;
+        5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
+        6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
+        7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
+        8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
+        9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+    esac
+fi
+
+# Escape application args
+save () {
+    for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
+    echo " "
+}
+APP_ARGS=`save "$@"`
+
+# Collect all arguments for the java command, following the shell quoting and substitution rules
+eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
+
+exec "$JAVACMD" "$@"
diff --git a/java/sim-pose-estimation/gradlew.bat b/java/sim-pose-estimation/gradlew.bat
new file mode 100644
index 0000000..24467a1
--- /dev/null
+++ b/java/sim-pose-estimation/gradlew.bat
@@ -0,0 +1,100 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%" == "" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%" == "" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if "%ERRORLEVEL%" == "0" goto init
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto init
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:init
+@rem Get command-line arguments, handling Windows variants
+
+if not "%OS%" == "Windows_NT" goto win9xME_args
+
+:win9xME_args
+@rem Slurp the command line arguments.
+set CMD_LINE_ARGS=
+set _SKIP=2
+
+:win9xME_args_slurp
+if "x%~1" == "x" goto execute
+
+set CMD_LINE_ARGS=%*
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
+
+:end
+@rem End local scope for the variables with windows NT shell
+if "%ERRORLEVEL%"=="0" goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/java/sim-pose-estimation/imgui.ini b/java/sim-pose-estimation/imgui.ini
new file mode 100644
index 0000000..615070b
--- /dev/null
+++ b/java/sim-pose-estimation/imgui.ini
@@ -0,0 +1,487 @@
+[Window][Debug##Default]
+Pos=60,60
+Size=400,400
+Collapsed=0
+
+[Window][###Other Devices]
+Pos=1281,25
+Size=312,868
+Collapsed=0
+
+[Window][###Timing]
+Pos=6,187
+Size=214,172
+Collapsed=0
+
+[Window][###NetworkTables]
+Pos=312,346
+Size=937,231
+Collapsed=0
+
+[Window][###FMS]
+Pos=6,675
+Size=371,232
+Collapsed=0
+
+[Window][###Joysticks]
+Pos=312,581
+Size=1336,354
+Collapsed=0
+
+[Window][###System Joysticks]
+Pos=6,437
+Size=313,326
+Collapsed=0
+
+[Window][###/SmartDashboard/Field]
+Pos=343,34
+Size=571,319
+Collapsed=0
+
+[Window][Robot State]
+Pos=5,20
+Size=144,144
+Collapsed=0
+
+[MainWindow][GLOBAL]
+width=1600
+height=900
+maximized=0
+xpos=-1
+ypos=-1
+userScale=4
+style=0
+
+[GlassStorage][/SmartDashboard/Field]
+image=
+top=0
+left=0
+bottom=-1
+right=-1
+width=15.980000
+height=8.210000
+
+[GlassStorage][/SmartDashboard/Field###ActPose]
+image=.\resources\blue_square.png
+width=0.685800
+length=0.820400
+
+[GlassStorage][/SmartDashboard/Field###DesPose]
+image=.\resources\green_square.png
+width=0.685800
+length=0.820400
+
+[GlassStorage][/SmartDashboard/Field###Robot]
+image=.\resources\red_square.png
+width=0.685800
+length=0.820400
+
+[GlassStorage][NetworkTables]
+tree=1
+connections=0
+flags=0
+timestamp=0
+createNonCanonical=0
+
+[GlassStorage][NetworkTables###FMSInfo]
+open=0
+
+[GlassStorage][NetworkTables###LiveWindow]
+open=0
+
+[GlassStorage][NetworkTables###SmartDashboard]
+open=0
+
+[GlassStorage][NetworkTables###photonvision]
+open=1
+
+[GlassStorage][NetworkTables###photonvision###mainCam]
+open=1
+
+[GlassStorage][Other Devices###AnalogGyro[0]]
+name=
+name###open=0
+
+[Data Sources][Joystick[0] Button[7]]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/d]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[1]/Distance]
+name=
+
+[Data Sources][NT:/FMSInfo/FMSControlData]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/i]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[0]/.controllable]
+name=
+
+[Data Sources][NT:/LiveWindow/.status/LW Enabled]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[1]/Speed]
+name=
+
+[Data Sources][AGyro Angle[0]]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/p]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[2]/.actuator]
+name=
+
+[Data Sources][NT:/FMSInfo/StationNumber]
+name=
+
+[Data Sources][Y Accel[0]]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/setpoint]
+name=
+
+[Data Sources][/SmartDashboard/Field/ActPose[0]/rot]
+name=
+
+[Data Sources][FMS:MatchTime]
+name=
+
+[Data Sources][Joystick[0] Axis[5]]
+name=
+
+[Data Sources][/SmartDashboard/Field/DesPose[0]/rot]
+name=
+
+[Data Sources][Joystick[0] Axis[1]]
+name=
+
+[Data Sources][Joystick[0] Button[4]]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[2]/Value]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/AnalogGyro[0]/.controllable]
+name=
+
+[Data Sources][Joystick[0] Button[8]]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/d]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/i]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[0]/Speed]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/p]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[1]/.actuator]
+name=
+
+[Data Sources][Joystick[0] POV[0]]
+name=
+
+[Data Sources][Joystick[0] Axis[2]]
+name=
+
+[Data Sources][Z Accel[0]]
+name=
+
+[Data Sources][NT:/FMSInfo/MatchType]
+name=
+
+[Data Sources][Joystick[0] Button[1]]
+name=
+
+[Data Sources][FMS:AutonomousMode]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[1]/Value]
+name=
+
+[Data Sources][NT:/FMSInfo/MatchNumber]
+name=
+
+[Data Sources][FMS:FMSAttached]
+name=
+
+[Data Sources][Joystick[0] Button[9]]
+name=
+
+[Data Sources][Joystick[0] Button[5]]
+name=
+
+[Data Sources][FMS:AllianceStationID]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[0]/Distance]
+name=
+
+[Data Sources][FMS:RobotEnabled]
+name=
+
+[Data Sources][/SmartDashboard/Field/Robot[0]/rot]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/.controllable]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[2]/.controllable]
+name=
+
+[Data Sources][FMS:DSAttached]
+name=
+
+[Data Sources][AGyro Rate[0]]
+name=
+
+[Data Sources][/SmartDashboard/Field/Robot[0]/y]
+name=
+
+[Data Sources][/SmartDashboard/Field/Robot[0]/x]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/setpoint]
+name=
+
+[Data Sources][Joystick[0] Axis[3]]
+name=
+
+[Data Sources][/SmartDashboard/Field/ActPose[0]/x]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[1]/Distance per Tick]
+name=
+
+[Data Sources][/SmartDashboard/Field/ActPose[0]/y]
+name=
+
+[Data Sources][Joystick[0] Button[2]]
+name=
+
+[Data Sources][NT:/FMSInfo/ReplayNumber]
+name=
+
+[Data Sources][/SmartDashboard/Field/DesPose[0]/y]
+name=
+
+[Data Sources][FMS:TestMode]
+name=
+
+[Data Sources][/SmartDashboard/Field/DesPose[0]/x]
+name=
+
+[Data Sources][FMS:EStop]
+name=
+
+[Data Sources][NT:/SmartDashboard/Field/.controllable]
+name=
+
+[Data Sources][Joystick[0] Button[10]]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/AnalogGyro[0]/Value]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[1]/.controllable]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[1]/.controllable]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/.controllable]
+name=
+
+[Data Sources][Joystick[0] Axis[0]]
+name=
+
+[Data Sources][Joystick[0] Button[6]]
+name=
+
+[Data Sources][X Accel[0]]
+name=
+
+[Data Sources][Joystick[0] Axis[4]]
+name=
+
+[Data Sources][NT:/FMSInfo/IsRedAlliance]
+name=
+
+[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[0]/Distance per Tick]
+name=
+
+[Data Sources][Joystick[0] Button[3]]
+name=
+
+[SimWindow][Mechanism 2D]
+name=
+visible=0
+enabled=1
+
+[HALProvider][Other Devices]
+name=
+visible=1
+enabled=1
+
+[HALProvider][Timing]
+name=
+visible=1
+enabled=1
+
+[NTProviderWindow][/SmartDashboard/Field]
+name=
+visible=1
+enabled=1
+
+[NTProviderWindow][NetworkTables]
+name=
+visible=1
+enabled=1
+
+[NTProvider][/FMSInfo]
+name=FMSInfo
+
+[NTProvider][/SmartDashboard/Field]
+name=Field2d
+
+[NTProvider][/LiveWindow/Ungrouped/PIDController[1]]
+name=PIDController
+
+[NTProvider][/LiveWindow/Ungrouped/SpeedControllerGroup[1]]
+name=Speed Controller
+
+[NTProvider][/LiveWindow/Ungrouped/SpeedControllerGroup[2]]
+name=Speed Controller
+
+[NTProvider][/LiveWindow/Ungrouped/AnalogGyro[0]]
+name=Gyro
+
+[NTProvider][/LiveWindow/Ungrouped/PIDController[2]]
+name=PIDController
+
+[DSManager][FMS]
+name=
+visible=1
+enabled=1
+
+[DSManager][Joysticks]
+name=
+visible=1
+enabled=1
+
+[DSManager][Keyboard 0 Settings]
+name=
+visible=0
+enabled=1
+
+[DSManager][Keyboard 1 Settings]
+name=
+visible=0
+enabled=1
+
+[DSManager][Keyboard 2 Settings]
+name=
+visible=0
+enabled=1
+
+[DSManager][Keyboard 3 Settings]
+name=
+visible=0
+enabled=1
+
+[DSManager][System Joysticks]
+name=
+visible=1
+enabled=1
+
+[Joystick][0]
+useGamepad=1
+guid=78696e70757401000000000000000000
+
+[KeyboardJoystick][0]
+axisCount=3
+buttonCount=4
+povCount=1
+axis0incKey=68
+axis0decKey=65
+axis0keyRate=0.050000
+axis0decayRate=0.050000
+axis1incKey=83
+axis1decKey=87
+axis1keyRate=0.050000
+axis1decayRate=0.050000
+axis2incKey=82
+axis2decKey=69
+axis2keyRate=0.010000
+axis2decayRate=0.000000
+button0=90
+button1=88
+button2=67
+button3=86
+pov0key0=328
+pov0key45=329
+pov0key90=326
+pov0key135=323
+pov0key180=322
+pov0key225=321
+pov0key270=324
+pov0key315=327
+
+[KeyboardJoystick][1]
+axisCount=2
+buttonCount=4
+povCount=0
+axis0incKey=76
+axis0decKey=74
+axis0keyRate=0.050000
+axis0decayRate=0.050000
+axis1incKey=75
+axis1decKey=73
+axis1keyRate=0.050000
+axis1decayRate=0.050000
+button0=77
+button1=44
+button2=46
+button3=47
+
+[KeyboardJoystick][2]
+axisCount=2
+buttonCount=6
+povCount=0
+axis0incKey=262
+axis0decKey=263
+axis0keyRate=0.050000
+axis0decayRate=0.050000
+axis1incKey=264
+axis1decKey=265
+axis1keyRate=0.050000
+axis1decayRate=0.050000
+button0=260
+button1=268
+button2=266
+button3=261
+button4=269
+button5=267
+
+[KeyboardJoystick][3]
+axisCount=0
+buttonCount=0
+povCount=0
+
+[DriverStation][Main]
+disable=0
+zeroDisconnectedJoysticks=1
+enableDisableKeys=0
+estopKey=0
+
+[Mechanism2D][Mechanism2D]
+jsonLocation=
+
diff --git a/java/sim-pose-estimation/resources/blue_square.png b/java/sim-pose-estimation/resources/blue_square.png
new file mode 100644
index 0000000..63b52bd
Binary files /dev/null and b/java/sim-pose-estimation/resources/blue_square.png differ
diff --git a/java/sim-pose-estimation/resources/green_square.png b/java/sim-pose-estimation/resources/green_square.png
new file mode 100644
index 0000000..d70576a
Binary files /dev/null and b/java/sim-pose-estimation/resources/green_square.png differ
diff --git a/java/sim-pose-estimation/resources/red_square.png b/java/sim-pose-estimation/resources/red_square.png
new file mode 100644
index 0000000..733814f
Binary files /dev/null and b/java/sim-pose-estimation/resources/red_square.png differ
diff --git a/java/sim-pose-estimation/settings.gradle b/java/sim-pose-estimation/settings.gradle
new file mode 100644
index 0000000..4ee6ec2
--- /dev/null
+++ b/java/sim-pose-estimation/settings.gradle
@@ -0,0 +1,27 @@
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        gradlePluginPortal()
+        String frcYear = '2021'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}
diff --git a/java/sim-pose-estimation/src/main/deploy/example.txt b/java/sim-pose-estimation/src/main/deploy/example.txt
new file mode 100644
index 0000000..bb82515
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/deploy/example.txt
@@ -0,0 +1,3 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
+to get a proper path relative to the deploy directory.
\ No newline at end of file
diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java
new file mode 100644
index 0000000..9967141
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java
@@ -0,0 +1,92 @@
+package frc.robot;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+
+/**
+ * Implements logic to convert a set of desired waypoints (ie, a trajectory) and
+ * the current estimate of where the robot is at (ie, the estimated Pose) into
+ * motion commands for a drivetrain. The Ramaste controller is used to smoothly
+ * move the robot from where it thinks it is to where it thinks it ought to be.
+ */
+public class AutoController {
+
+    private Trajectory trajectory;
+
+    private RamseteController ramsete = new RamseteController();
+
+    private Timer timer = new Timer();
+
+    boolean isRunning = false;
+
+    Trajectory.State desiredDtState;
+
+    public AutoController() {
+
+        // Change this trajectory if you need the robot to follow different paths.
+        trajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(2, 2, new Rotation2d()), List.of(),
+                new Pose2d(6, 4, new Rotation2d()), new TrajectoryConfig(2, 2));
+    }
+
+    /**
+     * @return The starting (initial) pose of the currently-active trajectory
+     */
+    public Pose2d getInitialPose() {
+        return trajectory.getInitialPose();
+    }
+
+    /**
+     * Starts the controller running. Call this at the start of autonomous
+     */
+    public void startPath() {
+        timer.reset();
+        timer.start();
+        isRunning = true;
+
+    }
+
+    /**
+     * Stops the controller from generating commands
+     */
+    public void stopPath() {
+        isRunning = false;
+        timer.reset();
+    }
+
+    /**
+     * Given the current estimate of the robot's position, calculate drivetrain
+     * speed commands which will best-execute the active trajectory. Be sure to call
+     * `startPath()` prior to calling this method.
+     * 
+     * @param curEstPose Current estimate of drivetrain pose on the field
+     * @return The commanded drivetrain motion
+     */
+    public ChassisSpeeds getCurMotorCmds(Pose2d curEstPose) {
+
+        if (isRunning) {
+            double elapsed = timer.get();
+            desiredDtState = trajectory.sample(elapsed);
+        } else {
+            desiredDtState = new Trajectory.State();
+        }
+
+        return ramsete.calculate(curEstPose, desiredDtState);
+    }
+
+    /**
+     * @return The position which the auto controller is attempting to move the
+     *         drivetrain to right now.
+     */
+    public Pose2d getCurPose2d() {
+        return desiredDtState.poseMeters;
+    }
+
+}
diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java
new file mode 100644
index 0000000..ad6a178
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,76 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.util.Units;
+
+import org.photonvision.SimVisionTarget;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Holding class for all physical constants that must be used throughout the
+ * codebase. These values should be set by one of a few methods: 1) Talk to your
+ * mechanical and electrical teams and determine how the physical robot is being
+ * built and configured. 2) Read the game manual and look at the field drawings
+ * 3) Match with how your vision coprocessor is configured.
+ */
+public class Constants {
+
+  //////////////////////////////////////////////////////////////////
+  // Drivetrain Physical
+  //////////////////////////////////////////////////////////////////
+  public static final double kMaxSpeed = 3.0; // 3 meters per second.
+  public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second.
+
+  public static final double kTrackWidth = 0.381 * 2;
+  public static final double kWheelRadius = 0.0508;
+  public static final int kEncoderResolution = 4096;
+
+  //////////////////////////////////////////////////////////////////
+  // Electrical IO
+  //////////////////////////////////////////////////////////////////
+  public static final int kGyroPin = 0;
+
+  public static final int kDtLeftEncoderPinA = 0;
+  public static final int kDtLeftEncoderPinB = 1;
+  public static final int kDtRightEncoderPinA = 2;
+  public static final int kDtRightEncoderPinB = 3;
+
+  public static final int kDtLeftLeaderPin = 1;
+  public static final int kDtLeftFollowerPin = 2;
+  public static final int kDtRightLeaderPin = 3;
+  public static final int kDtRightFollowerPin = 4;
+
+  //////////////////////////////////////////////////////////////////
+  // Vision Processing
+  //////////////////////////////////////////////////////////////////
+  // Name configured in the PhotonVision GUI for the camera
+  public static final String kCamName = "mainCam";
+
+  // Physical location of the camera on the robot, relative to the center of the
+  // robot.
+  public static final Transform2d kCameraToRobot = new Transform2d(new Translation2d(-0.25, 0), // in meters
+      new Rotation2d());
+
+  // See
+  // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
+  // page 208
+  public static final double targetWidth = Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70); // meters
+
+  // See
+  // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
+  // page 197
+  public static final double targetHeight = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
+  public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters
+  
+  // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf pages 4 and 5
+  public static final double kFarTgtXPos = Units.feetToMeters(54);
+  public static final double kFarTgtYPos = Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
+  public static final Pose2d kFarTargetPose = new Pose2d(new Translation2d(kFarTgtXPos, kFarTgtYPos), new Rotation2d(0.0));
+
+  public static final SimVisionTarget kFarTarget = new SimVisionTarget(kFarTargetPose, targetHeightAboveGround,
+      targetWidth, targetHeight);
+
+}
diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java
new file mode 100644
index 0000000..76285e9
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.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 frc.robot;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+
+/**
+ * Implements a controller for the drivetrain. Converts a set of chassis motion
+ * commands into motor controller PWM values which attempt to speed up or slow
+ * down the wheels to match the desired speed.
+ */
+public class Drivetrain {
+
+  // PWM motor controller output definitions
+  PWMVictorSPX leftLeader = new PWMVictorSPX(Constants.kDtLeftLeaderPin);
+  PWMVictorSPX leftFollower = new PWMVictorSPX(Constants.kDtLeftFollowerPin);
+  PWMVictorSPX rightLeader = new PWMVictorSPX(Constants.kDtRightLeaderPin);
+  PWMVictorSPX rightFollower = new PWMVictorSPX(Constants.kDtRightFollowerPin);
+
+  SpeedControllerGroup leftGroup = new SpeedControllerGroup(leftLeader, leftFollower);
+  SpeedControllerGroup rightGroup = new SpeedControllerGroup(rightLeader, rightFollower);
+
+  // Drivetrain wheel speed sensors
+  // Used both for speed control and pose estimation.
+  Encoder leftEncoder = new Encoder(Constants.kDtLeftEncoderPinA, Constants.kDtLeftEncoderPinB);
+  Encoder rightEncoder = new Encoder(Constants.kDtRightEncoderPinA, Constants.kDtRightEncoderPinB);
+
+  // Drivetrain Pose Estimation
+  DrivetrainPoseEstimator poseEst = new DrivetrainPoseEstimator();
+
+  // Kinematics - defines the physical size and shape of the drivetrain, which is
+  // required to convert from
+  // chassis speed commands to wheel speed commands.
+  DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.kTrackWidth);
+
+  // Closed-loop PIDF controllers for servoing each side of the drivetrain to a
+  // specific speed.
+  // Gains are for example purposes only - must be determined for your own robot!
+  SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3);
+  PIDController leftPIDController = new PIDController(8.5, 0, 0);
+  PIDController rightPIDController = new PIDController(8.5, 0, 0);
+
+  public Drivetrain() {
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    leftEncoder.setDistancePerPulse(2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution);
+    rightEncoder.setDistancePerPulse(2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution);
+
+    leftEncoder.reset();
+    rightEncoder.reset();
+
+    rightGroup.setInverted(true);
+
+  }
+
+  /**
+   * Given a set of chassis (fwd/rev + rotate) speed commands, perform all
+   * periodic tasks to assign new outputs to the motor controllers.
+   * 
+   * @param xSpeed Desired chassis Forward or Reverse speed (in meters/sec).
+   *               Positive is forward.
+   * @param rot    Desired chassis rotation speed in radians/sec. Positive is
+   *               counter-clockwise.
+   */
+  public void drive(double xSpeed, double rot) {
+    // Convert our fwd/rev and rotate commands to wheel speed commands
+    DifferentialDriveWheelSpeeds speeds = kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot));
+
+    // Calculate the feedback (PID) portion of our motor command, based on desired
+    // wheel speed
+    var leftOutput = leftPIDController.calculate(leftEncoder.getRate(), speeds.leftMetersPerSecond);
+    var rightOutput = rightPIDController.calculate(rightEncoder.getRate(), speeds.rightMetersPerSecond);
+
+    // Calculate the feedforward (F) portion of our motor command, based on desired
+    // wheel speed
+    var leftFeedforward = feedforward.calculate(speeds.leftMetersPerSecond);
+    var rightFeedforward = feedforward.calculate(speeds.rightMetersPerSecond);
+
+    // Update the motor controllers with our new motor commands
+    leftGroup.setVoltage(leftOutput + leftFeedforward);
+    rightGroup.setVoltage(rightOutput + rightFeedforward);
+
+    // Update the pose estimator with the most recent sensor readings.
+    poseEst.update(new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()),
+        leftEncoder.getDistance(), rightEncoder.getDistance());
+  }
+
+  /**
+   * Force the pose estimator and all sensors to a particular pose. This is useful
+   * for indicating to the software when you have manually moved your robot in a
+   * particular position on the field (EX: when you place it on the field at the
+   * start of the match).
+   * 
+   * @param pose
+   */
+  public void resetOdometry(Pose2d pose) {
+    leftEncoder.reset();
+    rightEncoder.reset();
+    poseEst.resetToPose(pose);
+  }
+
+  /**
+   * @return The current best-guess at drivetrain Pose on the field.
+   */
+  public Pose2d getCtrlsPoseEstimate() {
+    return poseEst.getPoseEst();
+  }
+
+}
diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java
new file mode 100644
index 0000000..8e22169
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java
@@ -0,0 +1,89 @@
+package frc.robot;
+
+import org.photonvision.PhotonCamera;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N5;
+
+/**
+ * Performs estimation of the drivetrain's current position on the field, using
+ * a vision system, drivetrain encoders, and a gyroscope. These sensor readings
+ * are fused together using a Kalman filter. This in turn creates a best-guess
+ * at a Pose2d of where our drivetrain is currently at.
+ */
+public class DrivetrainPoseEstimator {
+
+    // Sensors used as part of the Pose Estimation
+    private final AnalogGyro gyro = new AnalogGyro(Constants.kGyroPin);
+    private PhotonCamera cam = new PhotonCamera(Constants.kCamName);
+    // Note - drivetrain encoders are also used. The Drivetrain class must pass us
+    // the relevant readings.
+
+    // Kalman Filter Configuration. These can be "tuned-to-taste" based on how much
+    // you trust your
+    // various sensors. Smaller numbers will cause the filter to "trust" the
+    // estimate from that particular
+    // component more than the others. This in turn means the particualr component
+    // will have a stronger
+    // influence on the final pose estimate.
+    Matrix<N5, N1> stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05);
+    Matrix<N3, N1> localMeasurementStdDevs = VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1));
+    Matrix<N3, N1> visionMeasurementStdDevs = VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1));
+
+    private final DifferentialDrivePoseEstimator m_poseEstimator = new DifferentialDrivePoseEstimator(
+            gyro.getRotation2d(), new Pose2d(), stateStdDevs, localMeasurementStdDevs, visionMeasurementStdDevs);
+
+    public DrivetrainPoseEstimator() {
+
+    }
+
+    /**
+     * Perform all periodic pose estimation tasks.
+     * 
+     * @param actWheelSpeeds Current Speeds (in m/s) of the drivetrain wheels
+     * @param leftDist       Distance (in m) the left wheel has traveled
+     * @param rightDist      Distance (in m) the right wheel has traveled
+     */
+    public void update(DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist) {
+
+        m_poseEstimator.update(gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist);
+
+        var res = cam.getLatestResult();
+        if (res.hasTargets()) {
+            double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis();
+            Transform2d camToTargetTrans = res.getBestTarget().getCameraToTarget();
+            Pose2d camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());
+            m_poseEstimator.addVisionMeasurement(camPose.transformBy(Constants.kCameraToRobot), imageCaptureTime);
+        }
+    }
+
+    /**
+     * Force the pose estimator to a particular pose. This is useful for indicating
+     * to the software when you have manually moved your robot in a particular
+     * position on the field (EX: when you place it on the field at the start of the
+     * match).
+     * 
+     * @param pose
+     */
+    public void resetToPose(Pose2d pose) {
+        m_poseEstimator.resetPosition(pose, gyro.getRotation2d());
+    }
+
+    /**
+     * @return The current best-guess at drivetrain position on the field.
+     */
+    public Pose2d getPoseEst() {
+        return m_poseEstimator.getEstimatedPosition();
+    }
+
+}
diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Main.java b/java/sim-pose-estimation/src/main/java/frc/robot/Main.java
new file mode 100644
index 0000000..8776e5d
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/robot/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 frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java
new file mode 100644
index 0000000..1a9e280
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java
@@ -0,0 +1,37 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID;
+
+public class OperatorInterface {
+    private XboxController opCtrl = new XboxController(0);
+
+    // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+    // to 1.
+    private SlewRateLimiter speedLimiter = new SlewRateLimiter(3);
+    private SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
+
+    public OperatorInterface() {
+
+    }
+
+    public double getFwdRevSpdCmd() {
+        // Get the x speed. We are inverting this because Xbox controllers return
+        // negative values when we push forward.
+        return -speedLimiter.calculate(opCtrl.getY(GenericHID.Hand.kLeft)) * Constants.kMaxSpeed;
+    }
+
+    public double getRotateSpdCmd() {
+        // Get the rate of angular rotation. We are inverting this because we want a
+        // positive value when we pull to the left (remember, CCW is positive in
+        // mathematics). Xbox controllers return positive values when you pull to
+        // the right by default.
+        return -rotLimiter.calculate(opCtrl.getX(GenericHID.Hand.kRight)) * Constants.kMaxAngularSpeed;
+    }
+
+    public boolean getSimKickCmd() {
+        return opCtrl.getXButtonPressed();
+    }
+
+}
diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java
new file mode 100644
index 0000000..8cd3394
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java
@@ -0,0 +1,41 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Reports our expected, desired, and actual poses to dashboards
+ */
+public class PoseTelemetry {
+
+    Field2d field = new Field2d();
+
+    Pose2d actPose = new Pose2d();
+    Pose2d desPose = new Pose2d();
+    Pose2d estPose = new Pose2d();
+
+    public PoseTelemetry() {
+        SmartDashboard.putData("Field", field);
+        update();
+    }
+
+    public void update() {
+        field.getObject("DesPose").setPose(desPose);
+        field.getObject("ActPose").setPose(actPose);
+        field.getObject("Robot").setPose(estPose);
+    }
+
+    public void setActualPose(Pose2d in) {
+        actPose = in;
+    }
+
+    public void setDesiredPose(Pose2d in) {
+        desPose = in;
+    }
+
+    public void setEstimatedPose(Pose2d in) {
+        estPose = in;
+    }
+
+}
diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java
new file mode 100644
index 0000000..4f024fd
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import frc.sim.DrivetrainSim;
+
+public class Robot extends TimedRobot {
+
+  AutoController autoCtrl = new AutoController();
+  Drivetrain dt = new Drivetrain();
+  OperatorInterface opInf = new OperatorInterface();
+
+  DrivetrainSim dtSim = new DrivetrainSim();
+
+  PoseTelemetry pt = new PoseTelemetry();
+
+  @Override
+  public void robotInit() {
+    // Flush NetworkTables every loop. This ensures that robot pose and other values
+    // are sent during every iteration.
+    setNetworkTablesFlushEnabled(true);
+  }
+
+  @Override
+  public void autonomousInit() {
+    resetOdometery();
+    autoCtrl.startPath();
+  }
+
+  @Override
+  public void autonomousPeriodic() {
+    ChassisSpeeds speeds = autoCtrl.getCurMotorCmds(dt.getCtrlsPoseEstimate());
+    dt.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
+    pt.setDesiredPose(autoCtrl.getCurPose2d());
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    dt.drive(opInf.getFwdRevSpdCmd(), opInf.getRotateSpdCmd());
+  }
+
+  @Override
+  public void robotPeriodic() {
+    pt.setEstimatedPose(dt.getCtrlsPoseEstimate());
+    pt.update();
+  }
+
+  @Override
+  public void disabledPeriodic() {
+    dt.drive(0, 0);
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    if (opInf.getSimKickCmd()) {
+      dtSim.applyKick();
+    }
+    dtSim.update();
+    pt.setActualPose(dtSim.getCurPose());
+  }
+
+  private void resetOdometery() {
+    Pose2d startPose = autoCtrl.getInitialPose();
+    dtSim.resetPose(startPose);
+    dt.resetOdometry(startPose);
+  }
+}
diff --git a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java
new file mode 100644
index 0000000..033c8ec
--- /dev/null
+++ b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java
@@ -0,0 +1,136 @@
+package frc.sim;
+
+import org.photonvision.SimVisionSystem;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import frc.robot.Constants;
+
+/**
+ * Implementation of a simulation of robot physics, sensors, motor controllers
+ * Includes a Simulated PhotonVision system and one vision target.
+ * 
+ * This class and its methods are only relevant during simulation. While on the
+ * real robot, the real motors/sensors/physics are used instead.
+ */
+public class DrivetrainSim {
+
+    // Simulated Sensors
+    AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin);
+    EncoderSim leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA);
+    EncoderSim rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA);
+
+    // Simulated Motor Controllers
+    PWMSim leftLeader = new PWMSim(Constants.kDtLeftLeaderPin);
+    PWMSim leftFollower = new PWMSim(Constants.kDtLeftFollowerPin);
+    PWMSim rightLeader = new PWMSim(Constants.kDtRightLeaderPin);
+    PWMSim rightFollower = new PWMSim(Constants.kDtRightFollowerPin);
+
+    // Simulation Physics
+    // Configure these to match your drivetrain's physical dimensions
+    // and characterization results.
+    LinearSystem<N2, N2, N2> drivetrainSystem = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+    DifferentialDrivetrainSim drivetrainSimulator = new DifferentialDrivetrainSim(drivetrainSystem, DCMotor.getCIM(2),
+            8, Constants.kTrackWidth, Constants.kWheelRadius, null);
+
+    // Simulated Vision System.
+    // Configure these to match your PhotonVision Camera,
+    // pipeline, and LED setup.
+    double camDiagFOV = 75.0; // degrees
+    double camPitch = 15.0; // degrees
+    double camHeightOffGround = 0.85; // meters
+    double maxLEDRange = 20; // meters
+    int camResolutionWidth = 640; // pixels
+    int camResolutionHeight = 480; // pixels
+    double minTargetArea = 10; // square pixels
+
+    SimVisionSystem simVision = new SimVisionSystem(Constants.kCamName, camDiagFOV, camPitch, Constants.kCameraToRobot,
+            camHeightOffGround, maxLEDRange, camResolutionWidth, camResolutionHeight, minTargetArea);
+
+    public DrivetrainSim() {
+        simVision.addSimVisionTarget(Constants.kFarTarget);
+    }
+
+    /**
+     * Perform all periodic drivetrain simulation related tasks to advance our
+     * simulation of robot physics forward by a single 20ms step.
+     */
+    public void update() {
+
+        double leftMotorCmd = 0;
+        double rightMotorCmd = 0;
+
+        if (DriverStation.getInstance().isEnabled() && !RobotController.isBrownedOut()) {
+            // If the motor controllers are enabled...
+            // Roughly model the effect of leader and follower motor pushing on the same
+            // gearbox.
+            // Note if the software is incorrect and drives them against each other, speed
+            // will be
+            // roughly matching the physical situation, but current draw will _not_ be
+            // accurate.
+            leftMotorCmd = (leftLeader.getSpeed() + leftFollower.getSpeed()) / 2.0;
+            rightMotorCmd = (rightLeader.getSpeed() + rightFollower.getSpeed()) / 2.0;
+        }
+
+        // Update the physics simulation
+        drivetrainSimulator.setInputs(leftMotorCmd * RobotController.getInputVoltage(),
+                -rightMotorCmd * RobotController.getInputVoltage());
+        drivetrainSimulator.update(0.02);
+
+        // Update our sensors based on the simulated motion of the robot
+        leftEncoderSim.setDistance((drivetrainSimulator.getLeftPositionMeters()));
+        leftEncoderSim.setRate((drivetrainSimulator.getLeftVelocityMetersPerSecond()));
+        rightEncoderSim.setDistance((drivetrainSimulator.getRightPositionMeters()));
+        rightEncoderSim.setRate((drivetrainSimulator.getRightVelocityMetersPerSecond()));
+        gyroSim.setAngle(-drivetrainSimulator.getHeading().getDegrees()); // Gyros have an inverted reference frame for
+                                                                          // angles, so multiply by -1.0;
+
+        // Update PhotonVision based on our new robot position.
+        simVision.processFrame(drivetrainSimulator.getPose());
+
+    }
+
+    /**
+     * Resets the simulation back to a pre-defined pose Useful to simulate the
+     * action of placing the robot onto a specific spot in the field (IE, at the
+     * start of each match).
+     * 
+     * @param pose
+     */
+    public void resetPose(Pose2d pose) {
+        drivetrainSimulator.setPose(pose);
+    }
+
+    /**
+     * @return The simulated robot's position, in meters.
+     */
+    public Pose2d getCurPose() {
+        return drivetrainSimulator.getPose();
+    }
+
+    /**
+     * For testing purposes only! Applies an unmodeled, undetected offset to the
+     * pose Similar to if you magically kicked your robot to the side in a way the
+     * encoders and gyro didn't measure.
+     * 
+     * This distrubance should be corrected for once a vision target is in view.
+     */
+    public void applyKick() {
+        Pose2d newPose = drivetrainSimulator.getPose()
+                .transformBy(new Transform2d(new Translation2d(0, 0.5), new Rotation2d()));
+        drivetrainSimulator.setPose(newPose);
+    }
+
+}
diff --git a/java/sim-pose-estimation/vendordeps/WPILibOldCommands.json b/java/sim-pose-estimation/vendordeps/WPILibOldCommands.json
new file mode 100644
index 0000000..f9fbc4d
--- /dev/null
+++ b/java/sim-pose-estimation/vendordeps/WPILibOldCommands.json
@@ -0,0 +1,37 @@
+{
+  "fileName": "WPILibOldCommands.json",
+  "name": "WPILib-Old-Commands",
+  "version": "2020.0.0",
+  "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e",
+  "mavenUrls": [],
+  "jsonUrl": "",
+  "javaDependencies": [
+      {
+          "groupId": "edu.wpi.first.wpilibOldCommands",
+          "artifactId": "wpilibOldCommands-java",
+          "version": "wpilib"
+      }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+      {
+          "groupId": "edu.wpi.first.wpilibOldCommands",
+          "artifactId": "wpilibOldCommands-cpp",
+          "version": "wpilib",
+          "libName": "wpilibOldCommands",
+          "headerClassifier": "headers",
+          "sourcesClassifier": "sources",
+          "sharedLibrary": true,
+          "skipInvalidPlatforms": true,
+          "binaryPlatforms": [
+              "linuxathena",
+              "linuxraspbian",
+              "linuxaarch64bionic",
+              "windowsx86-64",
+              "windowsx86",
+              "linuxx86-64",
+              "osxx86-64"
+          ]
+      }
+  ]
+}
diff --git a/java/sim-pose-estimation/vendordeps/photonlib.json b/java/sim-pose-estimation/vendordeps/photonlib.json
new file mode 100644
index 0000000..662d44e
--- /dev/null
+++ b/java/sim-pose-estimation/vendordeps/photonlib.json
@@ -0,0 +1,35 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "2021.1.2",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/lib/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision.lib",
+            "artifactId": "PhotonLib-cpp",
+            "version": "2021.1.2",
+            "libName": "Photon",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxx86-64"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision.lib",
+            "artifactId": "PhotonLib-java",
+            "version": "2021.1.2"
+        }
+    ]
+}
\ No newline at end of file