File content as of revision 1:085d41355949:

#include "STATES_OBSERVER_DELAY.h"

STATES_OBSERVER_DELAY::STATES_OBSERVER_DELAY(size_t num_state, size_t num_in, size_t num_out, size_t delay_sample, float samplingTime):
        n(num_state), p(num_in), q(num_out), d(delay_sample), Ts(samplingTime),
        states_est_buffer((delay_sample+1),num_state)
{
    // To enble, run *.start() function
    enable = false;
    flag_reset = false;
    //
    zeros_n.assign(n, 0.0);
    zeros_p.assign(p, 0.0);
    zeros_q.assign(q, 0.0);
    // Parameters for observer
    Ad.assign(n,zeros_n);
    Bd.assign(n,zeros_p);
    if (num_out == 0){
        enable_Cd = false; // Default: not using Cd
        q = n;
        zeros_q.resize(q, 0.0);
        Cd.assign(q,zeros_n);
    }else{
        enable_Cd = true; // using Cd
        Cd.assign(q,zeros_n);
    }
    // Gain matrix
    Ld.assign(n*(d+1),zeros_q); // Ld is an n(d+1) by q matrix (or n(d+1) by n if Cd is not used)

    // Input-signals of the observer
    sys_inputs = zeros_p;
    sys_outputs = zeros_q;
    sys_extraDisturbance = zeros_n;
    // Variables of the observer
    states_est_buffer.Reset(zeros_n);
    state_est = zeros_n;
    // states_est_delay_d = zeros_n;
    y_est = zeros_q;
    est_error = zeros_q;
}
void STATES_OBSERVER_DELAY::start(){
    enable = true;
}
void STATES_OBSERVER_DELAY::stop(){
    if (!enable){
        return;
    }
    enable = false;
    flag_reset = true;
}
void STATES_OBSERVER_DELAY::reset(){
    //
    // Input-signals of the observer
    sys_inputs = zeros_p;
    sys_outputs = zeros_q;
    // Variables of the observer
    states_est_buffer.Reset(zeros_n);
    state_est = zeros_n;
    // states_est_delay_d = zeros_n;
    y_est = zeros_q;
    est_error = zeros_q;
}


