James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 1 | #ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_ |
| 2 | #define FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_ |
| 3 | |
| 4 | #include <chrono> |
| 5 | |
James Kuszmaul | 651fc3f | 2019-05-15 21:14:25 -0700 | [diff] [blame] | 6 | #include "Eigen/Dense" |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 7 | #include "aos/commonmath.h" |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 8 | #include "aos/containers/priority_queue.h" |
James Kuszmaul | fedc461 | 2019-03-10 11:24:51 -0700 | [diff] [blame] | 9 | #include "aos/util/math.h" |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 10 | #include "frc971/control_loops/c2d.h" |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 11 | #include "frc971/control_loops/drivetrain/drivetrain_config.h" |
James Kuszmaul | 651fc3f | 2019-05-15 21:14:25 -0700 | [diff] [blame] | 12 | #include "frc971/control_loops/runge_kutta.h" |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 13 | |
James Kuszmaul | 1057ce8 | 2019-02-09 17:58:24 -0800 | [diff] [blame] | 14 | namespace y2019 { |
| 15 | namespace control_loops { |
| 16 | namespace testing { |
| 17 | class ParameterizedLocalizerTest; |
| 18 | } // namespace testing |
| 19 | } // namespace control_loops |
| 20 | } // namespace y2019 |
| 21 | |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 22 | namespace frc971 { |
| 23 | namespace control_loops { |
| 24 | namespace drivetrain { |
| 25 | |
| 26 | namespace testing { |
| 27 | class HybridEkfTest; |
| 28 | } |
| 29 | |
| 30 | // HybridEkf is an EKF for use in robot localization. It is currently |
| 31 | // coded for use with drivetrains in particular, and so the states and inputs |
| 32 | // are chosen as such. |
| 33 | // The "Hybrid" part of the name refers to the fact that it can take in |
| 34 | // measurements with variable time-steps. |
| 35 | // measurements can also have been taken in the past and we maintain a buffer |
| 36 | // so that we can replay the kalman filter whenever we get an old measurement. |
| 37 | // Currently, this class provides the necessary utilities for doing |
| 38 | // measurement updates with an encoder/gyro as well as a more generic |
| 39 | // update function that can be used for arbitrary nonlinear updates (presumably |
| 40 | // a camera update). |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 41 | // |
| 42 | // Discussion of the model: |
| 43 | // In the current model, we try to rely primarily on IMU measurements for |
| 44 | // estimating robot state--we also need additional information (some combination |
| 45 | // of output voltages, encoders, and camera data) to help eliminate the biases |
| 46 | // that can accumulate due to integration of IMU data. |
| 47 | // We use IMU measurements as inputs rather than measurement outputs because |
| 48 | // that seemed to be easier to implement. I tried initially running with |
| 49 | // the IMU as a measurement, but it seemed to blow up the complexity of the |
| 50 | // model. |
| 51 | // |
| 52 | // On each prediction update, we take in inputs of the left/right voltages and |
| 53 | // the current measured longitudinal/lateral accelerations. In the current |
| 54 | // setup, the accelerometer readings will be used for estimating how the |
| 55 | // evolution of the longitudinal/lateral velocities. The voltages (and voltage |
| 56 | // errors) will solely be used for estimating the current rotational velocity of |
| 57 | // the robot (I do this because currently I suspect that the accelerometer is a |
| 58 | // much better indicator of current robot state than the voltages). We also |
| 59 | // deliberately decay all of the velocity estimates towards zero to help address |
| 60 | // potential accelerometer biases. We use two separate decay models: |
| 61 | // -The longitudinal velocity is modelled as decaying at a constant rate (see |
| 62 | // the documentation on the VelocityAccel() method)--this needs a more |
| 63 | // complex model because the robot will, under normal circumstances, be |
| 64 | // travelling at non-zero velocities. |
| 65 | // -The lateral velocity is modelled as exponentially decaying towards zero. |
| 66 | // This is simpler to model and should be reasonably valid, since we will |
| 67 | // not *normally* be travelling sideways consistently (this assumption may |
| 68 | // need to be revisited). |
| 69 | // -The "longitudinal velocity offset" (described below) also uses an |
| 70 | // exponential decay, albeit with a different time constant. A future |
| 71 | // improvement may remove the decay modelling on the longitudinal velocity |
| 72 | // itself and instead use that decay model on the longitudinal velocity offset. |
| 73 | // This would place a bit more trust in the encoder measurements but also |
| 74 | // more correctly model situations where the robot is legitimately moving at |
| 75 | // a certain velocity. |
| 76 | // |
| 77 | // For modelling how the drivetrain encoders evolve, and to help prevent the |
| 78 | // aforementioned decay functions from affecting legitimate high-velocity |
| 79 | // maneuvers too much, we have a "longitudinal velocity offset" term. This term |
| 80 | // models the difference between the actual longitudinal velocity of the robot |
| 81 | // (estimated by the average of the left/right velocities) and the velocity |
| 82 | // experienced by the wheels (which can be observed from the encoders more |
| 83 | // directly). Because we model this velocity offset as decaying towards zero, |
| 84 | // what this will do is allow the encoders to be a constant velocity off from |
| 85 | // the accelerometer updates for short periods of time but then gradually |
| 86 | // pull the "actual" longitudinal velocity offset towards that of the encoders, |
| 87 | // helping to reduce constant biases. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 88 | template <typename Scalar = double> |
| 89 | class HybridEkf { |
| 90 | public: |
| 91 | // An enum specifying what each index in the state vector is for. |
| 92 | enum StateIdx { |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 93 | // Current X/Y position, in meters, of the robot. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 94 | kX = 0, |
| 95 | kY = 1, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 96 | // Current heading of the robot. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 97 | kTheta = 2, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 98 | // Current estimated encoder reading of the left wheels, in meters. |
| 99 | // Rezeroed once on startup. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 100 | kLeftEncoder = 3, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 101 | // Current estimated actual velocity of the left side of the robot, in m/s. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 102 | kLeftVelocity = 4, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 103 | // Same variables, for the right side of the robot. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 104 | kRightEncoder = 5, |
| 105 | kRightVelocity = 6, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 106 | // Estimated offset to input voltage. Used as a generic error term, Volts. |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 107 | kLeftVoltageError = 7, |
James Kuszmaul | 651fc3f | 2019-05-15 21:14:25 -0700 | [diff] [blame] | 108 | kRightVoltageError = 8, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 109 | // These error terms are used to estimate the difference between the actual |
| 110 | // movement of the drivetrain and that implied by the wheel odometry. |
| 111 | // Angular error effectively estimates a constant angular rate offset of the |
| 112 | // encoders relative to the actual rotation of the robot. |
| 113 | // Semi-arbitrary units (we don't bother accounting for robot radius in |
| 114 | // this). |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 115 | kAngularError = 9, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 116 | // Estimate of slip between the drivetrain wheels and the actual |
| 117 | // forwards/backwards velocity of the robot, in m/s. |
| 118 | // I.e., (left velocity + right velocity) / 2.0 = (left wheel velocity + |
| 119 | // right wheel velocity) / 2.0 + longitudinal velocity offset |
| 120 | kLongitudinalVelocityOffset = 10, |
| 121 | // Current estimate of the lateral velocity of the robot, in m/s. |
| 122 | // Positive implies the robot is moving to its left. |
| 123 | kLateralVelocity = 11, |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 124 | }; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 125 | static constexpr int kNStates = 12; |
| 126 | enum InputIdx { |
| 127 | // Left/right drivetrain voltages. |
| 128 | kLeftVoltage = 0, |
| 129 | kRightVoltage = 1, |
| 130 | // Current accelerometer readings, in m/s/s, along the longitudinal and |
| 131 | // lateral axes of the robot. Should be projected onto the X/Y plane, to |
| 132 | // compensate for tilt of the robot before being passed to this filter. The |
| 133 | // HybridEkf has no knowledge of the current pitch/roll of the robot, and so |
| 134 | // can't do anything to compensate for it. |
| 135 | kLongitudinalAccel = 2, |
| 136 | kLateralAccel = 3, |
| 137 | }; |
| 138 | static constexpr int kNInputs = 4; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 139 | // Number of previous samples to save. |
| 140 | static constexpr int kSaveSamples = 50; |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 141 | // Whether we should completely rerun the entire stored history of |
| 142 | // kSaveSamples on every correction. Enabling this will increase overall CPU |
| 143 | // usage substantially; however, leaving it disabled makes it so that we are |
| 144 | // less likely to notice if processing camera frames is causing delays in the |
| 145 | // drivetrain. |
| 146 | // If we are having CPU issues, we have three easy avenues to improve things: |
| 147 | // (1) Reduce kSaveSamples (e.g., if all camera frames arive within |
| 148 | // 100 ms, then we can reduce kSaveSamples to be 25 (125 ms of samples)). |
| 149 | // (2) Don't actually rely on the ability to insert corrections into the |
| 150 | // timeline. |
| 151 | // (3) Set this to false. |
| 152 | static constexpr bool kFullRewindOnEverySample = false; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 153 | // Assume that all correction steps will have kNOutputs |
| 154 | // dimensions. |
| 155 | // TODO(james): Relax this assumption; relaxing it requires |
| 156 | // figuring out how to deal with storing variable size |
| 157 | // observation matrices, though. |
| 158 | static constexpr int kNOutputs = 3; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 159 | // Time constant to use for estimating how the longitudinal/lateral velocity |
| 160 | // offsets decay, in seconds. |
James Kuszmaul | 5f6d1d4 | 2020-03-01 18:10:07 -0800 | [diff] [blame] | 161 | static constexpr double kVelocityOffsetTimeConstant = 1.0; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 162 | static constexpr double kLateralVelocityTimeConstant = 1.0; |
James Kuszmaul | 91aa0cf | 2021-02-13 13:15:06 -0800 | [diff] [blame^] | 163 | |
James Kuszmaul | f395036 | 2020-10-11 18:29:15 -0700 | [diff] [blame] | 164 | // The maximum allowable timestep--we use this to check for situations where |
| 165 | // measurement updates come in too infrequently and this might cause the |
| 166 | // integrator and discretization in the prediction step to be overly |
| 167 | // aggressive. |
| 168 | static constexpr std::chrono::milliseconds kMaxTimestep{20}; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 169 | // Inputs are [left_volts, right_volts] |
| 170 | typedef Eigen::Matrix<Scalar, kNInputs, 1> Input; |
| 171 | // Outputs are either: |
| 172 | // [left_encoder, right_encoder, gyro_vel]; or [heading, distance, skew] to |
| 173 | // some target. This makes it so we don't have to figure out how we store |
| 174 | // variable-size measurement updates. |
| 175 | typedef Eigen::Matrix<Scalar, kNOutputs, 1> Output; |
| 176 | typedef Eigen::Matrix<Scalar, kNStates, kNStates> StateSquare; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 177 | // State contains the states defined by the StateIdx enum. See comments there. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 178 | typedef Eigen::Matrix<Scalar, kNStates, 1> State; |
| 179 | |
| 180 | // Constructs a HybridEkf for a particular drivetrain. |
| 181 | // Currently, we use the drivetrain config for modelling constants |
| 182 | // (continuous time A and B matrices) and for the noise matrices for the |
| 183 | // encoders/gyro. |
James Kuszmaul | d478f87 | 2020-03-16 20:54:27 -0700 | [diff] [blame] | 184 | HybridEkf(const DrivetrainConfig<double> &dt_config) |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 185 | : dt_config_(dt_config), |
| 186 | velocity_drivetrain_coefficients_( |
| 187 | dt_config.make_hybrid_drivetrain_velocity_loop() |
| 188 | .plant() |
| 189 | .coefficients()) { |
| 190 | InitializeMatrices(); |
| 191 | } |
| 192 | |
| 193 | // Set the initial guess of the state. Can only be called once, and before |
| 194 | // any measurement updates have occured. |
| 195 | // TODO(james): We may want to actually re-initialize and reset things on |
| 196 | // the field. Create some sort of Reset() function. |
| 197 | void ResetInitialState(::aos::monotonic_clock::time_point t, |
James Kuszmaul | 1057ce8 | 2019-02-09 17:58:24 -0800 | [diff] [blame] | 198 | const State &state, const StateSquare &P) { |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 199 | observations_.clear(); |
| 200 | X_hat_ = state; |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 201 | have_zeroed_encoders_ = true; |
James Kuszmaul | 1057ce8 | 2019-02-09 17:58:24 -0800 | [diff] [blame] | 202 | P_ = P; |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 203 | observations_.PushFromBottom({ |
| 204 | t, |
| 205 | t, |
| 206 | X_hat_, |
| 207 | P_, |
| 208 | Input::Zero(), |
| 209 | Output::Zero(), |
| 210 | {}, |
| 211 | [](const State &, const Input &) { return Output::Zero(); }, |
| 212 | [](const State &) { |
| 213 | return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero(); |
| 214 | }, |
| 215 | Eigen::Matrix<Scalar, kNOutputs, kNOutputs>::Identity(), |
| 216 | StateSquare::Identity(), |
| 217 | StateSquare::Zero(), |
| 218 | std::chrono::seconds(0), |
| 219 | State::Zero(), |
| 220 | }); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 221 | } |
| 222 | |
| 223 | // Correct with: |
| 224 | // A measurement z at time t with z = h(X_hat, U) + v where v has noise |
| 225 | // covariance R. |
| 226 | // Input U is applied from the previous timestep until time t. |
| 227 | // If t is later than any previous measurements, then U must be provided. |
| 228 | // If the measurement falls between two previous measurements, then U |
| 229 | // can be provided or not; if U is not provided, then it is filled in based |
| 230 | // on an assumption that the voltage was held constant between the time steps. |
| 231 | // TODO(james): Is it necessary to explicitly to provide a version with H as a |
| 232 | // matrix for linear cases? |
| 233 | void Correct( |
| 234 | const Output &z, const Input *U, |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 235 | std::function<void(const State &, const StateSquare &, |
| 236 | std::function<Output(const State &, const Input &)> *, |
| 237 | std::function<Eigen::Matrix< |
| 238 | Scalar, kNOutputs, kNStates>(const State &)> *)> |
| 239 | make_h, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 240 | std::function<Output(const State &, const Input &)> h, |
| 241 | std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 242 | dhdx, |
| 243 | const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R, |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 244 | aos::monotonic_clock::time_point t); |
| 245 | |
| 246 | // A utility function for specifically updating with encoder and gyro |
| 247 | // measurements. |
| 248 | void UpdateEncodersAndGyro(const Scalar left_encoder, |
| 249 | const Scalar right_encoder, const Scalar gyro_rate, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 250 | const Eigen::Matrix<Scalar, 2, 1> &voltage, |
| 251 | const Eigen::Matrix<Scalar, 3, 1> &accel, |
| 252 | aos::monotonic_clock::time_point t) { |
| 253 | Input U; |
| 254 | U.template block<2, 1>(0, 0) = voltage; |
| 255 | U.template block<2, 1>(kLongitudinalAccel, 0) = |
| 256 | accel.template block<2, 1>(0, 0); |
| 257 | RawUpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, t); |
| 258 | } |
| 259 | // Version of UpdateEncodersAndGyro that takes a input matrix rather than |
| 260 | // taking in a voltage/acceleration separately. |
| 261 | void RawUpdateEncodersAndGyro(const Scalar left_encoder, |
| 262 | const Scalar right_encoder, |
| 263 | const Scalar gyro_rate, const Input &U, |
| 264 | aos::monotonic_clock::time_point t) { |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 265 | // Because the check below for have_zeroed_encoders_ will add an |
| 266 | // Observation, do a check here to ensure that initialization has been |
| 267 | // performed and so there is at least one observation. |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 268 | CHECK(!observations_.empty()); |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 269 | if (!have_zeroed_encoders_) { |
| 270 | // This logic handles ensuring that on the first encoder reading, we |
| 271 | // update the internal state for the encoders to match the reading. |
| 272 | // Otherwise, if we restart the drivetrain without restarting |
| 273 | // wpilib_interface, then we can get some obnoxious initial corrections |
| 274 | // that mess up the localization. |
| 275 | State newstate = X_hat_; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 276 | newstate(kLeftEncoder) = left_encoder; |
| 277 | newstate(kRightEncoder) = right_encoder; |
| 278 | newstate(kLeftVoltageError) = 0.0; |
| 279 | newstate(kRightVoltageError) = 0.0; |
| 280 | newstate(kAngularError) = 0.0; |
| 281 | newstate(kLongitudinalVelocityOffset) = 0.0; |
| 282 | newstate(kLateralVelocity) = 0.0; |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 283 | ResetInitialState(t, newstate, P_); |
| 284 | // We need to set have_zeroed_encoders_ after ResetInitialPosition because |
| 285 | // the reset clears have_zeroed_encoders_... |
| 286 | have_zeroed_encoders_ = true; |
| 287 | } |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 288 | |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 289 | Output z(left_encoder, right_encoder, gyro_rate); |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 290 | |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 291 | Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R; |
| 292 | R.setZero(); |
| 293 | R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_; |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 294 | Correct(z, &U, {}, |
| 295 | [this](const State &X, const Input &) { |
| 296 | return H_encoders_and_gyro_ * X; |
| 297 | }, |
| 298 | [this](const State &) { return H_encoders_and_gyro_; }, R, t); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 299 | } |
| 300 | |
| 301 | // Sundry accessor: |
| 302 | State X_hat() const { return X_hat_; } |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 303 | Scalar X_hat(long i) const { return X_hat_(i); } |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 304 | StateSquare P() const { return P_; } |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 305 | aos::monotonic_clock::time_point latest_t() const { |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 306 | return observations_.top().t; |
| 307 | } |
| 308 | |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 309 | static Scalar CalcLongitudinalVelocity(const State &X) { |
| 310 | return (X(kLeftVelocity) + X(kRightVelocity)) / 2.0; |
| 311 | } |
| 312 | |
| 313 | Scalar CalcYawRate(const State &X) const { |
| 314 | return (X(kRightVelocity) - X(kLeftVelocity)) / 2.0 / |
| 315 | dt_config_.robot_radius; |
| 316 | } |
| 317 | |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 318 | // Returns the last state before the specified time. |
| 319 | // Returns nullopt if time is older than the oldest measurement. |
| 320 | std::optional<State> LastStateBeforeTime( |
| 321 | aos::monotonic_clock::time_point time) { |
| 322 | if (observations_.empty() || observations_.begin()->t > time) { |
| 323 | return std::nullopt; |
| 324 | } |
| 325 | for (const auto &observation : observations_) { |
| 326 | if (observation.t > time) { |
| 327 | // Note that observation.X_hat actually references the _previous_ X_hat. |
| 328 | return observation.X_hat; |
| 329 | } |
| 330 | } |
| 331 | return X_hat(); |
| 332 | } |
| 333 | |
| 334 | // Returns the most recent input vector. |
| 335 | Input MostRecentInput() { |
| 336 | CHECK(!observations_.empty()); |
| 337 | Input U = observations_.top().U; |
| 338 | return U; |
| 339 | } |
| 340 | |
James Kuszmaul | 91aa0cf | 2021-02-13 13:15:06 -0800 | [diff] [blame^] | 341 | void set_ignore_accel(bool ignore_accel) { ignore_accel_ = ignore_accel; } |
| 342 | |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 343 | private: |
| 344 | struct Observation { |
| 345 | // Time when the observation was taken. |
| 346 | aos::monotonic_clock::time_point t; |
| 347 | // Time that the previous observation was taken: |
| 348 | aos::monotonic_clock::time_point prev_t; |
| 349 | // Estimate of state at previous observation time t, after accounting for |
| 350 | // the previous observation. |
| 351 | State X_hat; |
| 352 | // Noise matrix corresponding to X_hat_. |
| 353 | StateSquare P; |
| 354 | // The input applied from previous observation until time t. |
| 355 | Input U; |
| 356 | // Measurement taken at that time. |
| 357 | Output z; |
| 358 | // A function to create h and dhdx from a given position/covariance |
| 359 | // estimate. This is used by the camera to make it so that we only have to |
| 360 | // match targets once. |
| 361 | // Only called if h and dhdx are empty. |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 362 | std::function<void(const State &, const StateSquare &, |
| 363 | std::function<Output(const State &, const Input &)> *, |
| 364 | std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>( |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 365 | const State &)> *)> |
| 366 | make_h; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 367 | // A function to calculate the expected output at a given state/input. |
| 368 | // TODO(james): For encoders/gyro, it is linear and the function call may |
| 369 | // be expensive. Potential source of optimization. |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 370 | std::function<Output(const State &, const Input &)> h; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 371 | // The Jacobian of h with respect to x. |
| 372 | // We assume that U has no impact on the Jacobian. |
| 373 | // TODO(james): Currently, none of the users of this actually make use of |
| 374 | // the ability to have dynamic dhdx (technically, the camera code should |
| 375 | // recalculate it to be strictly correct, but I was both too lazy to do |
| 376 | // so and it seemed unnecessary). This is a potential source for future |
| 377 | // optimizations if function calls are being expensive. |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 378 | std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> |
James Kuszmaul | 651fc3f | 2019-05-15 21:14:25 -0700 | [diff] [blame] | 379 | dhdx; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 380 | // The measurement noise matrix. |
| 381 | Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R; |
| 382 | |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 383 | // Discretized A and Q to use on this update step. These will only be |
| 384 | // recalculated if the timestep changes. |
| 385 | StateSquare A_d; |
| 386 | StateSquare Q_d; |
| 387 | aos::monotonic_clock::duration discretization_time; |
| 388 | |
| 389 | // A cached value indicating how much we change X_hat in the prediction step |
| 390 | // of this Observation. |
| 391 | State predict_update; |
| 392 | |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 393 | // In order to sort the observations in the PriorityQueue object, we |
| 394 | // need a comparison function. |
James Kuszmaul | 651fc3f | 2019-05-15 21:14:25 -0700 | [diff] [blame] | 395 | friend bool operator<(const Observation &l, const Observation &r) { |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 396 | return l.t < r.t; |
| 397 | } |
| 398 | }; |
| 399 | |
| 400 | void InitializeMatrices(); |
| 401 | |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 402 | // These constants and functions define how the longitudinal velocity |
| 403 | // (the average of the left and right velocities) decays. We model it as |
| 404 | // decaying at a constant rate, except very near zero where the decay rate is |
| 405 | // exponential (this is more numerically stable than just using a constant |
| 406 | // rate the whole time). We use this model rather than a simpler exponential |
| 407 | // decay because an exponential decay will result in the robot's velocity |
| 408 | // estimate consistently being far too low when at high velocities, and since |
| 409 | // the acceleromater-based estimate of the velocity will only drift at a |
| 410 | // relatively slow rate and doesn't get worse at higher velocities, we can |
| 411 | // safely decay pretty slowly. |
| 412 | static constexpr double kMaxVelocityAccel = 0.005; |
| 413 | static constexpr double kMaxVelocityGain = 1.0; |
| 414 | static Scalar VelocityAccel(Scalar velocity) { |
| 415 | return -std::clamp(kMaxVelocityGain * velocity, -kMaxVelocityAccel, |
| 416 | kMaxVelocityAccel); |
| 417 | } |
| 418 | |
| 419 | static Scalar VelocityAccelDiff(Scalar velocity) { |
| 420 | return (std::abs(kMaxVelocityGain * velocity) > kMaxVelocityAccel) |
| 421 | ? 0.0 |
| 422 | : -kMaxVelocityGain; |
| 423 | } |
| 424 | |
| 425 | // Returns the "A" matrix for a given state. See DiffEq for discussion of |
| 426 | // ignore_accel. |
James Kuszmaul | 91aa0cf | 2021-02-13 13:15:06 -0800 | [diff] [blame^] | 427 | StateSquare AForState(const State &X, bool ignore_accel) const { |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 428 | // Calculate the A matrix for a given state. Note that A = partial Xdot / |
| 429 | // partial X. This is distinct from saying that Xdot = A * X. This is |
| 430 | // particularly relevant for the (kX, kTheta) members which otherwise seem |
| 431 | // odd. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 432 | StateSquare A_continuous = A_continuous_; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 433 | const Scalar theta = X(kTheta); |
| 434 | const Scalar stheta = std::sin(theta); |
| 435 | const Scalar ctheta = std::cos(theta); |
| 436 | const Scalar lng_vel = CalcLongitudinalVelocity(X); |
| 437 | const Scalar lat_vel = X(kLateralVelocity); |
| 438 | const Scalar diameter = 2.0 * dt_config_.robot_radius; |
| 439 | const Scalar yaw_rate = CalcYawRate(X); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 440 | // X and Y derivatives |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 441 | A_continuous(kX, kTheta) = -stheta * lng_vel - ctheta * lat_vel; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 442 | A_continuous(kX, kLeftVelocity) = ctheta / 2.0; |
| 443 | A_continuous(kX, kRightVelocity) = ctheta / 2.0; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 444 | A_continuous(kX, kLateralVelocity) = -stheta; |
| 445 | A_continuous(kY, kTheta) = ctheta * lng_vel - stheta * lat_vel; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 446 | A_continuous(kY, kLeftVelocity) = stheta / 2.0; |
| 447 | A_continuous(kY, kRightVelocity) = stheta / 2.0; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 448 | A_continuous(kY, kLateralVelocity) = ctheta; |
| 449 | |
| 450 | if (!ignore_accel) { |
| 451 | const Eigen::Matrix<Scalar, 1, kNStates> lng_vel_row = |
| 452 | (A_continuous.row(kLeftVelocity) + A_continuous.row(kRightVelocity)) / |
| 453 | 2.0; |
| 454 | A_continuous.row(kLeftVelocity) -= lng_vel_row; |
| 455 | A_continuous.row(kRightVelocity) -= lng_vel_row; |
| 456 | // Terms to account for centripetal accelerations. |
| 457 | // lateral centripetal accel = -yaw_rate * lng_vel |
| 458 | A_continuous(kLateralVelocity, kLeftVelocity) += |
| 459 | X(kLeftVelocity) / diameter; |
| 460 | A_continuous(kLateralVelocity, kRightVelocity) += |
| 461 | -X(kRightVelocity) / diameter; |
| 462 | A_continuous(kRightVelocity, kLateralVelocity) += yaw_rate; |
| 463 | A_continuous(kLeftVelocity, kLateralVelocity) += yaw_rate; |
| 464 | const Scalar dlng_accel_dwheel_vel = X(kLateralVelocity) / diameter; |
| 465 | A_continuous(kRightVelocity, kRightVelocity) += dlng_accel_dwheel_vel; |
| 466 | A_continuous(kLeftVelocity, kRightVelocity) += dlng_accel_dwheel_vel; |
| 467 | A_continuous(kRightVelocity, kLeftVelocity) += -dlng_accel_dwheel_vel; |
| 468 | A_continuous(kLeftVelocity, kLeftVelocity) += -dlng_accel_dwheel_vel; |
| 469 | |
| 470 | A_continuous(kRightVelocity, kRightVelocity) += |
| 471 | VelocityAccelDiff(lng_vel) / 2.0; |
| 472 | A_continuous(kRightVelocity, kLeftVelocity) += |
| 473 | VelocityAccelDiff(lng_vel) / 2.0; |
| 474 | A_continuous(kLeftVelocity, kRightVelocity) += |
| 475 | VelocityAccelDiff(lng_vel) / 2.0; |
| 476 | A_continuous(kLeftVelocity, kLeftVelocity) += |
| 477 | VelocityAccelDiff(lng_vel) / 2.0; |
| 478 | } |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 479 | return A_continuous; |
| 480 | } |
| 481 | |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 482 | // Returns dX / dt given X and U. If ignore_accel is set, then we ignore the |
| 483 | // accelerometer-based components of U (this is solely used in testing). |
James Kuszmaul | 91aa0cf | 2021-02-13 13:15:06 -0800 | [diff] [blame^] | 484 | State DiffEq(const State &X, const Input &U, bool ignore_accel) const { |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 485 | State Xdot = A_continuous_ * X + B_continuous_ * U; |
| 486 | // And then we need to add on the terms for the x/y change: |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 487 | const Scalar theta = X(kTheta); |
| 488 | const Scalar lng_vel = CalcLongitudinalVelocity(X); |
| 489 | const Scalar lat_vel = X(kLateralVelocity); |
| 490 | const Scalar stheta = std::sin(theta); |
| 491 | const Scalar ctheta = std::cos(theta); |
| 492 | Xdot(kX) = ctheta * lng_vel - stheta * lat_vel; |
| 493 | Xdot(kY) = stheta * lng_vel + ctheta * lat_vel; |
| 494 | |
| 495 | const Scalar yaw_rate = CalcYawRate(X); |
| 496 | const Scalar expected_lat_accel = lng_vel * yaw_rate; |
| 497 | const Scalar expected_lng_accel = |
| 498 | CalcLongitudinalVelocity(Xdot) - yaw_rate * lat_vel; |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 499 | const Scalar lng_accel_offset = U(kLongitudinalAccel) - expected_lng_accel; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 500 | constexpr double kAccelWeight = 1.0; |
| 501 | if (!ignore_accel) { |
| 502 | Xdot(kLeftVelocity) += kAccelWeight * lng_accel_offset; |
| 503 | Xdot(kRightVelocity) += kAccelWeight * lng_accel_offset; |
| 504 | Xdot(kLateralVelocity) += U(kLateralAccel) - expected_lat_accel; |
| 505 | |
| 506 | Xdot(kRightVelocity) += VelocityAccel(lng_vel); |
| 507 | Xdot(kLeftVelocity) += VelocityAccel(lng_vel); |
| 508 | } |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 509 | return Xdot; |
| 510 | } |
| 511 | |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 512 | void PredictImpl(Observation *obs, std::chrono::nanoseconds dt, State *state, |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 513 | StateSquare *P) { |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 514 | // Only recalculate the discretization if the timestep has changed. |
| 515 | // Technically, this isn't quite correct, since the discretization will |
| 516 | // change depending on the current state. However, the slight loss of |
| 517 | // precision seems acceptable for the sake of significantly reducing CPU |
| 518 | // usage. |
| 519 | if (obs->discretization_time != dt) { |
| 520 | // TODO(james): By far the biggest CPU sink in the localization appears to |
| 521 | // be this discretization--it's possible the spline code spikes higher, |
| 522 | // but it doesn't create anywhere near the same sustained load. There |
| 523 | // are a few potential options for optimizing this code, but none of |
| 524 | // them are entirely trivial, e.g. we could: |
| 525 | // -Reduce the number of states (this function grows at O(kNStates^3)) |
| 526 | // -Adjust the discretization function itself (there're a few things we |
| 527 | // can tune there). |
| 528 | // -Try to come up with some sort of lookup table or other way of |
| 529 | // pre-calculating A_d and Q_d. |
| 530 | // I also have to figure out how much we care about the precision of |
| 531 | // some of these values--I don't think we care much, but we probably |
| 532 | // do want to maintain some of the structure of the matrices. |
James Kuszmaul | 91aa0cf | 2021-02-13 13:15:06 -0800 | [diff] [blame^] | 533 | const StateSquare A_c = AForState(*state, ignore_accel_); |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 534 | controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &obs->Q_d, &obs->A_d); |
| 535 | obs->discretization_time = dt; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 536 | |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 537 | obs->predict_update = |
| 538 | RungeKuttaU( |
James Kuszmaul | 91aa0cf | 2021-02-13 13:15:06 -0800 | [diff] [blame^] | 539 | [this](const State &X, const Input &U) { |
| 540 | return DiffEq(X, U, ignore_accel_); |
| 541 | }, |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 542 | *state, obs->U, aos::time::DurationInSeconds(dt)) - |
| 543 | *state; |
| 544 | } |
James Kuszmaul | b2a2f35 | 2019-03-02 16:59:34 -0800 | [diff] [blame] | 545 | |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 546 | *state += obs->predict_update; |
| 547 | |
| 548 | StateSquare Ptemp = obs->A_d * *P * obs->A_d.transpose() + obs->Q_d; |
James Kuszmaul | b2a2f35 | 2019-03-02 16:59:34 -0800 | [diff] [blame] | 549 | *P = Ptemp; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 550 | } |
| 551 | |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 552 | void CorrectImpl(Observation *obs, State *state, StateSquare *P) { |
| 553 | const Eigen::Matrix<Scalar, kNOutputs, kNStates> H = obs->dhdx(*state); |
| 554 | // Note: Technically, this does calculate P * H.transpose() twice. However, |
| 555 | // when I was mucking around with some things, I found that in practice |
| 556 | // putting everything into one expression and letting Eigen optimize it |
| 557 | // directly actually improved performance relative to precalculating P * |
| 558 | // H.transpose(). |
| 559 | const Eigen::Matrix<Scalar, kNStates, kNOutputs> K = |
| 560 | *P * H.transpose() * (H * *P * H.transpose() + obs->R).inverse(); |
| 561 | const StateSquare Ptemp = (StateSquare::Identity() - K * H) * *P; |
James Kuszmaul | b2a2f35 | 2019-03-02 16:59:34 -0800 | [diff] [blame] | 562 | *P = Ptemp; |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 563 | *state += K * (obs->z - obs->h(*state, obs->U)); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 564 | } |
| 565 | |
| 566 | void ProcessObservation(Observation *obs, const std::chrono::nanoseconds dt, |
| 567 | State *state, StateSquare *P) { |
| 568 | *state = obs->X_hat; |
| 569 | *P = obs->P; |
James Kuszmaul | f395036 | 2020-10-11 18:29:15 -0700 | [diff] [blame] | 570 | if (dt.count() != 0 && dt < kMaxTimestep) { |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 571 | PredictImpl(obs, dt, state, P); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 572 | } |
| 573 | if (!(obs->h && obs->dhdx)) { |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 574 | CHECK(obs->make_h); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 575 | obs->make_h(*state, *P, &obs->h, &obs->dhdx); |
| 576 | } |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 577 | CorrectImpl(obs, state, P); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 578 | } |
| 579 | |
James Kuszmaul | d478f87 | 2020-03-16 20:54:27 -0700 | [diff] [blame] | 580 | DrivetrainConfig<double> dt_config_; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 581 | State X_hat_; |
James Kuszmaul | d478f87 | 2020-03-16 20:54:27 -0700 | [diff] [blame] | 582 | StateFeedbackHybridPlantCoefficients<2, 2, 2, double> |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 583 | velocity_drivetrain_coefficients_; |
| 584 | StateSquare A_continuous_; |
| 585 | StateSquare Q_continuous_; |
| 586 | StateSquare P_; |
| 587 | Eigen::Matrix<Scalar, kNOutputs, kNStates> H_encoders_and_gyro_; |
| 588 | Scalar encoder_noise_, gyro_noise_; |
| 589 | Eigen::Matrix<Scalar, kNStates, kNInputs> B_continuous_; |
| 590 | |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 591 | bool have_zeroed_encoders_ = false; |
| 592 | |
James Kuszmaul | 91aa0cf | 2021-02-13 13:15:06 -0800 | [diff] [blame^] | 593 | // Whether to pay attention to accelerometer readings to compensate for wheel |
| 594 | // slip. |
| 595 | bool ignore_accel_ = false; |
| 596 | |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 597 | aos::PriorityQueue<Observation, kSaveSamples, std::less<Observation>> |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 598 | observations_; |
| 599 | |
| 600 | friend class testing::HybridEkfTest; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 601 | friend class y2019::control_loops::testing::ParameterizedLocalizerTest; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 602 | }; // class HybridEkf |
| 603 | |
| 604 | template <typename Scalar> |
| 605 | void HybridEkf<Scalar>::Correct( |
| 606 | const Output &z, const Input *U, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 607 | std::function<void(const State &, const StateSquare &, |
| 608 | std::function<Output(const State &, const Input &)> *, |
| 609 | std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>( |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 610 | const State &)> *)> |
| 611 | make_h, |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 612 | std::function<Output(const State &, const Input &)> h, |
| 613 | std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 614 | dhdx, |
| 615 | const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R, |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 616 | aos::monotonic_clock::time_point t) { |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 617 | CHECK(!observations_.empty()); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 618 | if (!observations_.full() && t < observations_.begin()->t) { |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 619 | LOG(ERROR) << "Dropped an observation that was received before we " |
| 620 | "initialized.\n"; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 621 | return; |
| 622 | } |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 623 | auto cur_it = observations_.PushFromBottom( |
| 624 | {t, t, State::Zero(), StateSquare::Zero(), Input::Zero(), z, make_h, h, |
| 625 | dhdx, R, StateSquare::Identity(), StateSquare::Zero(), |
| 626 | std::chrono::seconds(0), State::Zero()}); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 627 | if (cur_it == observations_.end()) { |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 628 | VLOG(1) << "Camera dropped off of end with time of " |
| 629 | << aos::time::DurationInSeconds(t.time_since_epoch()) |
| 630 | << "s; earliest observation in " |
| 631 | "queue has time of " |
| 632 | << aos::time::DurationInSeconds( |
| 633 | observations_.begin()->t.time_since_epoch()) |
| 634 | << "s.\n"; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 635 | return; |
| 636 | } |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 637 | // Now we populate any state information that depends on where the |
| 638 | // observation was inserted into the queue. X_hat and P must be populated |
| 639 | // from the values present in the observation *following* this one in |
| 640 | // the queue (note that the X_hat and P that we store in each observation |
| 641 | // is the values that they held after accounting for the previous |
| 642 | // measurement and before accounting for the time between the previous and |
| 643 | // current measurement). If we appended to the end of the queue, then |
| 644 | // we need to pull from X_hat_ and P_ specifically. |
| 645 | // Furthermore, for U: |
| 646 | // -If the observation was inserted at the end, then the user must've |
| 647 | // provided U and we use it. |
| 648 | // -Otherwise, only grab U if necessary. |
| 649 | auto next_it = cur_it; |
| 650 | ++next_it; |
| 651 | if (next_it == observations_.end()) { |
| 652 | cur_it->X_hat = X_hat_; |
| 653 | cur_it->P = P_; |
| 654 | // Note that if next_it == observations_.end(), then because we already |
| 655 | // checked for !observations_.empty(), we are guaranteed to have |
| 656 | // valid prev_it. |
| 657 | auto prev_it = cur_it; |
| 658 | --prev_it; |
| 659 | cur_it->prev_t = prev_it->t; |
| 660 | // TODO(james): Figure out a saner way of handling this. |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 661 | CHECK(U != nullptr); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 662 | cur_it->U = *U; |
| 663 | } else { |
| 664 | cur_it->X_hat = next_it->X_hat; |
| 665 | cur_it->P = next_it->P; |
| 666 | cur_it->prev_t = next_it->prev_t; |
| 667 | next_it->prev_t = cur_it->t; |
| 668 | cur_it->U = (U == nullptr) ? next_it->U : *U; |
| 669 | } |
James Kuszmaul | 06257f4 | 2020-05-09 15:40:09 -0700 | [diff] [blame] | 670 | |
| 671 | if (kFullRewindOnEverySample) { |
| 672 | next_it = observations_.begin(); |
| 673 | cur_it = next_it++; |
| 674 | } |
| 675 | |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 676 | // Now we need to rerun the predict step from the previous to the new |
| 677 | // observation as well as every following correct/predict up to the current |
| 678 | // time. |
| 679 | while (true) { |
| 680 | // We use X_hat_ and P_ to store the intermediate states, and then |
| 681 | // once we reach the end they will all be up-to-date. |
| 682 | ProcessObservation(&*cur_it, cur_it->t - cur_it->prev_t, &X_hat_, &P_); |
James Kuszmaul | 891f4f1 | 2020-10-31 17:13:23 -0700 | [diff] [blame] | 683 | // TOOD(james): Note that this can be triggered when there are extremely |
| 684 | // small values in P_. This is particularly likely if Scalar is just float |
| 685 | // and we are performing zero-time updates where the predict step never |
| 686 | // runs. |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 687 | CHECK(X_hat_.allFinite()); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 688 | if (next_it != observations_.end()) { |
| 689 | next_it->X_hat = X_hat_; |
| 690 | next_it->P = P_; |
| 691 | } else { |
| 692 | break; |
| 693 | } |
| 694 | ++cur_it; |
| 695 | ++next_it; |
| 696 | } |
| 697 | } |
| 698 | |
| 699 | template <typename Scalar> |
| 700 | void HybridEkf<Scalar>::InitializeMatrices() { |
| 701 | A_continuous_.setZero(); |
| 702 | const Scalar diameter = 2.0 * dt_config_.robot_radius; |
| 703 | // Theta derivative |
| 704 | A_continuous_(kTheta, kLeftVelocity) = -1.0 / diameter; |
| 705 | A_continuous_(kTheta, kRightVelocity) = 1.0 / diameter; |
| 706 | |
| 707 | // Encoder derivatives |
| 708 | A_continuous_(kLeftEncoder, kLeftVelocity) = 1.0; |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 709 | A_continuous_(kLeftEncoder, kAngularError) = 1.0; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 710 | A_continuous_(kLeftEncoder, kLongitudinalVelocityOffset) = -1.0; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 711 | A_continuous_(kRightEncoder, kRightVelocity) = 1.0; |
James Kuszmaul | 074429e | 2019-03-23 16:01:49 -0700 | [diff] [blame] | 712 | A_continuous_(kRightEncoder, kAngularError) = -1.0; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 713 | A_continuous_(kRightEncoder, kLongitudinalVelocityOffset) = -1.0; |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 714 | |
| 715 | // Pull velocity derivatives from velocity matrices. |
| 716 | // Note that this looks really awkward (doesn't use |
| 717 | // Eigen blocks) because someone decided that the full |
James Kuszmaul | bcd96fc | 2020-10-12 20:29:32 -0700 | [diff] [blame] | 718 | // drivetrain Kalman Filter should have a weird convention. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 719 | // TODO(james): Support shifting drivetrains with changing A_continuous |
| 720 | const auto &vel_coefs = velocity_drivetrain_coefficients_; |
| 721 | A_continuous_(kLeftVelocity, kLeftVelocity) = vel_coefs.A_continuous(0, 0); |
| 722 | A_continuous_(kLeftVelocity, kRightVelocity) = vel_coefs.A_continuous(0, 1); |
| 723 | A_continuous_(kRightVelocity, kLeftVelocity) = vel_coefs.A_continuous(1, 0); |
| 724 | A_continuous_(kRightVelocity, kRightVelocity) = vel_coefs.A_continuous(1, 1); |
| 725 | |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 726 | A_continuous_(kLongitudinalVelocityOffset, kLongitudinalVelocityOffset) = |
| 727 | -1.0 / kVelocityOffsetTimeConstant; |
| 728 | A_continuous_(kLateralVelocity, kLateralVelocity) = |
| 729 | -1.0 / kLateralVelocityTimeConstant; |
| 730 | |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 731 | // TODO(james): Decide what to do about these terms. They don't really matter |
| 732 | // too much when we have accelerometer readings available. |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 733 | B_continuous_.setZero(); |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 734 | B_continuous_.template block<1, 2>(kLeftVelocity, kLeftVoltage) = |
James Kuszmaul | d478f87 | 2020-03-16 20:54:27 -0700 | [diff] [blame] | 735 | vel_coefs.B_continuous.row(0).template cast<Scalar>(); |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 736 | B_continuous_.template block<1, 2>(kRightVelocity, kLeftVoltage) = |
James Kuszmaul | d478f87 | 2020-03-16 20:54:27 -0700 | [diff] [blame] | 737 | vel_coefs.B_continuous.row(1).template cast<Scalar>(); |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 738 | A_continuous_.template block<kNStates, 2>(0, kLeftVoltageError) = |
| 739 | B_continuous_.template block<kNStates, 2>(0, kLeftVoltage); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 740 | |
| 741 | Q_continuous_.setZero(); |
| 742 | // TODO(james): Improve estimates of process noise--e.g., X/Y noise can |
James Kuszmaul | 1057ce8 | 2019-02-09 17:58:24 -0800 | [diff] [blame] | 743 | // probably be reduced when we are stopped because you rarely jump randomly. |
| 744 | // Or maybe it's more appropriate to scale wheelspeed noise with wheelspeed, |
| 745 | // since the wheels aren't likely to slip much stopped. |
James Kuszmaul | a5632fe | 2019-03-23 20:28:33 -0700 | [diff] [blame] | 746 | Q_continuous_(kX, kX) = 0.002; |
| 747 | Q_continuous_(kY, kY) = 0.002; |
James Kuszmaul | 7f1a408 | 2019-04-14 10:50:44 -0700 | [diff] [blame] | 748 | Q_continuous_(kTheta, kTheta) = 0.0001; |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 749 | Q_continuous_(kLeftEncoder, kLeftEncoder) = std::pow(0.15, 2.0); |
| 750 | Q_continuous_(kRightEncoder, kRightEncoder) = std::pow(0.15, 2.0); |
| 751 | Q_continuous_(kLeftVelocity, kLeftVelocity) = std::pow(0.5, 2.0); |
| 752 | Q_continuous_(kRightVelocity, kRightVelocity) = std::pow(0.5, 2.0); |
| 753 | Q_continuous_(kLeftVoltageError, kLeftVoltageError) = std::pow(10.0, 2.0); |
| 754 | Q_continuous_(kRightVoltageError, kRightVoltageError) = std::pow(10.0, 2.0); |
| 755 | Q_continuous_(kAngularError, kAngularError) = std::pow(2.0, 2.0); |
| 756 | // This noise value largely governs whether we will trust the encoders or |
| 757 | // accelerometer more for estimating the robot position. |
James Kuszmaul | 5398fae | 2020-02-17 16:44:03 -0800 | [diff] [blame] | 758 | // Note that this also affects how we interpret camera measurements, |
| 759 | // particularly when using a heading/distance/skew measurement--if the |
| 760 | // noise on these numbers is particularly high, then we can end up with weird |
| 761 | // dynamics where a camera update both shifts our X/Y position and adjusts our |
| 762 | // velocity estimates substantially, causing the camera updates to create |
Austin Schuh | d749d93 | 2020-12-30 21:38:40 -0800 | [diff] [blame] | 763 | // "momentum" and if we don't trust the encoders enough, then we have no way |
| 764 | // of determining that the velocity updates are bogus. This also interacts |
| 765 | // with kVelocityOffsetTimeConstant. |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 766 | Q_continuous_(kLongitudinalVelocityOffset, kLongitudinalVelocityOffset) = |
| 767 | std::pow(1.1, 2.0); |
| 768 | Q_continuous_(kLateralVelocity, kLateralVelocity) = std::pow(0.1, 2.0); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 769 | |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 770 | H_encoders_and_gyro_.setZero(); |
| 771 | // Encoders are stored directly in the state matrix, so are a minor |
| 772 | // transform away. |
| 773 | H_encoders_and_gyro_(0, kLeftEncoder) = 1.0; |
| 774 | H_encoders_and_gyro_(1, kRightEncoder) = 1.0; |
| 775 | // Gyro rate is just the difference between right/left side speeds: |
| 776 | H_encoders_and_gyro_(2, kLeftVelocity) = -1.0 / diameter; |
| 777 | H_encoders_and_gyro_(2, kRightVelocity) = 1.0 / diameter; |
| 778 | |
James Kuszmaul | 3c5b4d3 | 2020-02-11 17:22:14 -0800 | [diff] [blame] | 779 | encoder_noise_ = 5e-9; |
| 780 | gyro_noise_ = 1e-13; |
Austin Schuh | 9fe68f7 | 2019-08-10 19:32:03 -0700 | [diff] [blame] | 781 | |
| 782 | X_hat_.setZero(); |
| 783 | P_.setZero(); |
James Kuszmaul | 2ed7b3c | 2019-02-09 18:26:19 -0800 | [diff] [blame] | 784 | } |
| 785 | |
| 786 | } // namespace drivetrain |
| 787 | } // namespace control_loops |
| 788 | } // namespace frc971 |
| 789 | |
| 790 | #endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_ |