Chun Feng Huang / MEASUREMENT_ERROR_ADAPTATION_CONTROL

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

MEASUREMENT_ERROR_ADAPTATION_CONTROL.cpp@0:533d5685b66c, 2017-01-11 (annotated)

Committer:
benson516
Date:
Wed Jan 11 09:32:26 2017 +0000
Revision:
0:533d5685b66c
First compilation of this library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:533d5685b66c 1 #include "MEASUREMENT_ERROR_ADAPTATION_CONTROL.h"
benson516 0:533d5685b66c 2 // The controller is for the plant with (p, n, q) system
benson516 0:533d5685b66c 3 // Dimensions:
benson516 0:533d5685b66c 4 //
benson516 0:533d5685b66c 5 // Inputs, u | States, x
benson516 0:533d5685b66c 6 // p ----> n
benson516 0:533d5685b66c 7 // q -- ^
benson516 0:533d5685b66c 8 // Measurement errors, phi
benson516 0:533d5685b66c 9
benson516 0:533d5685b66c 10 MEASUREMENT_ERROR_ADAPTATION_CONTROL::MEASUREMENT_ERROR_ADAPTATION_CONTROL(size_t num_state, size_t num_in, size_t num_MeasurementError, float samplingTime):
benson516 0:533d5685b66c 11 n(num_state), p(num_in), q(num_MeasurementError), Ts(samplingTime),
benson516 0:533d5685b66c 12 C_error(num_state, vector<float>(num_MeasurementError,0.0)),
benson516 0:533d5685b66c 13 K_full(num_in, vector<float>(num_state,0.0)),
benson516 0:533d5685b66c 14 K_phi(num_MeasurementError, vector<float>(num_state,0.0)),
benson516 0:533d5685b66c 15 N_xd(num_state, vector<float>(num_in, 0.0)),
benson516 0:533d5685b66c 16 N_ud(num_in, vector<float>(num_in, 0.0))
benson516 0:533d5685b66c 17 {
benson516 0:533d5685b66c 18 //
benson516 0:533d5685b66c 19 zeros_n.assign(n, 0.0);
benson516 0:533d5685b66c 20 zeros_p.assign(p, 0.0);
benson516 0:533d5685b66c 21 zeros_q.assign(q, 0.0);
benson516 0:533d5685b66c 22
benson516 0:533d5685b66c 23 // States
benson516 0:533d5685b66c 24 states_est = zeros_n; // states_est, "x_est"
benson516 0:533d5685b66c 25 sys_inputs = zeros_p; // The inputs of the plant, "u"
benson516 0:533d5685b66c 26 sys_output = zeros_n; // The output of the plant, "y"
benson516 0:533d5685b66c 27 MeasurementErrors = zeros_q; // The measurement error of the sensors, "phi"
benson516 0:533d5685b66c 28
benson516 0:533d5685b66c 29 // Command (equalibrium state)
benson516 0:533d5685b66c 30 states_d = zeros_n; // x_d
benson516 0:533d5685b66c 31 inputs_d = zeros_p; // u_d
benson516 0:533d5685b66c 32 command = zeros_p; // r
benson516 0:533d5685b66c 33
benson516 0:533d5685b66c 34
benson516 0:533d5685b66c 35 }
benson516 0:533d5685b66c 36 // Assign Parameters
benson516 0:533d5685b66c 37 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_C_error(float* C_error_in, size_t n_in, size_t q_in){
benson516 0:533d5685b66c 38 // C_error_in is the pointer of a mutidimentional array with size n_in by q_in
benson516 0:533d5685b66c 39 if (n != n_in || q != q_in){
benson516 0:533d5685b66c 40 n = n_in;
benson516 0:533d5685b66c 41 q = q_in;
benson516 0:533d5685b66c 42 zeros_n.resize(n, 0.0);
benson516 0:533d5685b66c 43 zeros_q.resize(q, 0.0);
benson516 0:533d5685b66c 44 C_error.assign(n, zeros_q);
benson516 0:533d5685b66c 45 }
benson516 0:533d5685b66c 46 //
benson516 0:533d5685b66c 47 for (size_t i = 0; i < n; ++i){
benson516 0:533d5685b66c 48 for (size_t j = 0; j < q; ++j){
benson516 0:533d5685b66c 49 // C_error[i][j] = C_error_in[i][j];
benson516 0:533d5685b66c 50 C_error[i][j] = *C_error_in;
benson516 0:533d5685b66c 51 C_error_in++;
benson516 0:533d5685b66c 52 }
benson516 0:533d5685b66c 53 }
benson516 0:533d5685b66c 54 }
benson516 0:533d5685b66c 55 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_K_full(float* K_full_in, size_t p_in, size_t n_in){
benson516 0:533d5685b66c 56 // K_full_in is the pointer of a mutidimentional array with size p_in by n_in
benson516 0:533d5685b66c 57 if (n != n_in || p != p_in){
benson516 0:533d5685b66c 58 p = p_in;
benson516 0:533d5685b66c 59 n = n_in;
benson516 0:533d5685b66c 60 zeros_n.resize(n, 0.0);
benson516 0:533d5685b66c 61 zeros_p.resize(p, 0.0);
benson516 0:533d5685b66c 62 K_full.assign(p, zeros_n);
benson516 0:533d5685b66c 63 }
benson516 0:533d5685b66c 64 //
benson516 0:533d5685b66c 65 for (size_t i = 0; i < p; ++i){
benson516 0:533d5685b66c 66 for (size_t j = 0; j < n; ++j){
benson516 0:533d5685b66c 67 // K_full[i][j] = K_full_in[i][j];
benson516 0:533d5685b66c 68 K_full[i][j] = *K_full_in;
benson516 0:533d5685b66c 69 K_full_in++;
benson516 0:533d5685b66c 70 }
benson516 0:533d5685b66c 71 }
benson516 0:533d5685b66c 72 }
benson516 0:533d5685b66c 73 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_K_phi(float* K_phi_in, size_t q_in, size_t n_in){
benson516 0:533d5685b66c 74 // K_phi_in is the pointer of a mutidimentional array with size q_in by n_in
benson516 0:533d5685b66c 75 if (q != q_in || n != n_in){
benson516 0:533d5685b66c 76 q = q_in;
benson516 0:533d5685b66c 77 n = n_in;
benson516 0:533d5685b66c 78 zeros_q.resize(q, 0.0);
benson516 0:533d5685b66c 79 zeros_n.resize(n, 0.0);
benson516 0:533d5685b66c 80 K_phi.assign(q, zeros_n);
benson516 0:533d5685b66c 81 }
benson516 0:533d5685b66c 82 //
benson516 0:533d5685b66c 83 for (size_t i = 0; i < q; ++i){
benson516 0:533d5685b66c 84 for (size_t j = 0; j < n; ++j){
benson516 0:533d5685b66c 85 // K_phi[i][j] = K_phi_in[i][j];
benson516 0:533d5685b66c 86 K_phi[i][j] = *K_phi_in;
benson516 0:533d5685b66c 87 K_phi_in++;
benson516 0:533d5685b66c 88 }
benson516 0:533d5685b66c 89 }
benson516 0:533d5685b66c 90 }
benson516 0:533d5685b66c 91 //
benson516 0:533d5685b66c 92 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_N_xd(float* N_xd_in, size_t n_in, size_t p_in){
benson516 0:533d5685b66c 93 // N_xd_in is the pointer of a mutidimentional array with size n_in by p_in
benson516 0:533d5685b66c 94 if (n != n_in || p != p_in){
benson516 0:533d5685b66c 95 n = n_in;
benson516 0:533d5685b66c 96 p = p_in;
benson516 0:533d5685b66c 97 zeros_n.resize(n, 0.0);
benson516 0:533d5685b66c 98 zeros_p.resize(p, 0.0);
benson516 0:533d5685b66c 99 N_xd.assign(n, zeros_p);
benson516 0:533d5685b66c 100 }
benson516 0:533d5685b66c 101 //
benson516 0:533d5685b66c 102 for (size_t i = 0; i < n; ++i){
benson516 0:533d5685b66c 103 for (size_t j = 0; j < p; ++j){
benson516 0:533d5685b66c 104 // N_xd[i][j] = N_xd_in[i][j];
benson516 0:533d5685b66c 105 N_xd[i][j] = *N_xd_in;
benson516 0:533d5685b66c 106 N_xd_in++;
benson516 0:533d5685b66c 107 }
benson516 0:533d5685b66c 108 }
benson516 0:533d5685b66c 109 }
benson516 0:533d5685b66c 110 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::assign_N_ud(float* N_ud_in, size_t p_in){
benson516 0:533d5685b66c 111 // N_ud_in is the pointer of a mutidimentional array with size p_in by p_in
benson516 0:533d5685b66c 112 if (p != p_in){
benson516 0:533d5685b66c 113 p = p_in;
benson516 0:533d5685b66c 114 zeros_p.resize(p, 0.0);
benson516 0:533d5685b66c 115 N_ud.assign(p, zeros_p);
benson516 0:533d5685b66c 116 }
benson516 0:533d5685b66c 117 //
benson516 0:533d5685b66c 118 for (size_t i = 0; i < p; ++i){
benson516 0:533d5685b66c 119 for (size_t j = 0; j < p; ++j){
benson516 0:533d5685b66c 120 // N_ud[i][j] = N_ud_in[i][j];
benson516 0:533d5685b66c 121 N_ud[i][j] = *N_ud_in;
benson516 0:533d5685b66c 122 N_ud_in++;
benson516 0:533d5685b66c 123 }
benson516 0:533d5685b66c 124 }
benson516 0:533d5685b66c 125 }
benson516 0:533d5685b66c 126 //
benson516 0:533d5685b66c 127 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::iterateOnce(bool enable){
benson516 0:533d5685b66c 128 //----------------------------//
benson516 0:533d5685b66c 129 // Input: sys_output ("y"), command ("r")
benson516 0:533d5685b66c 130 // Get: sys_inputs ("u")
benson516 0:533d5685b66c 131 //----------------------------//
benson516 0:533d5685b66c 132
benson516 0:533d5685b66c 133 // Control law
benson516 0:533d5685b66c 134 if (enable){
benson516 0:533d5685b66c 135 //
benson516 0:533d5685b66c 136 get_inputs_compensate(); // Get states_d, inputs_d from command
benson516 0:533d5685b66c 137
benson516 0:533d5685b66c 138 // states_est = (sys_output - C_error*MeasurementErrors) - states_d
benson516 0:533d5685b66c 139 get_states_est();
benson516 0:533d5685b66c 140
benson516 0:533d5685b66c 141 // sys_inputs = inputs_d - K_full*states_est
benson516 0:533d5685b66c 142 sys_inputs = Get_VectorPlus(inputs_d, Mat_multiply_Vec(K_full, states_est),true); // minus
benson516 0:533d5685b66c 143 }else{
benson516 0:533d5685b66c 144 sys_inputs = zeros_p;
benson516 0:533d5685b66c 145 }
benson516 0:533d5685b66c 146
benson516 0:533d5685b66c 147 // Integral action
benson516 0:533d5685b66c 148 get_MeasurementErrors_est(enable);
benson516 0:533d5685b66c 149 }
benson516 0:533d5685b66c 150
benson516 0:533d5685b66c 151 // Private functions
benson516 0:533d5685b66c 152 // Command (equalibrium state) related calculation
benson516 0:533d5685b66c 153 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_inputs_compensate(void){ // Calculate the compensation variable, states_d and sys_inputs_compensate
benson516 0:533d5685b66c 154 //
benson516 0:533d5685b66c 155 Mat_multiply_Vec(states_d, N_xd, command);
benson516 0:533d5685b66c 156 Mat_multiply_Vec(inputs_d, N_ud, command);
benson516 0:533d5685b66c 157 }
benson516 0:533d5685b66c 158 // Calculate the states_est
benson516 0:533d5685b66c 159 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_states_est(void){ // Calculate the sys_outputs from states_est, by mutiplying C_error
benson516 0:533d5685b66c 160 // states_est = (sys_output - C_error*MeasurementErrors) - states_d
benson516 0:533d5685b66c 161 states_est = Get_VectorPlus(Get_VectorPlus(sys_output, Mat_multiply_Vec(C_error, MeasurementErrors), true), states_d, true); // minus
benson516 0:533d5685b66c 162 }
benson516 0:533d5685b66c 163 // Calculate the estimation of MeasurementErrors
benson516 0:533d5685b66c 164 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::get_MeasurementErrors_est(bool enable){ // Calculate the MeasurementErrors
benson516 0:533d5685b66c 165 //
benson516 0:533d5685b66c 166 // Integral action
benson516 0:533d5685b66c 167 // MeasurementErrors -= Ts*(K_phi*states_est)
benson516 0:533d5685b66c 168 if (enable){
benson516 0:533d5685b66c 169 Get_VectorIncrement(MeasurementErrors, Get_VectorScalarMultiply(Mat_multiply_Vec(K_phi,states_est), Ts) , true); // -=
benson516 0:533d5685b66c 170 }else{
benson516 0:533d5685b66c 171 MeasurementErrors = zeros_q;
benson516 0:533d5685b66c 172 }
benson516 0:533d5685b66c 173 }
benson516 0:533d5685b66c 174 // Utilities
benson516 0:533d5685b66c 175 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
benson516 0:533d5685b66c 176 static vector<float>::iterator it_out;
benson516 0:533d5685b66c 177 static vector<const float>::iterator it_m_row;
benson516 0:533d5685b66c 178 static vector<const float>::iterator it_v;
benson516 0:533d5685b66c 179 //
benson516 0:533d5685b66c 180 it_out = v_out.begin();
benson516 0:533d5685b66c 181 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:533d5685b66c 182 *it_out = 0.0;
benson516 0:533d5685b66c 183 it_m_row = m_left[i].begin();
benson516 0:533d5685b66c 184 it_v = v_right.begin();
benson516 0:533d5685b66c 185 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:533d5685b66c 186 // *it_out += m_left[i][j] * v_right[j];
benson516 0:533d5685b66c 187 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:533d5685b66c 188 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:533d5685b66c 189 }else{
benson516 0:533d5685b66c 190 // (*it_out) += 0.0
benson516 0:533d5685b66c 191 }
benson516 0:533d5685b66c 192 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:533d5685b66c 193 //
benson516 0:533d5685b66c 194 it_m_row++;
benson516 0:533d5685b66c 195 it_v++;
benson516 0:533d5685b66c 196 }
benson516 0:533d5685b66c 197 it_out++;
benson516 0:533d5685b66c 198 }
benson516 0:533d5685b66c 199 }
benson516 0:533d5685b66c 200 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
benson516 0:533d5685b66c 201 static vector<float> v_out;
benson516 0:533d5685b66c 202 // Size check
benson516 0:533d5685b66c 203 if (v_out.size() != m_left.size()){
benson516 0:533d5685b66c 204 v_out.resize(m_left.size());
benson516 0:533d5685b66c 205 }
benson516 0:533d5685b66c 206 // Iterators
benson516 0:533d5685b66c 207 static vector<float>::iterator it_out;
benson516 0:533d5685b66c 208 static vector<const float>::iterator it_m_row;
benson516 0:533d5685b66c 209 static vector<const float>::iterator it_v;
benson516 0:533d5685b66c 210 //
benson516 0:533d5685b66c 211 it_out = v_out.begin();
benson516 0:533d5685b66c 212 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:533d5685b66c 213 *it_out = 0.0;
benson516 0:533d5685b66c 214 it_m_row = m_left[i].begin();
benson516 0:533d5685b66c 215 it_v = v_right.begin();
benson516 0:533d5685b66c 216 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:533d5685b66c 217 // *it_out += m_left[i][j] * v_right[j];
benson516 0:533d5685b66c 218 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:533d5685b66c 219 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:533d5685b66c 220 }else{
benson516 0:533d5685b66c 221 // (*it_out) += 0.0
benson516 0:533d5685b66c 222 }
benson516 0:533d5685b66c 223 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:533d5685b66c 224 //
benson516 0:533d5685b66c 225 it_m_row++;
benson516 0:533d5685b66c 226 it_v++;
benson516 0:533d5685b66c 227 }
benson516 0:533d5685b66c 228 it_out++;
benson516 0:533d5685b66c 229 }
benson516 0:533d5685b66c 230 return v_out;
benson516 0:533d5685b66c 231 }
benson516 0:533d5685b66c 232 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
benson516 0:533d5685b66c 233 {
benson516 0:533d5685b66c 234 static vector<float> v_c;
benson516 0:533d5685b66c 235 // Size check
benson516 0:533d5685b66c 236 if (v_c.size() != v_a.size()){
benson516 0:533d5685b66c 237 v_c.resize(v_a.size());
benson516 0:533d5685b66c 238 }
benson516 0:533d5685b66c 239 //
benson516 0:533d5685b66c 240 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:533d5685b66c 241 if (is_minus){
benson516 0:533d5685b66c 242 v_c[i] = v_a[i] - v_b[i];
benson516 0:533d5685b66c 243 }else{
benson516 0:533d5685b66c 244 v_c[i] = v_a[i] + v_b[i];
benson516 0:533d5685b66c 245 }
benson516 0:533d5685b66c 246 }
benson516 0:533d5685b66c 247 return v_c;
benson516 0:533d5685b66c 248 }
benson516 0:533d5685b66c 249 vector<float> MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 0:533d5685b66c 250 {
benson516 0:533d5685b66c 251 static vector<float> v_c;
benson516 0:533d5685b66c 252 // Size check
benson516 0:533d5685b66c 253 if (v_c.size() != v_a.size()){
benson516 0:533d5685b66c 254 v_c.resize(v_a.size());
benson516 0:533d5685b66c 255 }
benson516 0:533d5685b66c 256 // for pure negative
benson516 0:533d5685b66c 257 if (scale == -1.0){
benson516 0:533d5685b66c 258 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:533d5685b66c 259 v_c[i] = -v_a[i];
benson516 0:533d5685b66c 260 }
benson516 0:533d5685b66c 261 return v_c;
benson516 0:533d5685b66c 262 }
benson516 0:533d5685b66c 263 // else
benson516 0:533d5685b66c 264 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:533d5685b66c 265 v_c[i] = scale*v_a[i];
benson516 0:533d5685b66c 266
benson516 0:533d5685b66c 267 }
benson516 0:533d5685b66c 268 return v_c;
benson516 0:533d5685b66c 269 }
benson516 0:533d5685b66c 270 // Increment
benson516 0:533d5685b66c 271 void MEASUREMENT_ERROR_ADAPTATION_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
benson516 0:533d5685b66c 272 // Size check
benson516 0:533d5685b66c 273 if (v_a.size() != v_b.size()){
benson516 0:533d5685b66c 274 v_a.resize(v_b.size());
benson516 0:533d5685b66c 275 }
benson516 0:533d5685b66c 276 //
benson516 0:533d5685b66c 277 if (is_minus){ // -=
benson516 0:533d5685b66c 278 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:533d5685b66c 279 v_a[i] -= v_b[i];
benson516 0:533d5685b66c 280 }
benson516 0:533d5685b66c 281 }else{ // +=
benson516 0:533d5685b66c 282 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:533d5685b66c 283 v_a[i] += v_b[i];
benson516 0:533d5685b66c 284 }
benson516 0:533d5685b66c 285 }
benson516 0:533d5685b66c 286
benson516 0:533d5685b66c 287 }