A controller that is immune to measurement errors and keep the true states at the desired value, also known as "Zero-Torque Control"

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MEASUREMENT_ERROR_ADAPTATION_CONTROL.cpp Source File

MEASUREMENT_ERROR_ADAPTATION_CONTROL.cpp

00001 #include "MEASUREMENT_ERROR_ADAPTATION_CONTROL.h"
00002 // The controller is for the plant with (p, n, q) system
00003 // Dimensions:
00004 //
00005 // Inputs, u    |   States, x
00006 //     p     ---->      n
00007 //           q -- ^
00008 //  Measurement errors, phi
00009 
00010 MEASUREMENT_ERROR_ADAPTATION_CONTROL::MEASUREMENT_ERROR_ADAPTATION_CONTROL(size_t num_state, size_t num_in, size_t num_MeasurementError, float samplingTime):
00011     n(num_state), p(num_in), q(num_MeasurementError), Ts(samplingTime),
00012     C_error(num_state, vector<float>(num_MeasurementError,0.0)),
00013     K_full(num_in, vector<float>(num_state,0.0)),
00014     K_phi(num_MeasurementError, vector<float>(num_state,0.0)),
00015     N_xd(num_state, vector<float>(num_in, 0.0)),
00016     N_ud(num_in, vector<float>(num_in, 0.0))
00017 {
00018     //
00019     zeros_n.assign(n, 0.0);
00020     zeros_p.assign(p, 0.0);
00021     zeros_q.assign(q, 0.0);
00022 
00023     // States
00024     states_est = zeros_n; // states_est, "x_est"
00025     sys_inputs = zeros_p; // The inputs of the plant, "u"
00026     sys_output = zeros_n; // The output of the plant, "y"
00027     MeasurementErrors = zeros_q; // The measurement error of the sensors, "phi"
00028 
00029     // Command (equalibrium state)
00030     states_d = zeros_n; // x_d
00031     inputs_d = zeros_p; // u_d
00032     command = zeros_p; // r
00033 
00034 
00035 }
00036 // Assign Parameters
00037 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_C_error(float* C_error_in, size_t n_in, size_t q_in){
00038     // C_error_in is the pointer of a mutidimentional array with size n_in by q_in
00039     if (n != n_in || q != q_in){
00040         n = n_in;
00041         q = q_in;
00042         zeros_n.resize(n, 0.0);
00043         zeros_q.resize(q, 0.0);
00044         C_error.assign(n, zeros_q);
00045     }
00046     //
00047     for (size_t i = 0; i < n; ++i){
00048         for (size_t j = 0; j < q; ++j){
00049             // C_error[i][j] = C_error_in[i][j];
00050             C_error[i][j] = *C_error_in;
00051             C_error_in++;
00052         }
00053     }
00054 }
00055 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_K_full(float* K_full_in, size_t p_in, size_t n_in){
00056     // K_full_in is the pointer of a mutidimentional array with size p_in by n_in
00057     if (n != n_in || p != p_in){
00058         p = p_in;
00059         n = n_in;
00060         zeros_n.resize(n, 0.0);
00061         zeros_p.resize(p, 0.0);
00062         K_full.assign(p, zeros_n);
00063     }
00064     //
00065     for (size_t i = 0; i < p; ++i){
00066         for (size_t j = 0; j < n; ++j){
00067             // K_full[i][j] = K_full_in[i][j];
00068             K_full[i][j] = *K_full_in;
00069             K_full_in++;
00070         }
00071     }
00072 }
00073 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_K_phi(float* K_phi_in, size_t q_in, size_t n_in){
00074     // K_phi_in is the pointer of a mutidimentional array with size q_in by n_in
00075     if (q != q_in || n != n_in){
00076         q = q_in;
00077         n = n_in;
00078         zeros_q.resize(q, 0.0);
00079         zeros_n.resize(n, 0.0);
00080         K_phi.assign(q, zeros_n);
00081     }
00082     //
00083     for (size_t i = 0; i < q; ++i){
00084         for (size_t j = 0; j < n; ++j){
00085             // K_phi[i][j] = K_phi_in[i][j];
00086             K_phi[i][j] = *K_phi_in;
00087             K_phi_in++;
00088         }
00089     }
00090 }
00091 //
00092 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_N_xd(float* N_xd_in, size_t n_in, size_t p_in){
00093     // N_xd_in is the pointer of a mutidimentional array with size n_in by p_in
00094     if (n != n_in || p != p_in){
00095         n = n_in;
00096         p = p_in;
00097         zeros_n.resize(n, 0.0);
00098         zeros_p.resize(p, 0.0);
00099         N_xd.assign(n, zeros_p);
00100     }
00101     //
00102     for (size_t i = 0; i < n; ++i){
00103         for (size_t j = 0; j < p; ++j){
00104             // N_xd[i][j] = N_xd_in[i][j];
00105             N_xd[i][j] = *N_xd_in;
00106             N_xd_in++;
00107         }
00108     }
00109 }
00110 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_N_ud(float* N_ud_in, size_t p_in){
00111     // N_ud_in is the pointer of a mutidimentional array with size p_in by p_in
00112     if (p != p_in){
00113         p = p_in;
00114         zeros_p.resize(p, 0.0);
00115         N_ud.assign(p, zeros_p);
00116     }
00117     //
00118     for (size_t i = 0; i < p; ++i){
00119         for (size_t j = 0; j < p; ++j){
00120             // N_ud[i][j] = N_ud_in[i][j];
00121             N_ud[i][j] = *N_ud_in;
00122             N_ud_in++;
00123         }
00124     }
00125 }
00126 //
00127 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::iterateOnce(bool enable){
00128     //----------------------------//
00129     // Input: sys_output ("y"), command ("r")
00130     // Get: sys_inputs ("u")
00131     //----------------------------//
00132 
00133     // Control law
00134     if (enable){
00135         //
00136         get_inputs_compensate(); // Get states_d, inputs_d from command
00137 
00138         // states_est = (sys_output - C_error*MeasurementErrors) - states_d
00139         get_states_est();
00140 
00141         // sys_inputs = inputs_d - K_full*states_est
00142         sys_inputs = Get_VectorPlus(inputs_d, Mat_multiply_Vec(K_full, states_est),true); // minus
00143     }else{
00144         sys_inputs = zeros_p;
00145     }
00146 
00147     // Integral action
00148     get_MeasurementErrors_est(enable);
00149 }
00150 
00151 // Private functions
00152 // Command (equalibrium state) related calculation
00153 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_inputs_compensate(void){ // Calculate the compensation variable, states_d and sys_inputs_compensate
00154     //
00155     Mat_multiply_Vec(states_d, N_xd, command);
00156     Mat_multiply_Vec(inputs_d, N_ud, command);
00157 }
00158 // Calculate the states_est
00159 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_states_est(void){ // Calculate the sys_outputs from states_est, by mutiplying C_error
00160     // states_est = (sys_output - C_error*MeasurementErrors) - states_d
00161     states_est = Get_VectorPlus(Get_VectorPlus(sys_output, Mat_multiply_Vec(C_error, MeasurementErrors), true), states_d, true); // minus
00162 }
00163 // Calculate the estimation of MeasurementErrors
00164 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_MeasurementErrors_est(bool enable){ // Calculate the MeasurementErrors
00165     //
00166     // Integral action
00167     // MeasurementErrors -= Ts*(K_phi*states_est)
00168     if (enable){
00169         Get_VectorIncrement(MeasurementErrors, Get_VectorScalarMultiply(Mat_multiply_Vec(K_phi,states_est), Ts) , true); // -=
00170     }else{
00171         MeasurementErrors = zeros_q;
00172     }
00173 }
00174 // Utilities
00175 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
00176     static vector<float>::iterator it_out;
00177     static vector<const float>::iterator it_m_row;
00178     static vector<const float>::iterator it_v;
00179     //
00180     it_out = v_out.begin();
00181     for (size_t i = 0; i < m_left.size(); ++i){
00182         *it_out = 0.0;
00183         it_m_row = m_left[i].begin();
00184         it_v = v_right.begin();
00185         for (size_t j = 0; j < m_left[i].size(); ++j){
00186             // *it_out += m_left[i][j] * v_right[j];
00187             if (*it_m_row != 0.0 && *it_v != 0.0){
00188                 (*it_out) += (*it_m_row) * (*it_v);
00189             }else{
00190                 // (*it_out) += 0.0
00191             }
00192             // (*it_out) += (*it_m_row) * (*it_v);
00193             //
00194             it_m_row++;
00195             it_v++;
00196         }
00197         it_out++;
00198     }
00199 }
00200 vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
00201     static vector<float> v_out;
00202     // Size check
00203     if (v_out.size() != m_left.size()){
00204         v_out.resize(m_left.size());
00205     }
00206     // Iterators
00207     static vector<float>::iterator it_out;
00208     static vector<const float>::iterator it_m_row;
00209     static vector<const float>::iterator it_v;
00210     //
00211     it_out = v_out.begin();
00212     for (size_t i = 0; i < m_left.size(); ++i){
00213         *it_out = 0.0;
00214         it_m_row = m_left[i].begin();
00215         it_v = v_right.begin();
00216         for (size_t j = 0; j < m_left[i].size(); ++j){
00217             // *it_out += m_left[i][j] * v_right[j];
00218             if (*it_m_row != 0.0 && *it_v != 0.0){
00219                 (*it_out) += (*it_m_row) * (*it_v);
00220             }else{
00221                 // (*it_out) += 0.0
00222             }
00223             // (*it_out) += (*it_m_row) * (*it_v);
00224             //
00225             it_m_row++;
00226             it_v++;
00227         }
00228         it_out++;
00229     }
00230     return v_out;
00231 }
00232 vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
00233 {
00234     static vector<float> v_c;
00235     // Size check
00236     if (v_c.size() != v_a.size()){
00237         v_c.resize(v_a.size());
00238     }
00239     //
00240     for (size_t i = 0; i < v_a.size(); ++i){
00241         if (is_minus){
00242             v_c[i] = v_a[i] - v_b[i];
00243         }else{
00244             v_c[i] = v_a[i] + v_b[i];
00245         }
00246     }
00247     return v_c;
00248 }
00249 vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
00250 {
00251     static vector<float> v_c;
00252     // Size check
00253     if (v_c.size() != v_a.size()){
00254         v_c.resize(v_a.size());
00255     }
00256     // for pure negative
00257     if (scale == -1.0){
00258         for (size_t i = 0; i < v_a.size(); ++i){
00259             v_c[i] = -v_a[i];
00260         }
00261         return v_c;
00262     }
00263     // else
00264     for (size_t i = 0; i < v_a.size(); ++i){
00265         v_c[i] = scale*v_a[i];
00266 
00267     }
00268     return v_c;
00269 }
00270 // Increment
00271 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
00272     // Size check
00273     if (v_a.size() != v_b.size()){
00274         v_a.resize(v_b.size());
00275     }
00276     //
00277     if (is_minus){ // -=
00278         for (size_t i = 0; i < v_b.size(); ++i){
00279             v_a[i] -= v_b[i];
00280         }
00281     }else{ // +=
00282         for (size_t i = 0; i < v_b.size(); ++i){
00283             v_a[i] += v_b[i];
00284         }
00285     }
00286 
00287 }