Update some tunings & swap to heading/distance/skew
* Tune down the velocity offset variances, which significantly
reduces the odds of horrible corrections killing the robot's
position.
* Swap over to using heading/distance/skew for localizer corrections.
* Tune noise values associated with the heading/distance/skew.
Had to lazily make some of the tests pass....
Change-Id: I6ac7009bbbbbfa92cf0aa6957e2f7156de72510f
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index 61e78e7..dc7c568 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -29,11 +29,11 @@
DEFINE_double(max_distance_to_target, 5.0,
"Reject target poses that have a 3d distance of more than this "
"many meters.");
-DEFINE_double(max_auto_image_robot_speed, 2.0,
+DEFINE_double(max_auto_image_robot_speed, 5.0,
"Reject target poses when the robot is travelling faster than "
"this speed in auto.");
DEFINE_bool(
- do_xytheta_corrections, true,
+ do_xytheta_corrections, false,
"If set, uses the x/y/theta corrector rather than a heading/distance/skew "
"one. This is better conditioned currently, but is theoretically worse due "
"to not capturing noise effectively.");
@@ -85,55 +85,55 @@
Localizer::HybridEkf::kNStates>
P_transpose;
// Grabbed from when the robot was in a steady-state.
- P_transpose << 0.00344391020344026, 2.78255540964953e-05,
- -3.44257436790434e-09, 1.57165298196431e-09, 0.0207259965606711,
- 1.57165298180587e-09, 0.0207259965606711, 0.054775354511474,
- 0.0547753545094318, 3.6435938125014e-13, 0.0136249573295751,
- -1.00705421392865e-05, 2.78255540964953e-05, 0.00107448929200992,
- -7.42495169208041e-08, 1.85634700506266e-11, 0.000244343925617656,
- 1.85634874205036e-11, 0.000244343925617656, 0.000645553479721632,
- 0.000645553790286344, -3.98991820983687e-11, 0.000160471639203211,
- 0.00085437373557969, -3.44257436791122e-09, -7.42495169208033e-08,
- 8.84891122456971e-05, 5.60929454430362e-16, -3.19015358072956e-08,
- 1.00798618104673e-15, -3.19015357689791e-08, 4.05905848804053e-07,
- -5.37043312466153e-07, 2.59177623699213e-08, -3.54286115799832e-08,
- -2.46295184320124e-07, 1.57165298196416e-09, 1.85634700506217e-11,
- 5.60929459005148e-16, 4.99891338811926e-09, 3.59436612693873e-08,
- 3.54690022095621e-18, 3.59436612693713e-08, 1.47025442116767e-07,
- 1.47035806190949e-07, 4.66877989234937e-08, -1.08209016210542e-08,
- -3.39984473837553e-14, 0.0207259965606711, 0.000244343925617655,
- -3.19015358072649e-08, 3.59436612693935e-08, 0.301240404540565,
- 3.59436612690168e-08, 0.301240404540535, 1.05741200222346,
- 1.0574120022184, 8.29472747900822e-13, 0.138401597893958,
- -1.43941751907531e-06, 1.57165298180564e-09, 1.85634874205096e-11,
- 1.00798617244172e-15, 3.54690020212816e-18, 3.59436612690224e-08,
- 4.99891338811924e-09, 3.59436612690048e-08, 1.4703580619019e-07,
- 1.47025442115683e-07, -4.66877989234395e-08, -1.0820901621393e-08,
- -3.40023588372784e-14, 0.0207259965606711, 0.000244343925617655,
- -3.19015357689244e-08, 3.59436612693859e-08, 0.301240404540535,
- 3.5943661269025e-08, 0.301240404540565, 1.05741200222289,
- 1.05741200221897, 8.29752131358864e-13, 0.138401597893958,
- -1.43941751907531e-06, 0.0547753545114739, 0.000645553479721628,
- 4.05905848802199e-07, 1.4702544211661e-07, 1.05741200222346,
- 1.47035806190016e-07, 1.05741200222289, 5.51003071369415,
- 4.85571868991385, 3.11581831710161e-06, 0.388669918077443,
- -2.97795819369728e-06, 0.054775354509432, 0.000645553790286345,
- -5.37043312465425e-07, 1.47035806190839e-07, 1.0574120022184,
- 1.47025442115792e-07, 1.05741200221897, 4.85571868991385,
- 5.51003071367444, -3.11581462746269e-06, 0.388669918072973,
- -2.97799067538699e-06, 3.64359250152554e-13, -3.98991820983e-11,
- 2.5917762369921e-08, 4.66877989234987e-08, 8.2947423101614e-13,
- -4.66877989234886e-08, 8.29754326977491e-13, 3.11581831710969e-06,
- -3.11581462747612e-06, 0.212136173309098, 8.06835372350592e-13,
- 8.80190080862899e-12, 0.0136249573295751, 0.000160471639203211,
- -3.54286115799303e-08, -1.08209016210412e-08, 0.138401597893958,
- -1.08209016213997e-08, 0.138401597893958, 0.388669918077444,
- 0.388669918072972, 8.06834601598773e-13, 0.187427410345505,
- -1.28632768080328e-06, -1.00705421392865e-05, 0.000854373735579689,
- -2.46295184320122e-07, -3.39984473838037e-14, -1.4394175190755e-06,
- -3.40023588373113e-14, -1.4394175190755e-06, -2.97795819369787e-06,
- -2.9779906753875e-06, 8.80190080900245e-12, -1.28632768080338e-06,
- 0.00381653175156393;
+ P_transpose << 0.00478504226469438, 0.000253940126278529,
+ -0.000162526741742492, 2.25403185759796e-09, 0.0101734987442698,
+ 2.25403195618803e-09, 0.0101734987442698, 0.0253922208811703,
+ 0.0253922210268363, -2.21692792749728e-10, 1.30552506376491e-05,
+ 8.24314992005184e-07, 0.000253940126278532, 0.00189751717312843,
+ 0.000513974713526466, 2.03445653416419e-10, 0.00091777414692514,
+ 2.03445505573468e-10, 0.00091777414692514, 0.002190445323373,
+ 0.00219044511582939, 3.32473307499143e-10, 1.45178014834701e-06,
+ 1.71788107973058e-05, -0.000162526741742491, 0.000513974713526467,
+ 0.000241235997378754, -2.30353529071927e-12, -1.03627077991455e-05,
+ -2.30350039899681e-12, -1.03627077991157e-05, -6.36337811958761e-06,
+ -6.62065263890835e-06, 1.24447423005307e-09, -1.228397466134e-07,
+ 2.45695800192927e-06, 2.25403185760077e-09, 2.0344565341686e-10,
+ -2.30353529071687e-12, 4.99964876555835e-09, 4.09452976434092e-08,
+ -1.11086080247582e-15, 4.09452976433419e-08, 1.61945884581856e-07,
+ 1.61950413812579e-07, 4.58556491207338e-08, -1.0257731581937e-12,
+ 3.0118336328036e-13, 0.0101734987442698, 0.000917774146925141,
+ -1.03627077991456e-05, 4.09452976433247e-08, 0.186711669156372,
+ 4.09452978206736e-08, 0.186711669156351, 0.747606782854604,
+ 0.747606783311591, -3.98476625129118e-10, 4.53292935526394e-05,
+ 1.34809505728832e-06, 2.2540319561823e-09, 2.03445505573217e-10,
+ -2.30350039893596e-12, -1.11086077014401e-15, 4.09452978206229e-08,
+ 4.99964876557609e-09, 4.0945297820556e-08, 1.61950414014674e-07,
+ 1.61945884988215e-07, -4.58556492982177e-08, -1.02577287448067e-12,
+ 3.01180296453645e-13, 0.0101734987442698, 0.000917774146925141,
+ -1.03627077991158e-05, 4.09452976433153e-08, 0.186711669156351,
+ 4.09452978206613e-08, 0.186711669156372, 0.747606782852986,
+ 0.747606783313209, -3.98476449084643e-10, 4.53292935526394e-05,
+ 1.34809505728832e-06, 0.0253922208811701, 0.00219044532337299,
+ -6.36337811958279e-06, 1.61945884583411e-07, 0.747606782854602,
+ 1.61950414014798e-07, 0.747606782852984, 4.36530695987946,
+ 4.17234874741425, 7.37989263565032e-07, 0.000112905097332305,
+ 1.0761727407346e-06, 0.025392221026836, 0.00219044511582942,
+ -6.62065263891535e-06, 1.61950413812353e-07, 0.747606783311594,
+ 1.61945884987625e-07, 0.747606783313212, 4.17234874741427,
+ 4.36530696204959, -7.39350829913324e-07, 0.000112905097765367,
+ 1.07616825738023e-06, -2.21692793550929e-10, 3.32473307500738e-10,
+ 1.24447423005688e-09, 4.58556491207295e-08, -3.98476620685685e-10,
+ -4.58556492984426e-08, -3.98476445724907e-10, 7.3798926341289e-07,
+ -7.39350829974392e-07, 0.212257282137077, -6.38021734059486e-13,
+ 6.89673203238e-12, 1.30552506376492e-05, 1.451780148347e-06,
+ -1.22839746613403e-07, -1.02577315819363e-12, 4.53292935526395e-05,
+ -1.02577287448185e-12, 4.53292935526395e-05, 0.000112905097332305,
+ 0.000112905097765368, -6.38021733687597e-13, 4.99487202342848e-05,
+ 7.45706935797857e-09, 8.24314992005172e-07, 1.7178810797306e-05,
+ 2.45695800192931e-06, 3.01183363281006e-13, 1.34809505728833e-06,
+ 3.01180296453493e-13, 1.34809505728833e-06, 1.07617274073465e-06,
+ 1.07616825738027e-06, 6.89673203233812e-12, 7.45706935797858e-09,
+ 4.97065161286885e-05;
return P_transpose.transpose();
}
} // namespace
@@ -419,7 +419,11 @@
RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, debug_builder);
return;
} else {
- april_tag_noise_scalar = 10.0;
+ if (utils_.MaybeInAutonomous()) {
+ april_tag_noise_scalar = 2.0;
+ } else {
+ april_tag_noise_scalar = 10.0;
+ }
}
}
@@ -453,12 +457,18 @@
debug_builder);
}
+ const double robot_speed =
+ (state_at_capture.value()(StateIdx::kLeftVelocity) +
+ state_at_capture.value()(StateIdx::kRightVelocity)) /
+ 2.0;
+
Corrector corrector(state_at_capture.value(), H_field_target, H_robot_camera,
H_camera_target);
const double distance_to_target = corrector.observed()(Corrector::kDistance);
// Heading, distance, skew at 1 meter.
- Eigen::Matrix<double, 3, 1> noises(0.01, 0.05, 0.05);
+ Eigen::Matrix<double, 3, 1> noises(0.03, 0.25, 0.15);
+ noises *= 2.0;
const double distance_noise_scalar =
std::min(1.0, std::pow(distance_to_target, 2.0));
noises(Corrector::kDistance) *= distance_noise_scalar;
@@ -467,6 +477,7 @@
// Scale noise by the distortion factor for this detection
noises *= (1.0 + FLAGS_distortion_noise_scalar * target.distortion_factor());
noises *= april_tag_noise_scalar;
+ noises *= (1.0 + std::abs(robot_speed));
Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
R.diagonal() = noises.cwiseAbs2();
@@ -501,11 +512,6 @@
aos::math::NormalizeAngle(corrector.expected_camera_pose().abs_theta() -
corrector.observed_camera_pose().abs_theta());
constexpr double kDegToRad = M_PI / 180.0;
-
- const double robot_speed =
- (state_at_capture.value()(StateIdx::kLeftVelocity) +
- state_at_capture.value()(StateIdx::kRightVelocity)) /
- 2.0;
const double yaw_threshold =
(utils_.MaybeInAutonomous() ? FLAGS_max_implied_yaw_error
: FLAGS_max_implied_teleop_yaw_error) *