File content as of revision 0:37e27f2930a3:

#include "STATE_FEEDBACK_INTEGRAL.h"
// The controller is for the plant with (p, n, q) system
// Dimensions:
//
// Inputs, u   |   States, x    |   outputs, y
//     p     -->      n        -->      q
//
//

STATE_FEEDBACK_INTEGRAL::STATE_FEEDBACK_INTEGRAL(size_t num_state, size_t num_in, size_t num_out, float samplingTime):
    n(num_state), p(num_in), q(num_out), Ts(samplingTime),
    E_out(num_out, vector<float>(num_state,0.0)),
    K_full(num_in, vector<float>(num_state,0.0)),
    K_int(num_in, vector<float>(num_out,0.0))
{
    // Normally, q = p
    if (q > p)
        q = p;
    //
    zeros_n.assign(n, 0.0);
    zeros_p.assign(p, 0.0);
    zeros_q.assign(q, 0.0);

    // States
    states = zeros_n;
    sys_inputs = zeros_p;
    sys_outputs = zeros_q;

    // Command (equalibrium state)
    sys_inputs_compensate = zeros_p;
    command = zeros_q; // q = p

    // Integral state
    state_int = zeros_q;

}
// Assign Parameters
void STATE_FEEDBACK_INTEGRAL::assign_E_out(float* E_out_in, size_t q_in, size_t n_in){
    // E_out_in is the pointer of a mutidimentional array with size q_in by n_in
    if (q != q_in || n != n_in){
        q = q_in;
        n = n_in;
        zeros_q.resize(q, 0.0);
        zeros_n.resize(n, 0.0);
        E_out.assign(q, zeros_n);
    }
    //
    for (size_t i = 0; i < q; ++i){
        for (size_t j = 0; j < n; ++j){
            // E_out[i][j] = E_out_in[i][j];
            E_out[i][j] = *E_out_in;
            E_out_in++;
        }
    }
}
void STATE_FEEDBACK_INTEGRAL::assign_K_full(float* K_full_in, size_t p_in, size_t n_in){
    // K_full_in is the pointer of a mutidimentional array with size p_in by n_in
    if (n != n_in || p != p_in){
        n = n_in;
        p = p_in;
        zeros_n.resize(n, 0.0);
        zeros_p.resize(p, 0.0);
        K_full.assign(p, zeros_n);
    }
    //
    for (size_t i = 0; i < p; ++i){
        for (size_t j = 0; j < n; ++j){
            // K_full[i][j] = K_full_in[i][j];
            K_full[i][j] = *K_full_in;
            K_full_in++;
        }
    }
}
void STATE_FEEDBACK_INTEGRAL::assign_K_int(float* K_int_in, size_t p_in, size_t q_in){
    // K_int_in is the pointer of a mutidimentional array with size p_in by q_in
    if (p != p_in || q != q_in){
        p = p_in;
        q = q_in;
        zeros_p.resize(p, 0.0);
        zeros_q.resize(q, 0.0);
        K_int.assign(p, zeros_q);
    }
    //
    for (size_t i = 0; i < p; ++i){
        for (size_t j = 0; j < q; ++j){
            // K_int[i][j] = K_int_in[i][j];
            K_int[i][j] = *K_int_in;
            K_int_in++;
        }
    }
}
//
void STATE_FEEDBACK_INTEGRAL::fullStateFeedBack_calc(bool enable){

    // Control law
    if (enable){
        // sys_inputs = (-K_full*states) - K_int*state_int
        sys_inputs = Get_VectorPlus(Get_VectorScalarMultiply(Mat_multiply_Vec(K_full, states),-1.0), Mat_multiply_Vec(K_int, state_int) ,true); // minus
    }else{
        sys_inputs = zeros_p;
    }

    // Integral action
    get_integral(enable);
}

