commit 289756f59dbce726a96c1578a3d1145fbe339dc5
author James Kuszmaul <jabukuszmaul@gmail.com> Tue Mar 05 21:52:10 2019 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Thu Mar 07 20:56:25 2019 -0800
tree 72ded53fee93ede7ac0a778a9e73bd51fd277f6a
parent 7616d66e6ed7156dd967620e59622c0100bedff1

Make skew in Ekf match skew from cameras

Also, add a few extra comments to the camera code, and add a couple
extra logging statements to the Ekf to help with future debugging.

Change-Id: I5fcfc852a3de42a2c320a3ff724d6d72e01f94ad

y2019/control_loops/drivetrain/localizer.h

diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index 3e20a1e..095c8a9 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -254,6 +254,7 @@
     }
 
     if (camera_views.size() == 0) {
+      LOG(DEBUG, "Unable to identify potential target matches.\n");
       // If we can't get a match, provide H = zero, which will make this
       // correction step a nop.
       *h = [](const State &, const Input &) { return Output::Zero(); };
@@ -279,6 +280,10 @@
         const TargetView target_view = target_views[ii];
         const Scalar match_score = scores(ii, view_idx);
         if (match_score > kRejectionScore) {
+          LOG(DEBUG,
+              "Rejecting target at (%f, %f, %f, %f) due to high score.\n",
+              target_view.reading.heading, target_view.reading.distance,
+              target_view.reading.skew, target_view.reading.height);
           h_functions->push_back(
               [](const State &, const Input &) { return Output::Zero(); });
           dhdx_functions->push_back([](const State &) {
@@ -296,8 +301,8 @@
                 best_view.target->pose().Rebase(&camera.pose());
             const Scalar heading = relative_pose.heading();
             const Scalar distance = relative_pose.xy_norm();
-            const Scalar skew =
-                ::aos::math::NormalizeAngle(relative_pose.rel_theta());
+            const Scalar skew = ::aos::math::NormalizeAngle(
+                relative_pose.rel_theta() - heading);
             return Output(heading, distance, skew);
           });
 
@@ -333,8 +338,10 @@
     Scalar ddistdx = -diffx / distance;
     Scalar ddistdy = -diffy / distance;
 
-    // Skew = target.theta - camera.theta:
-    Scalar dskewdtheta = -1.0;
+    // Skew = target.theta - camera.theta - heading
+    //      = target.theta - arctan2(target_pos - camera_pos)
+    Scalar dskewdx = -dheadingdx;
+    Scalar dskewdy = -dheadingdy;
     Eigen::Matrix<Scalar, kNOutputs, kNStates> H;
     H.setZero();
     H(0, 0) = dheadingdx;
@@ -342,7 +349,8 @@
     H(0, 2) = dheadingdtheta;
     H(1, 0) = ddistdx;
     H(1, 1) = ddistdy;
-    H(2, 2) = dskewdtheta;
+    H(2, 0) = dskewdx;
+    H(2, 1) = dskewdy;
     return H;
   }