File content as of revision 4:958c25c1b151:

#ifndef STATE_FEEDBACK_H
#define STATE_FEEDBACK_H
//
#include <vector>

using std::vector;

class STATE_FEEDBACK{
public:
    // Dimensions
    size_t n; // Number of states
    size_t p; // Number of inputs of the plant
    size_t q; // Number of outputs of the plant, no use in full state feed back case

    float Ts; // Sampling time

    vector<vector<float> > K_full; // Full state feedback gain
    // vector<vector<float> > N_xd; // Feed-forward gain for x_d, x_d = N_xd*r
    // vector<vector<float> > N_ud; // Feed-forward gain for u_d, u_d = N_ud*r
    vector<vector<float> > N_total; // Feed-forward gain for compensating u, u = -K*x + N_total*r

    //
    vector<float> states; // States
    vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
    vector<float> sys_outputs; // The output of the plant, "y", the input of the controller
    // Command (equalibrium state)
    // vector<float> states_d; // x_d
    // vector<float> inputs_d; // u_d
    vector<float> sys_inputs_compensate; // N_total*r
    vector<float> command; // r

    STATE_FEEDBACK(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
    // Assign Parameters
    void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
    // void assign_N_xd(float* N_xd_in, size_t n_in, size_t q_in);
    // void assign_N_ud(float* N_ud_in, size_t p_in, size_t q_in);
    void assign_N_total(float* N_ud_in, size_t p_in, size_t q_in);
    //
    void fullStateFeedBack_calc(bool enable);

private:

    vector<float> zeros_n;
    vector<float> zeros_p;
    vector<float> zeros_q;

    // Command (equalibrium state) related calculation
    void get_inputs_compensate(void); // Calculate the compensation variable, states_d and sys_inputs_compensate

    // Utilities
    void Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
    vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
    vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
    vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a


};

#endif