// Assignments for observer
// Assign continuous-time version of system matrices
void STATES_OBSERVER_DELAY::assign_At(float* At_in, size_t n_in){ // Continuous-time version
    // At_in is the pointer of a mutidimentional array with size n_in by n_in
    if (n != n_in){
        n = n_in;
        zeros_n.resize(n, 0.0);
        Ad.assign(n, zeros_n);
    }
    //
    for (size_t i = 0; i < n; ++i){
        for (size_t j = 0; j < n; ++j){
            // Ad[i][j] = At_in[i][j];
            Ad[i][j] = (*At_in)*Ts;
            //
            if (i == j){
                Ad[i][j] += 1.0;
            }
            //
            At_in++;
        }
    }
}
void STATES_OBSERVER_DELAY::assign_Bt(float* Bt_in, size_t n_in, size_t p_in){ // Continuous-time version
    // Bt_in is the pointer of a mutidimentional array with size n_in by p_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);
        Bd.assign(n, zeros_p);
    }
    //
    for (size_t i = 0; i < n; ++i){
        for (size_t j = 0; j < p; ++j){
            // Bd[i][j] = Bt_in[i][j];
            Bd[i][j] = (*Bt_in)*Ts;
            Bt_in++;
        }
    }
}
void STATES_OBSERVER_DELAY::assign_Lt(float* Lt_in, size_t n_in, size_t d_in, size_t q_in){ // Continuous-time version
    // Lt_in is the pointer of a mutidimentional array with size n_in*(d_in+1) by q_in
    if (n != n_in || d != d_in || q != q_in){
        n = n_in;
        q = q_in;
        zeros_n.resize(n, 0.0);
        zeros_q.resize(q, 0.0);
        //
        if (d != d_in){
            d = d_in;
            states_est_buffer.Init((d+1), zeros_n);
        }
        Ld.assign(n*(d+1), zeros_q);
    }
    //
    for (size_t i = 0; i < n*(d+1); ++i){
        for (size_t j = 0; j < q; ++j){
            // Ld[i][j] = Lt_in[i][j];
            Ld[i][j] = (*Lt_in)*Ts;
            Lt_in++;
        }
    }
}
// ** Assign the continuous-time version of system matrices by matrices
void STATES_OBSERVER_DELAY::assign_At(const vector<vector<float> > &At_in){
    size_t n_in = At_in.size();
    if (n != n_in){
        n = n_in;
        zeros_n.resize(n, 0.0);
        Ad.assign(n, zeros_n);
    }
    // Ad
    for (size_t i = 0; i < n; ++i){
        for (size_t j = 0; j < n; ++j){
            //
            Ad[i][j] = Ts*At_in[i][j];
            //
            if (i == j){
                Ad[i][j] += 1.0;
            }
        }
    }
}
void STATES_OBSERVER_DELAY::assign_Bt(const vector<vector<float> > &Bt_in){
    size_t n_in = Bt_in.size();
    size_t p_in = Bt_in[0].size();
    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);
        Bd.assign(n, zeros_p);
    }
    // Bd
    for (size_t i = 0; i < n; ++i){
        for (size_t j = 0; j < p; ++j){
            // Bd[i][j] = Bt_in[i][j];
            Bd[i][j] = Ts*Bt_in[i][j];
        }
    }
}
void STATES_OBSERVER_DELAY::assign_Lt(const vector<vector<float> > &Lt_in){
    size_t ndp1_in = Lt_in.size();
    size_t q_in = Lt_in[0].size();
    //
    if ( n*(d+1) != ndp1_in || q != q_in){
        n = ndp1_in/(d+1);
        q = q_in;
        zeros_n.resize(n, 0.0);
        zeros_q.resize(q, 0.0);
        Ld.assign(n*(d+1), zeros_q);
    }
    // Ld
    for (size_t i = 0; i < n*(d+1); ++i){
        for (size_t j = 0; j < q; ++j){
            //
            Ld[i][j] = Ts*Lt_in[i][j];
        }
    }
}
// Assign discrete-time version of system matrices directly
void STATES_OBSERVER_DELAY::assign_Ad(float* Ad_in, size_t n_in){ // Discrete-time version
    // Ad_in is the pointer of a mutidimentional array with size n_in by n_in
    if (n != n_in){
        n = n_in;
        zeros_n.resize(n, 0.0);
        Ad.assign(n, zeros_n);
    }
    //
    for (size_t i = 0; i < n; ++i){
        for (size_t j = 0; j < n; ++j){
            // Ad[i][j] = Ad_in[i][j];
            Ad[i][j] = *Ad_in;
            Ad_in++;
        }
    }
}
void STATES_OBSERVER_DELAY::assign_Bd(float* Bd_in, size_t n_in, size_t p_in){ // Discrete-time version
    // Bd_in is the pointer of a mutidimentional array with size n_in by p_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);
        Bd.assign(n, zeros_p);
    }
    //
    for (size_t i = 0; i < n; ++i){
        for (size_t j = 0; j < p; ++j){
            // Bd[i][j] = Bd_in[i][j];
            Bd[i][j] = *Bd_in;
            Bd_in++;
        }
    }
}
//
void STATES_OBSERVER_DELAY::assign_Cd(float* Cd_in, size_t q_in, size_t n_in){
    // Cd_in is the pointer of a mutidimentional array with size q_in by n_in
    // q = n if not using Cd
    if (q_in == 0){
        enable_Cd = false; // Default: not using Cd
        q_in = n_in;
        //
        q = q_in;
        n = n_in;
        zeros_n.resize(n,0.0);
        zeros_q.resize(q, 0.0);
        Cd.assign(q,zeros_n);
        //
        return;
    }else{
        enable_Cd = true; // using Cd
        // Cd.assign(q,zeros_n);
    }

    if (n != n_in || q != q_in){
        n = n_in;
        q = q_in;
        zeros_n.resize(n, 0.0);
        zeros_q.resize(q, 0.0);
        Cd.assign(q, zeros_n);
    }
    //
    for (size_t i = 0; i < q; ++i){
        for (size_t j = 0; j < n; ++j){
            // Cd[i][j] = Cd_in[i][j];
            Cd[i][j] = *Cd_in;
            Cd_in++;
        }
    }

}
// Assignment for observer Gain
void STATES_OBSERVER_DELAY::assign_Ld(float* Ld_in, size_t n_in, size_t d_in, size_t q_in){
    // Ld_in is the pointer of a mutidimentional array with size n_in*(d_in+1) by q_in
    if (n != n_in || d != d_in || q != q_in){
        n = n_in;
        q = q_in;
        zeros_n.resize(n, 0.0);
        zeros_q.resize(q, 0.0);
        //
        if (d != d_in){
            d = d_in;
            states_est_buffer.Init((d+1), zeros_n);
        }
        Ld.assign(n*(d+1), zeros_q);
    }
    //
    for (size_t i = 0; i < n*(d+1); ++i){
        for (size_t j = 0; j < q; ++j){
            // Ld[i][j] = Ld_in[i][j];
            Ld[i][j] = *Ld_in;
            Ld_in++;
        }
    }
}


