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) {
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index d491c14..8c699c5 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -212,7 +212,7 @@
.Send();
RunForTime(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate(1e-8);
+ VerifyEstimatorAccurate(1e-7);
}
// Tests that not having cameras with an initial disturbance results in
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;
}