// Private functions
// Calculate the sys_outputs
void STATE_FEEDBACK_INTEGRAL::get_sys_outputs(void){ // Calculate the sys_outputs from states, by mutiplying E_out
    // sys_outputs = E_out*states
    Mat_multiply_Vec(sys_outputs, E_out, states);

}
// Calculate the Integral
void STATE_FEEDBACK_INTEGRAL::get_integral(bool enable){ // Calculate the state_int
    //
    // Calculate the sys_outputs
    get_sys_outputs();

    // Integral action
    // state_int += sys_outputs - command
    if (enable){
        Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // +=
    }else{
        state_int = zeros_q;
    }
}
// Utilities
void STATE_FEEDBACK_INTEGRAL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
    static vector<float>::iterator it_out;
    static vector<const float>::iterator it_m_row;
    static vector<const float>::iterator it_v;
    //
    it_out = v_out.begin();
    for (size_t i = 0; i < m_left.size(); ++i){
        *it_out = 0.0;
        it_m_row = m_left[i].begin();
        it_v = v_right.begin();
        for (size_t j = 0; j < m_left[i].size(); ++j){
            // *it_out += m_left[i][j] * v_right[j];
            if (*it_m_row != 0.0 && *it_v != 0.0){
                (*it_out) += (*it_m_row) * (*it_v);
            }else{
                // (*it_out) += 0.0
            }
            // (*it_out) += (*it_m_row) * (*it_v);
            //
            it_m_row++;
            it_v++;
        }
        it_out++;
    }
}
vector<float> STATE_FEEDBACK_INTEGRAL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
    static vector<float> v_out;
    // Size check
    if (v_out.size() != m_left.size()){
        v_out.resize(m_left.size());
    }
    // Iterators
    static vector<float>::iterator it_out;
    static vector<const float>::iterator it_m_row;
    static vector<const float>::iterator it_v;
    //
    it_out = v_out.begin();
    for (size_t i = 0; i < m_left.size(); ++i){
        *it_out = 0.0;
        it_m_row = m_left[i].begin();
        it_v = v_right.begin();
        for (size_t j = 0; j < m_left[i].size(); ++j){
            // *it_out += m_left[i][j] * v_right[j];
            if (*it_m_row != 0.0 && *it_v != 0.0){
                (*it_out) += (*it_m_row) * (*it_v);
            }else{
                // (*it_out) += 0.0
            }
            // (*it_out) += (*it_m_row) * (*it_v);
            //
            it_m_row++;
            it_v++;
        }
        it_out++;
    }
    return v_out;
}
vector<float> STATE_FEEDBACK_INTEGRAL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
{
    static vector<float> v_c;
    // Size check
    if (v_c.size() != v_a.size()){
        v_c.resize(v_a.size());
    }
    //
    for (size_t i = 0; i < v_a.size(); ++i){
        if (is_minus){
            v_c[i] = v_a[i] - v_b[i];
        }else{
            v_c[i] = v_a[i] + v_b[i];
        }
    }
    return v_c;
}
vector<float> STATE_FEEDBACK_INTEGRAL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
{
    static vector<float> v_c;
    // Size check
    if (v_c.size() != v_a.size()){
        v_c.resize(v_a.size());
    }
    // for pure negative
    if (scale == -1.0){
        for (size_t i = 0; i < v_a.size(); ++i){
            v_c[i] = -v_a[i];
        }
        return v_c;
    }
    // else
    for (size_t i = 0; i < v_a.size(); ++i){
        v_c[i] = scale*v_a[i];

    }
    return v_c;
}
// Increment
void STATE_FEEDBACK_INTEGRAL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
    // Size check
    if (v_a.size() != v_b.size()){
        v_a.resize(v_b.size());
    }
    //
    if (is_minus){ // -=
        for (size_t i = 0; i < v_b.size(); ++i){
            v_a[i] -= v_b[i];
        }
    }else{ // +=
        for (size_t i = 0; i < v_b.size(); ++i){
            v_a[i] += v_b[i];
        }
    }

}