remove vestigial structures

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/DistanceSensorPair.java

@@ -6,9 +6,7 @@
 import static java.lang.Math.PI;
 import static java.lang.Math.atan2;
 import static java.lang.Math.cos;
-import static java.lang.Math.sqrt;
 
-import com.kuriosityrobotics.firstforward.robot.sensors.kf.ExtendedKalmanFilter;
 import com.kuriosityrobotics.firstforward.robot.util.math.Point;
 import com.kuriosityrobotics.firstforward.robot.util.math.Pose;
 
@@ -114,19 +112,19 @@ public Wall bestWall(Pose covariance, Pose robotPose) {
         var sensor2Line = Lines.fromPointAndAngle(sensor2, sensorHeading, DistanceSensorLocaliser.p);
         var sensor2Segment = sensor2Line.rayFrom(sensor2);
 
-        if (wallInRangeOfRays(covariance, robotPose.heading, Wall.LEFT, sensor1Segment, sensor2Segment))
+        if (wallInRangeOfRays(covariance, Wall.LEFT, sensor1Segment, sensor2Segment))
             return Wall.LEFT;
-        else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.RIGHT, sensor1Segment, sensor2Segment))
+        else if (wallInRangeOfRays(covariance, Wall.RIGHT, sensor1Segment, sensor2Segment))
             return Wall.RIGHT;
-        else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.FRONT, sensor1Segment, sensor2Segment))
+        else if (wallInRangeOfRays(covariance, Wall.FRONT, sensor1Segment, sensor2Segment))
             return Wall.FRONT;
-        else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.BACK, sensor1Segment, sensor2Segment))
+        else if (wallInRangeOfRays(covariance, Wall.BACK, sensor1Segment, sensor2Segment))
             return Wall.BACK;
         else
             return null;
     }
 
