frc971/control_loops/drivetrain/hybrid_ekf.h

#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_

#include <chrono>

#include "aos/containers/priority_queue.h"
#include "aos/util/math.h"
#include "frc971/control_loops/c2d.h"
#include "frc971/control_loops/runge_kutta.h"
#include "Eigen/Dense"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"

namespace y2019 {
namespace control_loops {
namespace testing {
class ParameterizedLocalizerTest;
}  // namespace testing
}  // namespace control_loops
}  // namespace y2019

namespace frc971 {
namespace control_loops {
namespace drivetrain {

namespace testing {
class HybridEkfTest;
}

// HybridEkf is an EKF for use in robot localization. It is currently
// coded for use with drivetrains in particular, and so the states and inputs
// are chosen as such.
// The "Hybrid" part of the name refers to the fact that it can take in
// measurements with variable time-steps.
// measurements can also have been taken in the past and we maintain a buffer
// so that we can replay the kalman filter whenever we get an old measurement.
// Currently, this class provides the necessary utilities for doing
// measurement updates with an encoder/gyro as well as a more generic
// update function that can be used for arbitrary nonlinear updates (presumably
// a camera update).
template <typename Scalar = double>
class HybridEkf {
 public:
  // An enum specifying what each index in the state vector is for.
  enum StateIdx {
    kX = 0,
    kY = 1,
    kTheta = 2,
    kLeftEncoder = 3,
    kLeftVelocity = 4,
    kRightEncoder = 5,
    kRightVelocity = 6,
    kLeftVoltageError = 7,
    kRightVoltageError = 8 ,
    kAngularError = 9,
  };
  static constexpr int kNStates = 10;
  static constexpr int kNInputs = 2;
  // Number of previous samples to save.
  static constexpr int kSaveSamples = 50;
  // Assume that all correction steps will have kNOutputs
  // dimensions.
  // TODO(james): Relax this assumption; relaxing it requires
  // figuring out how to deal with storing variable size
  // observation matrices, though.
  static constexpr int kNOutputs = 3;
  // Inputs are [left_volts, right_volts]
  typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
  // Outputs are either:
  // [left_encoder, right_encoder, gyro_vel]; or [heading, distance, skew] to
  // some target. This makes it so we don't have to figure out how we store
  // variable-size measurement updates.
  typedef Eigen::Matrix<Scalar, kNOutputs, 1> Output;
  typedef Eigen::Matrix<Scalar, kNStates, kNStates> StateSquare;
  // State is [x_position, y_position, theta, Kalman States], where
  // Kalman States are the states from the standard drivetrain Kalman Filter,
  // which is: [left encoder, left ground vel, right encoder, right ground vel,
  // left voltage error, right voltage error, angular_error], where:
  // left/right encoder should correspond directly to encoder readings
  // left/right velocities are the velocity of the left/right sides over the
  //   ground (i.e., corrected for angular_error).
  // voltage errors are the difference between commanded and effective voltage,
  //   used to estimate consistent modelling errors (e.g., friction).
  // angular error is the difference between the angular velocity as estimated
  //   by the encoders vs. estimated by the gyro, such as might be caused by
  //   wheels on one side of the drivetrain being too small or one side's
  //   wheels slipping more than the other.
  typedef Eigen::Matrix<Scalar, kNStates, 1> State;

  // Constructs a HybridEkf for a particular drivetrain.
  // Currently, we use the drivetrain config for modelling constants
  // (continuous time A and B matrices) and for the noise matrices for the
  // encoders/gyro.
  HybridEkf(const DrivetrainConfig<Scalar> &dt_config)
      : dt_config_(dt_config),
        velocity_drivetrain_coefficients_(
            dt_config.make_hybrid_drivetrain_velocity_loop()
                .plant()
                .coefficients()) {
    InitializeMatrices();
  }

