diff --git a/R scripts/poseEstimationChecker.R b/R scripts/poseEstimationChecker.R
index e5adc2ce..fdc270d6 100644
--- a/R scripts/poseEstimationChecker.R	
+++ b/R scripts/poseEstimationChecker.R	
@@ -2,36 +2,8 @@
 rad2deg <- function(rad) {(rad * 180) / (pi)}
 deg2rad <- function(deg) {(deg * pi) / (180)}
 
-angleBetween <- function(leftX, leftY, rightX, rightY){
-  deltaX <- leftX-rightX
-  deltaY <- leftY-rightY
-  if (identical(deltaX, 0)){
-    ans <- pi/2
-  } else {
-    #Pretend it's first quadrant because we manually determine quadrants
-    ans <- atan(abs(deltaY/deltaX))
-  }
-  if (deltaY > 0){
-    if (deltaX > 0){
-      #If it's actually quadrant 1
-      return(ans)
-    }else {
-      #quadrant 2
-      return(pi - ans)
-    }
-    return(ans)
-  } else {
-    if (deltaX > 0){
-      #quadrant 4
-      return(-ans)
-    }else {
-      #quadrant 3
-      return(-(pi - ans))
-    }
-  }
-}
-
 logLogSlope <- function(effectiveWheelbase, angularVel, n){
+  library("zoo")
   avg <- weighted.mean(x=effectiveWheelbase, w=angularVel, na.rm=TRUE)
   error <- effectiveWheelbase-avg
   error <- error^2
@@ -44,12 +16,10 @@ logLogSlope <- function(effectiveWheelbase, angularVel, n){
   return(model)
 }
 
-drawRobot <- function(robotFile, robotPos){
-  theta <- angleBetween(leftX = robotPos[2], leftY = robotPos[3], rightX = robotPos[4], rightY = robotPos[5])
-  perp <- theta - pi/2
-  robotCenter <- c((robotPos[2]+robotPos[4])/2.,(robotPos[3]+robotPos[5])/2.)
+drawRobot <- function(robotFile, x, y, theta){
+  robotCenter <- c(x,y)
   robot <- read.csv(robotFile)
-  rotMatrix <- matrix(c(cos(perp), -sin(perp), sin(perp), cos(perp)), nrow=2, ncol=2, byrow=TRUE)
+  rotMatrix <- matrix(c(cos(theta), -sin(theta), sin(theta), cos(theta)), nrow=2, ncol=2, byrow=TRUE)
   
   point1s <- rotMatrix %*% matrix(c(robot$x1, robot$y1), nrow = 2, ncol = length(robot$x1), byrow = TRUE) 
   point1s <- point1s + c(robotCenter[1], robotCenter[2])
@@ -69,7 +39,7 @@ plotField <- function(filename, xOffset=0, yOffset=0){
   #Strings are read as factors by default, so we need to do this to make it read them as strings
   field$col <- as.character(field$col)
   for (i in 1:length(field$x1)){
-    lines(c(field$x1[i]+xOffset, field$x2[i]+xOffset), c(field$y1[i]+yOffset, field$y2[i]+yOffset), col=field$col[i])
+    lines(c(field$x1[i]+xOffset, field$x2[i]+xOffset), c(-field$y1[i]+yOffset, -field$y2[i]+yOffset), col=field$col[i])
   }
 }
 
@@ -109,7 +79,6 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
   #Find the mean wheelbase, weighted by angular vel because higher angular vel decreases variance
   avgWheelbase = weighted.mean(x=combinedAngular[,2], w=combinedAngular[,1], na.rm=TRUE)
   
-  return(combinedAngular)
   #plot wheelbase vs angular vel
   plot(x=combinedAngular[,1], y=combinedAngular[,2], xlab="Angular Velocity (rad/sec)", ylab="Effective wheelbase diameter (feet)", main="Effective Wheelbase Diameter vs. Angular Velocity")
   abline(a=avgWheelbase, b=0, col='green')
@@ -119,7 +88,7 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
   abline(a=avgWheelbase, b=0, col='green')
   
   #Plot wheelbase vs turn radius
-  plot(x=combinedAngular[,3]/combinedAngular[,1], y=combinedAngular[,2]-2.0833, xlab="Turn Radius (feet)", ylab="Error in wheelbase diameter (feet)", main="Error in Wheelbase Diameter vs. Turn Radius")
+  plot(x=combinedAngular[,3]/combinedAngular[,1], y=combinedAngular[,2], xlab="Turn Radius (feet)", ylab="Effective wheelbase diameter (feet)", main="Effective Wheelbase Diameter vs. Turn Radius")
   abline(a=avgWheelbase, b=0, col='green')
   
   return(avgWheelbase)
@@ -324,4 +293,42 @@ doEverything <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angular
   ignoreWorstPoseEstimation(leftPos, rightPos, rawAngleDegrees, timeMillis, actualWheelbase)
   encoderOnlyPoseEstimation(leftPos=leftPos, rightPos=rightPos, startingAngleDegrees=rawAngleDegrees[1], timeMillis=timeMillis, fakeWheelbase=avg)
   return(avg)
-}
\ No newline at end of file
+}
+
+animateRobot <- function(x, y, headingRadians, deltaTime, fieldFile, robotFile, robotRadius, frameSize=-1, filename="animation.mp4"){
+  library("animation")
+  theta <- deg2rad(headingRadians)
+  saveVideo({
+    for(i in 1:length(x)){
+      #Set up frame
+      if(frameSize == -1){
+        plot(x=c(),y=c(),xlim=c(min(x)-robotRadius, max(x)+robotRadius),ylim=c(min(y)-robotRadius, max(y)+robotRadius), asp=1, xlab="X position (feet)", ylab="Y position (feet)")
+      } else {
+        plot(x=c(),y=c(),xlim=c(x[i]-frameSize/2, x[i]+frameSize/2),ylim=c(y[i]-frameSize/2, y[i]+frameSize/2),asp=1, xlab="X position (feet)", ylab="Y position (feet)")
+      }
+      plotField(fieldFile, 0, 0)
+      drawRobot(robotFile, x[i],y[i],theta[i])
+    }
+  }, interval = deltaTime, ani.width = 600, ani.height = 600, video.name=filename)
+}
+
+tracedAnimation <- function(x, y, leftX, leftY, rightX, rightY, headingRadians, deltaTime, fieldFile, robotFile, robotRadius, frameSize=-1, filename="animation.mp4"){
+  library("animation")
+  theta <- deg2rad(headingRadians)
+  saveVideo({
+    for(i in 1:length(x)){
+      #Set up frame
+      if(frameSize == -1){
+        plot(x=c(),y=c(),xlim=c(min(x)-robotRadius, max(x)+robotRadius),ylim=c(min(y)-robotRadius, max(y)+robotRadius), asp=1, xlab="X position (feet)", ylab="Y position (feet)")
+      } else {
+        plot(x=c(),y=c(),xlim=c(x[i]-frameSize/2, x[i]+frameSize/2),ylim=c(y[i]-frameSize/2, y[i]+frameSize/2),asp=1, xlab="X position (feet)", ylab="Y position (feet)")
+      }
+      lines(x=leftX[1:i], y=leftY[1:i], col="Green")
+      lines(x=rightX[1:i], y=rightY[1:i], col="Red")
+      plotField(fieldFile, 0, 0)
+      drawRobot(robotFile, x[i],y[i],theta[i])
+    }
+  }, interval = deltaTime, ani.width = 1920, ani.height = 1080, video.name=filename)
+}
+
+#tracedAnimation(equalScrub[,1],equalScrub[,2], equalScrub[,3],equalScrub[,4],equalScrub[,5],equalScrub[,6],rel$Drive.raw_angle,0.05,"field.csv","robot.csv",27.138/12, -1, "traceAnimation.mp4")
\ No newline at end of file
diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/Logger.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/Logger.java
index c15e1dba..236d8438 100644
--- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/Logger.java
+++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/Logger.java
@@ -59,6 +59,11 @@ public class Logger implements Runnable {
 	 */
 	private final double loopTimeSecs;
 
+	/**
+	 * The time this that logging started. We don't use {@link Clock} because this is a separate thread.
+	 */
+	private final long startTime;
+
 	/**
 	 * Default constructor.
 	 *
@@ -77,6 +82,7 @@ public Logger(@NotNull @JsonProperty(required = true) Loggable[] subsystems,
 	              @NotNull @JsonProperty(required = true) String telemetryLogFilename) throws IOException {
 		//Set up the file names, using a time stamp to avoid overwriting old log files.
 		String timeStamp = new SimpleDateFormat("yyyy.MM.dd.HH.mm.ss").format(new Date());
+		startTime = System.currentTimeMillis();
 		this.eventLogFilename = eventLogFilename + timeStamp + ".csv";
 		this.telemetryLogFilename = telemetryLogFilename + timeStamp + ".csv";
 
@@ -95,7 +101,7 @@ public Logger(@NotNull @JsonProperty(required = true) Loggable[] subsystems,
 		eventLogWriter.write("time,class,message"+"\n");
 		//We use a StringBuilder because it's better for building up a string via concatenation.
 		StringBuilder telemetryHeader = new StringBuilder();
-		telemetryHeader.append("time,");
+		telemetryHeader.append("time,Clock.time,");
 		for (int i = 0; i < this.subsystems.length; i++) {
 			String[] items = this.subsystems[i].getHeader();
 			//Initialize itemNames rows
@@ -161,8 +167,12 @@ public void run() {
 		events = new ArrayList<>();
 		//We use a StringBuilder because it's better for building up a string via concatenation.
 		StringBuilder telemetryData = new StringBuilder();
-		//Loop through each datum
+
+		//Log the times
+		telemetryData.append(System.currentTimeMillis()-startTime).append(",");
 		telemetryData.append(Clock.currentTimeMillis()).append(",");
+
+		//Loop through each datum
 		for (int i = 0; i < subsystems.length; i++) {
 			Object[] data = subsystems[i].getData();
 			for (int j = 0; j < data.length; j++) {
diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java
index fefc2ebb..2d40a5e3 100644
--- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java
+++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java
@@ -5,8 +5,6 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import com.fasterxml.jackson.annotation.ObjectIdGenerators;
 import org.jetbrains.annotations.NotNull;
-import org.jetbrains.annotations.Nullable;
-import org.usfirst.frc.team449.robot.Robot;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
 import org.usfirst.frc.team449.robot.generalInterfaces.loggable.Loggable;
 import org.usfirst.frc.team449.robot.jacksonWrappers.MappedRunnable;
@@ -21,12 +19,6 @@
 @JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
 public class UnidirectionalPoseEstimator <T extends SubsystemNavX & DriveUnidirectional> implements MappedRunnable, Loggable {
 
-	/**
-	 * The wheel-to-wheel diameter of the robot, in feet.
-	 */
-	@Nullable
-	private final Double robotDiameter;
-
 	/**
 	 * The subsystem to get gyro and encoder data from.
 	 */
@@ -108,7 +100,6 @@ public class UnidirectionalPoseEstimator <T extends SubsystemNavX & DriveUnidire
 	/**
 	 * Default constructor.
 	 *
-	 * @param robotDiameter             The wheel-to-wheel diameter of the robot, in feet.
 	 * @param subsystem                 The subsystem to get gyro and encoder data from.
 	 * @param absolutePosAngleTolerance The maximum amount, in degrees, a new absolute position's angle can be off from
 	 *                                  the gyro reading and still be accepted as valid.
@@ -117,13 +108,11 @@ public class UnidirectionalPoseEstimator <T extends SubsystemNavX & DriveUnidire
 	 * @param startTheta                The starting angle of the robot, in degrees. Defaults to 0.
 	 */
 	@JsonCreator
-	public UnidirectionalPoseEstimator(@Nullable Double robotDiameter,
-	                                   @JsonProperty(required = true) @NotNull T subsystem,
+	public UnidirectionalPoseEstimator(@JsonProperty(required = true) @NotNull T subsystem,
 	                                   @JsonProperty(required = true) double absolutePosAngleTolerance,
 	                                   double startX,
 	                                   double startY,
 	                                   double startTheta) {
-		this.robotDiameter = robotDiameter;
 		this.subsystem = subsystem;
 		this.absolutePosAngleTolerance = absolutePosAngleTolerance;
 		lastTheta = startTheta;
@@ -144,47 +133,29 @@ public UnidirectionalPoseEstimator(@Nullable Double robotDiameter,
 		lastTime = 0;
 	}
 
-	private static double[] calcEliVector(double left, double right, double deltaTheta, double lastAngle){
-		//The vector for how much the robot moves, element 0 is x and element 1 is y.
-		double[] vector = new double[2];
-
-		//If we're going in a straight line
-		if (deltaTheta == 0) {
-			//we could use deltaRight here, doesn't matter. Going straight means no change in angle and left and right are the same.
-			vector[0] = left * Math.cos(lastAngle);
-			vector[1] = left * Math.sin(lastAngle);
-		} else {
-			//This next part is too complicated to explain in comments. Read this wiki page instead:
-			// http://team449.shoutwiki.com/wiki/Pose_Estimation
-			double r = ((left+right)/2.)/deltaTheta;
-			double vectorAngle = lastAngle + deltaTheta/2.;
-			double vectorMagnitude = 2. * r * Math.sin(deltaTheta / 2.);
-			vector[0] = vectorMagnitude * Math.cos(vectorAngle);
-			vector[1] = vectorMagnitude * Math.sin(vectorAngle);
-		}
-		return vector;
-	}
-
-	private static double[] calcVector(double left, double right, double robotDiameter, double deltaTheta, double lastAngle) {
+	/**
+	 * Calculate the x and y movement vector for the robot.
+	 *
+	 * @param left The displacement of the left encoder, in feet
+	 * @param right The displacement of the right encoder, in feet
+	 * @param deltaTheta The angular displacement, in radians
+	 * @param lastAngle The previous heading, in radians
+	 * @return An array of length 2 containing the [x, y] displacement of the robot.
+	 */
+	private static double[] calcVector(double left, double right, double deltaTheta, double lastAngle){
 		//The vector for how much the robot moves, element 0 is x and element 1 is y.
 		double[] vector = new double[2];
 
 		//If we're going in a straight line
 		if (deltaTheta == 0) {
 			//we could use deltaRight here, doesn't matter. Going straight means no change in angle and left and right are the same.
-			vector[0] = left * Math.cos(lastAngle);
-			vector[1] = left * Math.sin(lastAngle);
+			vector[0] = (left+right)/2. * Math.cos(lastAngle);
+			vector[1] = (left+right)/2. * Math.sin(lastAngle);
 		} else {
 			//This next part is too complicated to explain in comments. Read this wiki page instead:
 			// http://team449.shoutwiki.com/wiki/Pose_Estimation
-			double r;
-			if (left-right == 0){
-				r = left/deltaTheta;
-			} else {
-				r = robotDiameter / 2. * (left + right) / (left - right);
-			}
 			double vectorAngle = lastAngle + deltaTheta/2.;
-			double vectorMagnitude = 2. * r * Math.sin(deltaTheta / 2.);
+			double vectorMagnitude = 2. * ((left+right)/2.)/deltaTheta * Math.sin(deltaTheta / 2.);
 			vector[0] = vectorMagnitude * Math.cos(vectorAngle);
 			vector[1] = vectorMagnitude * Math.sin(vectorAngle);
 		}
@@ -206,53 +177,14 @@ public synchronized void run() {
 		double deltaLeft = left - lastLeftPos;
 		double deltaRight = right - lastRightPos;
 		double deltaTheta = theta - lastTheta;
-		double robotDiameter;
+
 		if (deltaTheta == 0){
 			fudgedWheelbaseDiameter = -1;
 		} else {
 			fudgedWheelbaseDiameter = (deltaLeft - deltaRight) / deltaTheta;
 		}
 
-		double[] vector;
-		if (this.robotDiameter != null) {
-			//Noah's Approach:
-
-			//For this next part, we assume that the gyro is 100% accurate at measuring the change in angle over the given
-
-			//time period and that the encoders will possibly overmeasure (due to wheel slip) but never undermeasure.
-			//Given those constraints, we have an overdetermined system because deltaTheta should be equal to
-			//(deltaLeft-deltaRight)/robotDiameter. We can use this to determine which wheel slipped more, and replace its
-			//reading with a value calculated from the other encoder and the gyro.
-			robotDiameter = this.robotDiameter;
-			if (deltaTheta < (deltaLeft - deltaRight) / robotDiameter) {
-				if (deltaLeft > 0) {
-					percentChanged = ((deltaRight + robotDiameter * deltaTheta) - deltaLeft) / deltaLeft;
-					deltaLeft = deltaRight + robotDiameter * deltaTheta;
-					recalcedLeft = true;
-				} else {
-					percentChanged = ((deltaLeft - robotDiameter * deltaTheta) - deltaRight) / deltaRight;
-					deltaRight = deltaLeft - robotDiameter * deltaTheta;
-					recalcedLeft = false;
-				}
-			} else if (deltaTheta > (deltaLeft - deltaRight) / robotDiameter) {
-				if (deltaLeft < 0) {
-					percentChanged = ((deltaRight + robotDiameter * deltaTheta) - deltaLeft) / deltaLeft;
-					deltaLeft = deltaRight + robotDiameter * deltaTheta;
-					recalcedLeft = true;
-				} else {
-					percentChanged = ((deltaLeft - robotDiameter * deltaTheta) - deltaRight) / deltaRight;
-					deltaRight = deltaLeft - robotDiameter * deltaTheta;
-					recalcedLeft = false;
-				}
-			}
-			vector = calcVector(deltaLeft, deltaRight, robotDiameter, deltaTheta, lastTheta);
-		} else {
-
-			//Eli's Approach
-
-			//Here we assume all the measured values are correct and adjust the diameter to match.
-			vector = calcEliVector(deltaLeft, deltaRight, deltaTheta, lastTheta);
-		}
+		double[] vector = calcVector(deltaLeft, deltaRight, deltaTheta, lastTheta);
 
 		//The vector for how much the robot moves, element 0 is x and element 1 is y.
 
@@ -447,9 +379,6 @@ public Object[] getData() {
 	@NotNull
 	@Override
 	public String getName() {
-		if(robotDiameter != null){
-			return "NoahPoseEstimator";
-		}
-		return "EliPoseEstimator";
+		return "PoseEstimator";
 	}
 }
diff --git a/RoboRIO/src/main/resources/calcifer_map.yml b/RoboRIO/src/main/resources/calcifer_map.yml
index 71239ba1..4ab8b444 100644
--- a/RoboRIO/src/main/resources/calcifer_map.yml
+++ b/RoboRIO/src/main/resources/calcifer_map.yml
@@ -305,7 +305,7 @@ logger:
             absolutePosAngleTolerance: 5
 #        - org.usfirst.frc.team449.robot.subsystem.complexooter.LoggingShooter:
 #            multiSubsystem
-    loopTimeSecs: 0.05
+    loopTimeSecs: 0.02
     eventLogFilename: "/home/lvuser/logs/eventLog-"
     telemetryLogFilename: "/home/lvuser/logs/telemetryLog-"
 centerAuto: