File content as of revision 0:533d5685b66c:

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

using std::vector;

class MEASUREMENT_ERROR_ADAPTATION_CONTROL{
public:
    // Dimensions
    size_t n; // Number of states
    size_t p; // Number of inputs of the plant
    size_t q; // Number of channels that have measurement errors

    float Ts; // Sampling time

    // System parameters
    vector<vector<float> > C_error; // Measurement error input matrix
    // Controller parameters
    vector<vector<float> > K_full; // Full state feedback gain
    vector<vector<float> > K_phi; // Gain for integral action
    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


    // States
    vector<float> states_est; // States, "x_est", related to states_d (treats states_d as the origin)
    vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
    vector<float> sys_output; // The output of the plant, "y", the "input" of the controller
    vector<float> MeasurementErrors; // The measurement error of the sensors, "phi"
    // Command (equalibrium state)
    vector<float> states_d; // x_d
    vector<float> inputs_d; // u_d
    vector<float> command; // r


    MEASUREMENT_ERROR_ADAPTATION_CONTROL(size_t num_state, size_t num_in, size_t num_out, float samplingTime);
    // Assign Parameters
    void assign_C_error(float* C_error_in, size_t n_in, size_t q_in);
    void assign_K_full(float* K_full_in, size_t p_in, size_t n_in);
    void assign_K_phi(float* K_phi_in, size_t q_in, size_t n_in);
    //
    void assign_N_xd(float* N_xd_in, size_t n_in, size_t p_in);
    void assign_N_ud(float* N_ud_in, size_t p_in); // p by p square matrix

    //
    void iterateOnce(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

    // Calculate the states_est
    void get_states_est(void); // Calculate the states_est from MeasurementErrors and states_d

    // Calculate the estimation of MeasurementErrors
    void get_MeasurementErrors_est(bool enable); // Calculate the MeasurementErrors


    // 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