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
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index 04e4157..2ab4b4f 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -110,7 +110,8 @@
Scalar distance; // meters
// Height of the target from the camera.
Scalar height; // meters
- // The angle of the target relative to the frame of the camera.
+ // The angle of the target relative to line between the camera and
+ // the center of the target.
Scalar skew; // radians
};
Reading reading;
@@ -281,7 +282,8 @@
const Pose relative_pose = target.pose().Rebase(&camera_abs_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);
// Check if the camera is in the angular FOV.
if (::std::abs(heading) > fov_ / 2.0) {