  // Set the initial guess of the state. Can only be called once, and before
  // any measurement updates have occured.
  // TODO(james): We may want to actually re-initialize and reset things on
  // the field. Create some sort of Reset() function.
  void ResetInitialState(::aos::monotonic_clock::time_point t,
                         const State &state, const StateSquare &P) {
    observations_.clear();
    X_hat_ = state;
    have_zeroed_encoders_ = true;
    P_ = P;
    observations_.PushFromBottom(
        {t,
         t,
         X_hat_,
         P_,
         Input::Zero(),
         Output::Zero(),
         {},
         [](const State &, const Input &) { return Output::Zero(); },
         [](const State &) {
           return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
         },
         Eigen::Matrix<Scalar, kNOutputs, kNOutputs>::Identity()});
  }

  // Correct with:
  // A measurement z at time t with z = h(X_hat, U) + v where v has noise
  // covariance R.
  // Input U is applied from the previous timestep until time t.
  // If t is later than any previous measurements, then U must be provided.
  // If the measurement falls between two previous measurements, then U
  // can be provided or not; if U is not provided, then it is filled in based
  // on an assumption that the voltage was held constant between the time steps.
  // TODO(james): Is it necessary to explicitly to provide a version with H as a
  // matrix for linear cases?
  void Correct(
      const Output &z, const Input *U,
      ::std::function<
          void(const State &, const StateSquare &,
               ::std::function<Output(const State &, const Input &)> *,
               ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
                   const State &)> *)> make_h,
      ::std::function<Output(const State &, const Input &)> h,
      ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
          dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
      aos::monotonic_clock::time_point t);

  // A utility function for specifically updating with encoder and gyro
  // measurements.
  void UpdateEncodersAndGyro(const Scalar left_encoder,
                             const Scalar right_encoder, const Scalar gyro_rate,
                             const Input &U,
                             ::aos::monotonic_clock::time_point t) {
    // Because the check below for have_zeroed_encoders_ will add an
    // Observation, do a check here to ensure that initialization has been
    // performed and so there is at least one observation.
    CHECK(!observations_.empty());
    if (!have_zeroed_encoders_) {
      // This logic handles ensuring that on the first encoder reading, we
      // update the internal state for the encoders to match the reading.
      // Otherwise, if we restart the drivetrain without restarting
      // wpilib_interface, then we can get some obnoxious initial corrections
      // that mess up the localization.
      State newstate = X_hat_;
      newstate(kLeftEncoder, 0) = left_encoder;
      newstate(kRightEncoder, 0) = right_encoder;
      newstate(kLeftVoltageError, 0) = 0.0;
      newstate(kRightVoltageError, 0) = 0.0;
      newstate(kAngularError, 0) = 0.0;
      ResetInitialState(t, newstate, P_);
      // We need to set have_zeroed_encoders_ after ResetInitialPosition because
      // the reset clears have_zeroed_encoders_...
      have_zeroed_encoders_ = true;
    }
    Output z(left_encoder, right_encoder, gyro_rate);
    Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
    R.setZero();
    R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
    Correct(z, &U, {}, [this](const State &X, const Input &) {
                         return H_encoders_and_gyro_ * X;
                       },
            [this](const State &) { return H_encoders_and_gyro_; }, R, t);
  }

  // Sundry accessor:
  State X_hat() const { return X_hat_; }
  Scalar X_hat(long i) const { return X_hat_(i, 0); }
  StateSquare P() const { return P_; }
  ::aos::monotonic_clock::time_point latest_t() const {
    return observations_.top().t;
  }

 private:
  struct Observation {
    // Time when the observation was taken.
    aos::monotonic_clock::time_point t;
    // Time that the previous observation was taken:
    aos::monotonic_clock::time_point prev_t;
    // Estimate of state at previous observation time t, after accounting for
    // the previous observation.
    State X_hat;
    // Noise matrix corresponding to X_hat_.
    StateSquare P;
    // The input applied from previous observation until time t.
    Input U;
    // Measurement taken at that time.
    Output z;
    // A function to create h and dhdx from a given position/covariance
    // estimate. This is used by the camera to make it so that we only have to
    // match targets once.
    // Only called if h and dhdx are empty.
    ::std::function<
        void(const State &, const StateSquare &,
             ::std::function<Output(const State &, const Input &)> *,
             ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
                 const State &)> *)> make_h;
    // A function to calculate the expected output at a given state/input.
    // TODO(james): For encoders/gyro, it is linear and the function call may
    // be expensive. Potential source of optimization.
    ::std::function<Output(const State &, const Input &)> h;
    // The Jacobian of h with respect to x.
    // We assume that U has no impact on the Jacobian.
    // TODO(james): Currently, none of the users of this actually make use of
    // the ability to have dynamic dhdx (technically, the camera code should
    // recalculate it to be strictly correct, but I was both too lazy to do
    // so and it seemed unnecessary). This is a potential source for future
    // optimizations if function calls are being expensive.
    ::std::function<
        Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> dhdx;
    // The measurement noise matrix.
    Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;

