Tune down estimator for ADIS16505
* Update down_estimator_plotter for the ADIS16505 setup (would be
nice to refactor things to make it so that you can apply a plot
config to a particular submessage type...).
* Crank up the threshold to consider the IMU still, to avoid resetting
in the middle of real motion.
* Also check in the IMU transform from the jankily-mounted IMU on the
2020 robot.
Change-Id: I4c6791524c3436614db045d3f2ba371f2400b084
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/control_loops/drivetrain/down_estimator_plotter.ts b/frc971/control_loops/drivetrain/down_estimator_plotter.ts
index 7f5bd58..178ab0b 100644
--- a/frc971/control_loops/drivetrain/down_estimator_plotter.ts
+++ b/frc971/control_loops/drivetrain/down_estimator_plotter.ts
@@ -12,10 +12,10 @@
const aosPlotter = new AosPlotter(conn);
const status = aosPlotter.addMessageSource(
- '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+ '/localizer', 'frc971.controls.LocalizerStatus');
const imu = aosPlotter.addRawMessageSource(
- '/drivetrain', 'frc971.IMUValuesBatch',
+ '/localizer', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
const accelPlot = aosPlotter.addPlot(element, [width, height]);
@@ -24,11 +24,11 @@
accelPlot.plot.getAxisLabels().setYLabel('Acceleration (m/s/s)');
accelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
- const accelX = accelPlot.addMessageLine(status, ['down_estimator', 'accel_x']);
+ const accelX = accelPlot.addMessageLine(status, ['model_based', 'down_estimator', 'accel_x']);
accelX.setColor(RED);
- const accelY = accelPlot.addMessageLine(status, ['down_estimator', 'accel_y']);
+ const accelY = accelPlot.addMessageLine(status, ['model_based', 'down_estimator', 'accel_y']);
accelY.setColor(GREEN);
- const accelZ = accelPlot.addMessageLine(status, ['down_estimator', 'accel_z']);
+ const accelZ = accelPlot.addMessageLine(status, ['model_based', 'down_estimator', 'accel_z']);
accelZ.setColor(BLUE);
const velPlot = aosPlotter.addPlot(element, [width, height]);
@@ -36,11 +36,11 @@
velPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
velPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
- const velX = velPlot.addMessageLine(status, ['down_estimator', 'velocity_x']);
+ const velX = velPlot.addMessageLine(status, ['model_based', 'down_estimator', 'velocity_x']);
velX.setColor(RED);
- const velY = velPlot.addMessageLine(status, ['down_estimator', 'velocity_y']);
+ const velY = velPlot.addMessageLine(status, ['model_based', 'down_estimator', 'velocity_y']);
velY.setColor(GREEN);
- const velZ = velPlot.addMessageLine(status, ['down_estimator', 'velocity_z']);
+ const velZ = velPlot.addMessageLine(status, ['model_based', 'down_estimator', 'velocity_z']);
velZ.setColor(BLUE);
const gravityPlot = aosPlotter.addPlot(element, [width, height]);
@@ -48,7 +48,7 @@
gravityPlot.plot.getAxisLabels().setXLabel('Monotonic Sent Time (sec)');
gravityPlot.plot.setDefaultYRange([0.95, 1.05]);
const gravityLine =
- gravityPlot.addMessageLine(status, ['down_estimator', 'gravity_magnitude']);
+ gravityPlot.addMessageLine(status, ['model_based', 'down_estimator', 'gravity_magnitude']);
gravityLine.setColor(RED);
gravityLine.setDrawLine(false);
const accelMagnitudeLine =
@@ -63,15 +63,15 @@
orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
const roll = orientationPlot.addMessageLine(
- status, ['down_estimator', 'lateral_pitch']);
+ status, ['model_based', 'down_estimator', 'lateral_pitch']);
roll.setColor(RED);
roll.setLabel('roll');
const pitch = orientationPlot.addMessageLine(
- status, ['down_estimator', 'longitudinal_pitch']);
+ status, ['model_based', 'down_estimator', 'longitudinal_pitch']);
pitch.setColor(GREEN);
pitch.setLabel('pitch');
const yaw = orientationPlot.addMessageLine(
- status, ['down_estimator', 'yaw']);
+ status, ['model_based', 'down_estimator', 'yaw']);
yaw.setColor(BLUE);
yaw.setLabel('yaw');
@@ -101,15 +101,15 @@
imuAccelZFiltered.setPointSize(0);
const expectedAccelX = imuAccelPlot.addMessageLine(
- status, ['down_estimator', 'expected_accel_x']);
+ status, ['model_based', 'down_estimator', 'expected_accel_x']);
expectedAccelX.setColor(RED);
expectedAccelX.setPointSize(0);
const expectedAccelY = imuAccelPlot.addMessageLine(
- status, ['down_estimator', 'expected_accel_y']);
+ status, ['model_based', 'down_estimator', 'expected_accel_y']);
expectedAccelY.setColor(GREEN);
expectedAccelY.setPointSize(0);
const expectedAccelZ = imuAccelPlot.addMessageLine(
- status, ['down_estimator', 'expected_accel_z']);
+ status, ['model_based', 'down_estimator', 'expected_accel_z']);
expectedAccelZ.setColor(BLUE);
expectedAccelZ.setPointSize(0);
@@ -150,7 +150,7 @@
zeroedLine.setColor(RED);
zeroedLine.setDrawLine(false);
const consecutiveStill =
- statePlot.addMessageLine(status, ['down_estimator', 'consecutive_still']);
+ statePlot.addMessageLine(status, ['model_based', 'down_estimator', 'consecutive_still']);
consecutiveStill.setColor(BLUE);
consecutiveStill.setPointSize(0);
const faultedLine =
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index b5990c3..9150e8a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -40,6 +40,15 @@
IMU_Z = 3, // Use the z-axis of the IMU.
};
+struct DownEstimatorConfig {
+ // Threshold, in g's, to use for detecting whether we are stopped in the down
+ // estimator.
+ double gravity_threshold = 0.025;
+ // Number of cycles of being still to require before taking accelerometer
+ // corrections.
+ int do_accel_corrections = 50;
+};
+
template <typename Scalar = double>
struct DrivetrainConfig {
// Shifting method we are using.
@@ -107,6 +116,8 @@
// True if we are running a simulated drivetrain.
bool is_simulated = false;
+ DownEstimatorConfig down_estimator_config{};
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 2c129f7..449c70d 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -88,7 +88,8 @@
// If the only obvious acceleration is that due to gravity, then accept the
// measurement.
- const double kUseAccelThreshold = assume_perfect_gravity_ ? 1e-10 : 0.025;
+ const double kUseAccelThreshold =
+ assume_perfect_gravity_ ? 1e-10 : config_.gravity_threshold;
const double accel_norm = measurement.norm();
if (std::abs(accel_norm - 1.0) > kUseAccelThreshold) {
P_ = P_prior;
@@ -99,12 +100,12 @@
// velocity. Because we just use this for debugging, only set it once per time
// duration when we are paused--this lets us observe how far things drift
// while sitting still.
- if (consecutive_still_ == 1000) {
+ if (consecutive_still_ == 2000) {
pos_vel_.block<3, 1>(3, 0).setZero();
}
// Don't do accelerometer updates unless we have been roughly still for a
// decent number of iterations.
- if (++consecutive_still_ < 50) {
+ if (++consecutive_still_ < config_.do_accel_corrections) {
return;
}
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index ddbf0bb..b4b0b76 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -57,9 +57,9 @@
// Measurements to use for correcting the estimated system state. These
// correspond to (x, y, z) measurements from the accelerometer.
constexpr static int kNumMeasurements = 3;
- QuaternionUkf(const Eigen::Matrix<double, 3, 3> &imu_transform =
- Eigen::Matrix<double, 3, 3>::Identity())
- : imu_transform_(imu_transform) {
+ QuaternionUkf(const DrivetrainConfig<double> &dt_config)
+ : imu_transform_(dt_config.imu_transform),
+ config_(dt_config.down_estimator_config) {
Reset();
}
@@ -193,6 +193,8 @@
// The transformation from the IMU's frame to the robot frame.
Eigen::Matrix<double, 3, 3> imu_transform_;
+ const DownEstimatorConfig config_;
+
bool assume_perfect_gravity_ = false;
};
@@ -204,7 +206,7 @@
class DrivetrainUkf : public QuaternionUkf {
public:
DrivetrainUkf(const DrivetrainConfig<double> &dt_config)
- : QuaternionUkf(dt_config.imu_transform) {}
+ : QuaternionUkf(dt_config) {}
// UKF for http://kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf
// Reference in case the link is dead:
// Kraft, Edgar. "A quaternion-based unscented Kalman filter for orientation
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 3312030..af2f3b2 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -119,7 +119,7 @@
EXPECT_EQ(0.0,
(Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs()))
.norm());
- for (int ii = 0; ii < 200; ++ii) {
+ for (int ii = 0; ii < 2000; ++ii) {
dtukf.Predict({0.0, 0.0, 0.0}, measurement,
frc971::controls::kLoopFrequency);
}
diff --git a/frc971/wpilib/imu_plotter.ts b/frc971/wpilib/imu_plotter.ts
index 0c735eb..6768e1c 100644
--- a/frc971/wpilib/imu_plotter.ts
+++ b/frc971/wpilib/imu_plotter.ts
@@ -19,7 +19,7 @@
'/drivetrain', 'frc971.control_loops.drivetrain.Status');
const imu = aosPlotter.addRawMessageSource(
- '/drivetrain', 'frc971.IMUValuesBatch',
+ '/localizer', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
const accelX = accelPlot.addMessageLine(imu, ['accelerometer_x']);
diff --git a/y2022/control_loops/drivetrain/drivetrain_base.cc b/y2022/control_loops/drivetrain/drivetrain_base.cc
index 93233d2..1a76afc 100644
--- a/y2022/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_base.cc
@@ -10,6 +10,7 @@
#include "y2022/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::DownEstimatorConfig;
namespace chrono = ::std::chrono;
@@ -22,6 +23,8 @@
const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
const DrivetrainConfig<double> &GetDrivetrainConfig() {
+ // Yaw of the IMU relative to the robot frame.
+ static constexpr double kImuYaw = -M_PI_2 + 0.15;
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
@@ -53,7 +56,12 @@
1.2 /* quickturn_wheel_multiplier */,
1.2 /* wheel_multiplier */,
true /*pistol_grip_shift_enables_line_follow*/,
- };
+ (Eigen::Matrix<double, 3, 3>() << std::cos(kImuYaw), -std::sin(kImuYaw),
+ 0.0, std::sin(kImuYaw), std::cos(kImuYaw), 0.0, 0.0, 0.0, 1.0)
+ .finished(),
+ false /*is_simulated*/,
+ DownEstimatorConfig{.gravity_threshold = 0.015,
+ .do_accel_corrections = 1000}};
return kDrivetrainConfig;
};