File content as of revision 0:8dbe2a4687b8:

#include "STATE_FEEDBACK.h"

STATE_FEEDBACK::STATE_FEEDBACK(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), K_full(num_in, vector<float>(num_state,0.0))
{
    zeros_n.assign(n, 0.0);
    zeros_p.assign(p, 0.0);
    zeros_q.assign(q, 0.0);
    //
    states = zeros_n;
    sys_inputs = zeros_p;
    sys_outputs = zeros_q;

}
void STATE_FEEDBACK::fullStateFeedBack_calc(){
    Mat_multiply_Vec(sys_inputs, K_full, states);
}
void STATE_FEEDBACK::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>::iterator it_out;
    vector<const float>::iterator it_m_row;
    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];
            (*it_out) += (*it_m_row) * (*it_v);
            //
            it_m_row++;
            it_v++;
        }
        it_out++;
    }
}