    // In order to sort the observations in the PriorityQueue object, we
    // need a comparison function.
    friend bool operator <(const Observation &l, const Observation &r) {
      return l.t < r.t;
    }
  };

  void InitializeMatrices();

  StateSquare AForState(const State &X) const {
    StateSquare A_continuous = A_continuous_;
    const Scalar theta = X(kTheta, 0);
    const Scalar linear_vel =
        (X(kLeftVelocity, 0) + X(kRightVelocity, 0)) / 2.0;
    const Scalar stheta = ::std::sin(theta);
    const Scalar ctheta = ::std::cos(theta);
    // X and Y derivatives
    A_continuous(kX, kTheta) = -stheta * linear_vel;
    A_continuous(kX, kLeftVelocity) = ctheta / 2.0;
    A_continuous(kX, kRightVelocity) = ctheta / 2.0;
    A_continuous(kY, kTheta) = ctheta * linear_vel;
    A_continuous(kY, kLeftVelocity) = stheta / 2.0;
    A_continuous(kY, kRightVelocity) = stheta / 2.0;
    return A_continuous;
  }

  State DiffEq(const State &X, const Input &U) const {
    State Xdot = A_continuous_ * X + B_continuous_ * U;
    // And then we need to add on the terms for the x/y change:
    const Scalar theta = X(kTheta, 0);
    const Scalar linear_vel =
        (X(kLeftVelocity, 0) + X(kRightVelocity, 0)) / 2.0;
    const Scalar stheta = ::std::sin(theta);
    const Scalar ctheta = ::std::cos(theta);
    Xdot(kX, 0) = ctheta * linear_vel;
    Xdot(kY, 0) = stheta * linear_vel;
    return Xdot;
  }

  void PredictImpl(const Input &U, std::chrono::nanoseconds dt, State *state,
                   StateSquare *P) {
    StateSquare A_c = AForState(*state);
    StateSquare A_d;
    StateSquare Q_d;
    controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &Q_d, &A_d);

    *state = RungeKuttaU(
        [this](const State &X,
               const Input &U) { return DiffEq(X, U); },
        *state, U,
        ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
            .count());

    StateSquare Ptemp = A_d * *P * A_d.transpose() + Q_d;
    *P = Ptemp;
  }

  void CorrectImpl(const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
                   const Output &Z, const Output &expected_Z,
                   const Eigen::Matrix<Scalar, kNOutputs, kNStates> &H,
                   State *state, StateSquare *P) {
    Output err = Z - expected_Z;
    Eigen::Matrix<Scalar, kNStates, kNOutputs> PH = *P * H.transpose();
    Eigen::Matrix<Scalar, kNOutputs, kNOutputs> S = H * PH + R;
    Eigen::Matrix<Scalar, kNStates, kNOutputs> K = PH * S.inverse();
    *state += K * err;
    StateSquare Ptemp = (StateSquare::Identity() - K * H) * *P;
    *P = Ptemp;
  }

  void ProcessObservation(Observation *obs, const std::chrono::nanoseconds dt,
                          State *state, StateSquare *P) {
    *state = obs->X_hat;
    *P = obs->P;
    if (dt.count() != 0) {
      PredictImpl(obs->U, dt, state, P);
    }
    if (!(obs->h && obs->dhdx)) {
      CHECK(obs->make_h);
      obs->make_h(*state, *P, &obs->h, &obs->dhdx);
    }
    CorrectImpl(obs->R, obs->z, obs->h(*state, obs->U), obs->dhdx(*state),
                state, P);
  }

