Fix bad numerical derivative (fixes #79)

technototes · Jun 5, 2020 · 84b81c8 · 84b81c8
1 parent 21a4ebf
commit 84b81c8
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java
@@ -52,11 +52,14 @@ public AccelResult(double kA, double rSquare) {
      */
     private static List<Double> numericalDerivative(List<Double> x, List<Double> y) {
         List<Double> deriv = new ArrayList<>(x.size());
-        deriv.add(0.0);
-        for (int i = 0; i < x.size(); i++) {
-            deriv.add((y.get(i + 1) - y.get(i - 1)) / (x.get(i + 1) - x.get(i - 1)));
+        for (int i = 1; i < x.size() - 1; i++) {
+            deriv.add(
+                    (y.get(i + 1) - y.get(i - 1)) /
+                    (x.get(i + 1) - x.get(i - 1))
+            );
         }
-        deriv.set(0, deriv.get(1));
+        // copy endpoints to pad output
+        deriv.add(0, deriv.get(0));
         deriv.add(deriv.get(deriv.size() - 1));
         return deriv;
     }