linear_plant_inversion_feedforward.h

#pragma once
 
#include "core/math/eigen_interface.h"
 
#include "core/math/systems/linear_system.h"
#include "core/math/systems/discretization.h"

template <int STATES, int INPUTS> class LinearPlantInversionFeedforward {
  public:
    template <int OUTPUTS>
    LinearPlantInversionFeedforward(LinearSystem<STATES, INPUTS, OUTPUTS> &plant, const double &dt)
        : LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {}

    LinearPlantInversionFeedforward(const EMat<STATES, STATES> &A, const EMat<STATES, INPUTS> &B, const double &dt)
        : A_(A), B_(B), m_dt(dt) {
        auto [Ad, Bd] = discretize_AB(A, B, dt);
        Ad_ = Ad;
        Bd_ = Bd;
    }

    EVec<INPUTS> calculate(const EVec<STATES> &r, const EVec<STATES> &next_r) {
        /*
         * ẋ = Ax + Bu
         * Bu = ẋ - Ax
         * u = B \ (ẋ - Ax)
         * u = B \ (next_r - Br)
         */
        uff_ = Bd_.householderQr().solve(next_r - (Ad_ * r));
        r_ = next_r;
 
        return uff_;
    }

    EVec<INPUTS> calculate(const EVec<STATES> &next_r) { return calculate(r_, next_r); }

    EVec<INPUTS> calculate(const EVec<STATES> &r, const EVec<STATES> &next_r, const double &dt) {
        auto [Ad, Bd] = discretize_AB(A_, B_, dt);
        /*
         * ẋ = Ax + Bu
         * Bu = ẋ - Ax
         * u = B \ (ẋ - Ax)
         * u = B \ (next_r - Br)
         */
        uff_ = Bd.householderQr().solve(next_r - (Ad * r));
        r_ = next_r;
 
        return uff_;
    }

    EVec<INPUTS> calculate(const EVec<STATES> &next_r, const double &dt) { return calculate(r_, next_r, dt); }

    void reset(const EVec<STATES> &initial_state) {
        r_ = initial_state;
        uff_.setZero();
    }

    void reset() {
        r_.setZero();
        uff_.setZero();
    }

    void set_r(const EVec<STATES> &r) { r_ = r; }
 
  private:
    // The continuous system matrices
    EMat<STATES, STATES> A_;
    EMat<STATES, INPUTS> B_;
 
    // The discrete system matrices discretized on the nominal timestep
    EMat<STATES, STATES> Ad_;
    EMat<STATES, INPUTS> Bd_;
 
    // The feedforward control input
    EVec<INPUTS> uff_;
    // The current reference state
    EVec<STATES> r_;
 
    double m_dt;
};