-    private boolean wallInRangeOfRays(Pose variance, double heading, Wall wall, Ray... rays) {
+    private boolean wallInRangeOfRays(Pose variance, Wall wall, Ray... rays) {
         for (var sensorRay : rays) {
             var wallIntersection = sensorRay.intersection(wall.getSegment());
             if (wallIntersection == null)

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/IMU.java

@@ -1,8 +1,6 @@
 package com.kuriosityrobotics.firstforward.robot.sensors;
 
 import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.angleWrap;
-import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.mean;
-import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.median;
 import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.rotate;
 import static java.lang.Math.abs;
 import static java.lang.Math.toDegrees;
@@ -20,7 +18,6 @@
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.util.ReadWriteFile;
 
-import org.apache.commons.collections4.queue.CircularFifoQueue;
 import org.apache.commons.geometry.euclidean.twod.Vector2D;
 import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
 import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
@@ -41,7 +38,6 @@
 public class IMU implements Module {
     public static final String CALIBRATION_FILE = "imu_calibration.json";
     private static final File SETTINGS_FILE = AppUtil.getInstance().getSettingsFile(CALIBRATION_FILE);
-    private static final double BIAS_PER_REVOLUTION = toRadians(-1);
 
     private final BNO055IMU imu;
     private final ExtendedKalmanFilter filter;

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/Odometry.java

@@ -1,7 +1,5 @@
 package com.kuriosityrobotics.firstforward.robot.sensors;
 
-import static com.kuriosityrobotics.firstforward.robot.pathfollow.motionprofiling.MotionProfile.ROBOT_MAX_ACCEL;
-import static com.kuriosityrobotics.firstforward.robot.pathfollow.motionprofiling.MotionProfile.ROBOT_MAX_DECCEL;
 import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.rotate;
 import static java.lang.Math.pow;
 
@@ -37,10 +35,6 @@ public class Odometry extends RollingVelocityCalculator implements Module, Locat
     private double xVel = 0;
     private double yVel = 0;
     private double angleVel = 0;
-    // old vel for acceleration calculations
-    private double oldxVel = 0;
-    private double oldyVel = 0;
-    private double oldangleVel = 0;
     // change in position of the robot
     private double dx = 0;
     private double dy = 0;
@@ -55,8 +49,6 @@ public class Odometry extends RollingVelocityCalculator implements Module, Locat
     private double oldY = 0;
     private double oldHeading = 0;
     private double lastUpdateTime = 0;
-    private double thetaLR = 0;
-    private double thetaFB = 0;
 
     public Odometry(HardwareMap hardwareMap, Pose pose, ExtendedKalmanFilter kalmanFilter, IMU imu) {
         this.worldX = pose.x;
@@ -76,36 +68,26 @@ public Odometry(HardwareMap hardwareMap, Pose pose, ExtendedKalmanFilter kalmanF
         FileDump.addField("yVel", this);
     }
 
-    private double accelerationSigmoid(double dq, double accel) {
-        if (accel > 0)
-            return dq * (imu.isOffGround() /*|| accel <= 2 * ROBOT_MAX_ACCEL*/ ? 2 : .8);
-        else
-            return dq * (imu.isOffGround() /*|| -accel <= 2 * ROBOT_MAX_DECCEL*/ ? .8 : 2);
-
-    }
-
     public void update() {
         var now = SystemClock.elapsedRealtime();
-        var dt  = (now - lastUpdateTime) / 1000.;
+        var dt = (now - lastUpdateTime) / 1000.;
         if (lastUpdateTime == 0)
             dt = 1. / maxFrequency();
 
         calculatePosition();
 
-        var accel = imu.getAcceleration();
-        var headingDeviation = accelerationSigmoid(dHeading, Math.hypot(accel.x, accel.y));
-
         calculateInstantaneousVelocity();
         this.calculateRollingVelocity(new PoseInstant(getPose(), SystemClock.elapsedRealtime() / 1000.0));
 
+        var accel = imu.getAcceleration();
         kalmanFilter.builder()
                 .time(now)
                 .mean(dx, dy, dHeading)
                 .outputToState(rotate(kalmanFilter.outputVector()[2]))
                 .variance(
                         pow(dt * accel.x * .3, 2),
                         pow(dt * accel.y * .3, 2),
-                        pow(headingDeviation, 2)
+                        pow(dHeading * (imu.isOffGround() ? 2 : .8), 2)
                 )
                 .predict();
     }
@@ -143,10 +125,6 @@ private void calculateInstantaneousVelocity() {
         oldY = worldY;
         oldHeading = worldHeadingRad;
 
-        oldxVel = xVel;
-        oldyVel = yVel;
-        oldangleVel = angleVel;
-
         lastUpdateTime = currentUpdateTime;
     }
 
@@ -165,9 +143,6 @@ private void updateWorldPosition(double dLeftPod, double dRightPod, double dMeca
         double dThetaLR = (dLeftPodInches - dRightPodInches) / (2 * P);
         double dThetaFB = (dMecanumFrontPodInches - dMecanumBackPodInches) / (2 * Q);
 
-        thetaLR += dThetaLR;
-        thetaFB += dThetaFB;
-
         double X = Math.abs(dLeftPodInches + dRightPodInches);
         double Y = Math.abs(dMecanumFrontPodInches + dMecanumBackPodInches);
 
@@ -284,10 +259,6 @@ public Pose getInstantaneousVelocity() {
         return new Pose(xVel, yVel, angleVel);
     }
 
-    public double getVelMag() {
-        return Math.hypot(xVel, yVel);
-    }
-
     @Override
     public String getName() {
         return "Odometry";

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/kf/ExtendedKalmanFilter.java

@@ -140,8 +140,6 @@ private void backwardPass(int before) {
             }
 
             history.forEach(state -> {
-//                if (!state.getMean().equals(smoothedMeans.get(state)))
-//                    Log.d("EKF/smooth", format("before:  {0},  after:  {1}", state.getMean(), smoothedMeans.get(state)));
                 state.setMean(smoothedMeans.get(state));
                 state.setCovariance(smoothedVariances.get(state));
             });

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Point.java

@@ -3,6 +3,7 @@
 import static com.kuriosityrobotics.firstforward.robot.util.Constants.Field.FULL_FIELD;
 import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.doublesEqual;
 import static java.lang.Math.cos;
+import static java.lang.Math.hypot;
 import static java.lang.Math.sin;
 
 import androidx.annotation.NonNull;
@@ -112,5 +113,9 @@ public Point add(Point point) {
     public Point rotate(double angle) {
         return new Point(x * cos(angle) + y * sin(angle), -x * sin(angle) + y * cos(angle));
     }
+
+    public double norm() {
+        return hypot(x, y);
+    }
 }
 

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Pose.java

@@ -37,10 +37,6 @@ public static Pose fieldMirror(double x, double y, double heading) {
         return new Pose(FULL_FIELD - x, y, angleWrap(-heading));
     }
 
-    public double norm() {
-        return hypot(x, y);
-    }
-
     public static Pose relativeMirror(double x, double y, double heading) {
         return new Pose(-x, y, angleWrap(-heading));
     }