  DrivetrainConfig<Scalar> dt_config_;
  State X_hat_;
  StateFeedbackHybridPlantCoefficients<2, 2, 2, Scalar>
      velocity_drivetrain_coefficients_;
  StateSquare A_continuous_;
  StateSquare Q_continuous_;
  StateSquare P_;
  Eigen::Matrix<Scalar, kNOutputs, kNStates> H_encoders_and_gyro_;
  Scalar encoder_noise_, gyro_noise_;
  Eigen::Matrix<Scalar, kNStates, kNInputs> B_continuous_;

  bool have_zeroed_encoders_ = false;

  aos::PriorityQueue<Observation, kSaveSamples, ::std::less<Observation>>
      observations_;

  friend class testing::HybridEkfTest;
  friend class ::y2019::control_loops::testing::ParameterizedLocalizerTest;
};  // class HybridEkf

template <typename Scalar>
void HybridEkf<Scalar>::Correct(
    const Output &z, const Input *U,
    ::std::function<
        void(const State &, const StateSquare &,
             ::std::function<Output(const State &, const Input &)> *,
             ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
                 const State &)> *)> make_h,
    ::std::function<Output(const State &, const Input &)> h,
    ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
        dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
    aos::monotonic_clock::time_point t) {
  CHECK(!observations_.empty());
  if (!observations_.full() && t < observations_.begin()->t) {
    LOG(ERROR,
        "Dropped an observation that was received before we "
        "initialized.\n");
    return;
  }
  auto cur_it =
      observations_.PushFromBottom({t, t, State::Zero(), StateSquare::Zero(),
                                    Input::Zero(), z, make_h, h, dhdx, R});
  if (cur_it == observations_.end()) {
    LOG(DEBUG,
        "Camera dropped off of end with time of %fs; earliest observation in "
        "queue has time of %fs.\n",
        ::std::chrono::duration_cast<::std::chrono::duration<double>>(
            t.time_since_epoch()).count(),
        ::std::chrono::duration_cast<::std::chrono::duration<double>>(
            observations_.begin()->t.time_since_epoch()).count());
    return;
  }

  // Now we populate any state information that depends on where the
  // observation was inserted into the queue. X_hat and P must be populated
  // from the values present in the observation *following* this one in
  // the queue (note that the X_hat and P that we store in each observation
  // is the values that they held after accounting for the previous
  // measurement and before accounting for the time between the previous and
  // current measurement). If we appended to the end of the queue, then
  // we need to pull from X_hat_ and P_ specifically.
  // Furthermore, for U:
  // -If the observation was inserted at the end, then the user must've
  //  provided U and we use it.
  // -Otherwise, only grab U if necessary.
  auto next_it = cur_it;
  ++next_it;
  if (next_it == observations_.end()) {
    cur_it->X_hat = X_hat_;
    cur_it->P = P_;
    // Note that if next_it == observations_.end(), then because we already
    // checked for !observations_.empty(), we are guaranteed to have
    // valid prev_it.
    auto prev_it = cur_it;
    --prev_it;
    cur_it->prev_t = prev_it->t;
    // TODO(james): Figure out a saner way of handling this.
    CHECK(U != nullptr);
    cur_it->U = *U;
  } else {
    cur_it->X_hat = next_it->X_hat;
    cur_it->P = next_it->P;
    cur_it->prev_t = next_it->prev_t;
    next_it->prev_t = cur_it->t;
    cur_it->U = (U == nullptr) ? next_it->U : *U;
  }
  // Now we need to rerun the predict step from the previous to the new
  // observation as well as every following correct/predict up to the current
  // time.
  while (true) {
    // We use X_hat_ and P_ to store the intermediate states, and then
    // once we reach the end they will all be up-to-date.
    ProcessObservation(&*cur_it, cur_it->t - cur_it->prev_t, &X_hat_, &P_);
    CHECK(X_hat_.allFinite());
    if (next_it != observations_.end()) {
      next_it->X_hat = X_hat_;
      next_it->P = P_;
    } else {
      break;
    }
    ++cur_it;
    ++next_it;
  }
}

