File content as of revision 1:f19909200517:

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

using std::vector;

class STATE_FEEDBACK_INTEGRAL{
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

    // System parameters
    vector<vector<float> > E_out; // System output matrix
    // Controller parameters
    vector<vector<float> > K_full; // Full state feedback gain
    vector<vector<float> > K_int; // Gain for integral action


    // States
    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> command; // r
    // Integral state
    vector<float> state_int; // x_i

    STATE_FEEDBACK_INTEGRAL(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
    // Assign Parameters
    void assign_E_out(float* E_out_in, size_t q_in, size_t n_in);
    void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
    void assign_K_int(float* K_int_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;

    // Calculate the sys_outputs
    void get_sys_outputs(void); // Calculate the sys_outputs from states, by mutiplying E_out

    // Calculate the Integral
    void get_integral(bool enable); // Calculate the state_int


    // 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
    // Increment
    void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b


};

#endif