void STATES_OBSERVER_DELAY::iterateOnce(void){
    if(!enable){
        if (flag_reset){
            reset();
            flag_reset = false;
        }
        return;
    }


    // Inputs of the observer:  sys_inputs, sys_outputs
    //
    // Get the delayed estimation of states
    // states_est_delay_d = states_est_buffer.Get(d); // Get the element that is d samples ago
    //
    if (enable_Cd){
        // y_est = Cd*states_est_delay_d
        y_est = Mat_multiply_Vec(Cd, states_est_buffer.Get(d));
        // est_error = y_est - sys_outputs
        est_error = Get_VectorPlus(y_est,sys_outputs,true); // minus
    }else{
        // est_error = y_est - sys_outputs
        est_error = Get_VectorPlus(states_est_buffer.Get(d), sys_outputs,true); // minus
    }


    // Get estimation
    state_est = states_est_buffer.Get(0);

    //-------------------------------Update-----------------------------//
    static vector<vector<float> >::iterator it_Ld;
    it_Ld = Ld.begin(); // it_Ld points to L0
    //
    static vector<float> states_new;
    // x_(k+1) = (Ad*x_k + Bd*sys_inputs + sys_extraDisturbance) - L*est_error)
    states_new = Get_VectorPlus(Mat_multiply_Vec(Ad,state_est), Mat_multiply_Vec(Bd,sys_inputs), false); // plus
    Get_VectorIncrement(states_new, sys_extraDisturbance, false); // +=, extra disturbances
    Get_VectorIncrement(states_new, Partial_Mat_multiply_Vec(it_Ld, n, est_error), true); // -=
    // states_new is equal to x_(k+1) and will be used to .Insert() the buffer

    // Update all the other states
    // from k to (k-d)+1, totally d samples
    // Currently it_Ld is pointing to "L1"
    for (size_t i = 0; i < d; ++i){
        // it_Ld will be automatically updated during iteration
        states_est_buffer.Increase(i, Partial_Mat_multiply_Vec(it_Ld, n, est_error), true); // -=
    }

    // Rotate the buffer
    states_est_buffer.Insert(states_new);
    

    /*
    // No-delay version
    //----------------------------------------//
    if (enable_Cd){
        // y_est = Cd*states_est_delay_d
        y_est = Mat_multiply_Vec(Cd, state_est);
        // est_error = y_est - sys_outputs
        est_error = Get_VectorPlus(y_est,sys_outputs, true); // minus
    }else{
        // est_error = y_est - sys_outputs
        est_error = Get_VectorPlus(state_est, sys_outputs, true); // minus
    }
    //
    state_est = Get_VectorPlus(Mat_multiply_Vec(Ad,state_est), Mat_multiply_Vec(Bd,sys_inputs), false); // plus
    // Get_VectorIncrement(state_est, sys_extraDisturbance, false); // +=, extra disturbances
    Get_VectorIncrement(state_est, Mat_multiply_Vec(Ld, est_error), true); // -=
    */
}

// Utilities
void STATES_OBSERVER_DELAY::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right

    // Size check
    if (v_out.size() != m_left.size()){
        v_out.resize(m_left.size());
    }

    /*
    //
    static vector<float>::iterator it_out;
    static vector<float>::iterator it_m_row;
    static vector<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 = vector<vector<float> >(m_left)[i].begin();
        it_v = vector<float>(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++;
    }
    */

    // Indexing
    for (size_t i = 0; i < m_left.size(); ++i){
        //
        v_out[i] = 0.0;
        //
        for (size_t j = 0; j < v_right.size(); ++j){
            if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
                v_out[i] += m_left[i][j]*v_right[j];
        }
    }

}
vector<float> STATES_OBSERVER_DELAY::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<float>::iterator it_m_row;
    static vector<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 = vector<vector<float> >(m_left)[i].begin();
        it_v = vector<float>(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++;
    }
    */

    // Indexing
    for (size_t i = 0; i < m_left.size(); ++i){
        //
        v_out[i] = 0.0;
        //
        for (size_t j = 0; j < v_right.size(); ++j){
            if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
                v_out[i] += m_left[i][j]*v_right[j];
        }
    }

    return v_out;
}
vector<float> STATES_OBSERVER_DELAY::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> STATES_OBSERVER_DELAY::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 STATES_OBSERVER_DELAY::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];
        }
    }

}
// Partial matrix multiplication
vector<float> STATES_OBSERVER_DELAY::Partial_Mat_multiply_Vec(vector<vector<float> >::iterator &it_m_left, size_t numRow_m_left, const vector<float> &v_right){ // v_out = m_left*v_right
    static vector<float> v_out;
    // Size check
    if (v_out.size() != numRow_m_left){
        v_out.resize(numRow_m_left);
    }
    /*
    // Iterators
    static vector<float>::iterator it_out;
    static vector<float>::iterator it_m_row;
    static vector<float>::iterator it_v;
    //
    it_out = v_out.begin();
    for (size_t i = 0; i < numRow_m_left; ++i){
        *it_out = 0.0;
        it_m_row = (*it_m_left).begin();
        it_v = vector<float>(v_right).begin();
        for (size_t j = 0; j < (*it_m_left).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++;
        it_m_left++;
    }
    */

    //
    // Indexing
    for (size_t i = 0; i < numRow_m_left; ++i){
        //
        v_out[i] = 0.0;
        //
        for (size_t j = 0; j < v_right.size(); ++j){
            if ( (*it_m_left)[j] != 0.0 && v_right[j] != 0.0)
                v_out[i] += (*it_m_left)[j]*v_right[j];
        }
        //
        it_m_left++;
    }


    return v_out;
}