template <typename Scalar>
void HybridEkf<Scalar>::InitializeMatrices() {
  A_continuous_.setZero();
  const Scalar diameter = 2.0 * dt_config_.robot_radius;
  // Theta derivative
  A_continuous_(kTheta, kLeftVelocity) = -1.0 / diameter;
  A_continuous_(kTheta, kRightVelocity) = 1.0 / diameter;

  // Encoder derivatives
  A_continuous_(kLeftEncoder, kLeftVelocity) = 1.0;
  A_continuous_(kLeftEncoder, kAngularError) = 1.0;
  A_continuous_(kRightEncoder, kRightVelocity) = 1.0;
  A_continuous_(kRightEncoder, kAngularError) = -1.0;

  // Pull velocity derivatives from velocity matrices.
  // Note that this looks really awkward (doesn't use
  // Eigen blocks) because someone decided that the full
  // drivetrain Kalman Filter should half a weird convention.
  // TODO(james): Support shifting drivetrains with changing A_continuous
  const auto &vel_coefs = velocity_drivetrain_coefficients_;
  A_continuous_(kLeftVelocity, kLeftVelocity) = vel_coefs.A_continuous(0, 0);
  A_continuous_(kLeftVelocity, kRightVelocity) = vel_coefs.A_continuous(0, 1);
  A_continuous_(kRightVelocity, kLeftVelocity) = vel_coefs.A_continuous(1, 0);
  A_continuous_(kRightVelocity, kRightVelocity) = vel_coefs.A_continuous(1, 1);

  // Provide for voltage error terms:
  B_continuous_.setZero();
  B_continuous_.row(kLeftVelocity) = vel_coefs.B_continuous.row(0);
  B_continuous_.row(kRightVelocity) = vel_coefs.B_continuous.row(1);
  A_continuous_.template block<kNStates, kNInputs>(0, 7) = B_continuous_;

  Q_continuous_.setZero();
  // TODO(james): Improve estimates of process noise--e.g., X/Y noise can
  // probably be reduced when we are stopped because you rarely jump randomly.
  // Or maybe it's more appropriate to scale wheelspeed noise with wheelspeed,
  // since the wheels aren't likely to slip much stopped.
  Q_continuous_(kX, kX) = 0.002;
  Q_continuous_(kY, kY) = 0.002;
  Q_continuous_(kTheta, kTheta) = 0.0002;
  Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.15, 2.0);
  Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.15, 2.0);
  Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.5, 2.0);
  Q_continuous_(kRightVelocity, kRightVelocity) = ::std::pow(0.5, 2.0);
  Q_continuous_(kLeftVoltageError, kLeftVoltageError) = ::std::pow(10.0, 2.0);
  Q_continuous_(kRightVoltageError, kRightVoltageError) = ::std::pow(10.0, 2.0);
  Q_continuous_(kAngularError, kAngularError) = ::std::pow(2.0, 2.0);

  P_.setZero();
  P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;

  H_encoders_and_gyro_.setZero();
  // Encoders are stored directly in the state matrix, so are a minor
  // transform away.
  H_encoders_and_gyro_(0, kLeftEncoder) = 1.0;
  H_encoders_and_gyro_(1, kRightEncoder) = 1.0;
  // Gyro rate is just the difference between right/left side speeds:
  H_encoders_and_gyro_(2, kLeftVelocity) = -1.0 / diameter;
  H_encoders_and_gyro_(2, kRightVelocity) = 1.0 / diameter;

  const Eigen::Matrix<Scalar, 4, 4> R_kf_drivetrain =
      dt_config_.make_kf_drivetrain_loop().observer().coefficients().R;
  // TODO(james): The multipliers here are hand-waving things that I put in when
  // tuning things. I haven't yet tried messing with these values again.
  encoder_noise_ = 0.05 * R_kf_drivetrain(0, 0);
  gyro_noise_ = 0.1 * R_kf_drivetrain(2, 2);
}

}  // namespace drivetrain
}  // namespace control_loops
}  // namespace frc971

#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_