Project_WIPV_antiSlip / STATE_FEEDBACK

The generic full-state feedback control libery

Fork of STATE_FEEDBACK by Project_WIPV_antiSlip

STATE_FEEDBACK.cpp@4:958c25c1b151, 2017-01-04 (annotated)

Committer:
benson516
Date:
Wed Jan 04 10:09:44 2017 +0000
Revision:
4:958c25c1b151
Parent:
3:7ff53317e0a4 
Add "enable" state control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:8dbe2a4687b8 1 #include "STATE_FEEDBACK.h"
benson516 2:0544e16ea933 2 // The controller is for the plant with (p, n, q) system
benson516 2:0544e16ea933 3 // Dimensions:
benson516 2:0544e16ea933 4 //
benson516 2:0544e16ea933 5 // Inputs, u | States, x | outputs, y
benson516 2:0544e16ea933 6 // p --> n --> q
benson516 2:0544e16ea933 7 //
benson516 2:0544e16ea933 8 //
benson516 0:8dbe2a4687b8 9
benson516 0:8dbe2a4687b8 10 STATE_FEEDBACK::STATE_FEEDBACK(size_t num_state, size_t num_in, size_t num_out, float samplingTime):
benson516 2:0544e16ea933 11 n(num_state), p(num_in), q(num_out), Ts(samplingTime),
benson516 2:0544e16ea933 12 K_full(num_in, vector<float>(num_state,0.0)),
benson516 2:0544e16ea933 13 // N_xd(num_state, vector<float>(num_out, 0.0)),
benson516 2:0544e16ea933 14 // N_ud(num_in, vector<float>(num_out, 0.0)),
benson516 2:0544e16ea933 15 N_total(num_in, vector<float>(num_out,0.0))
benson516 0:8dbe2a4687b8 16 {
benson516 2:0544e16ea933 17 // Normally, q = p
benson516 2:0544e16ea933 18 if (q > p)
benson516 2:0544e16ea933 19 q = p;
benson516 2:0544e16ea933 20 //
benson516 0:8dbe2a4687b8 21 zeros_n.assign(n, 0.0);
benson516 0:8dbe2a4687b8 22 zeros_p.assign(p, 0.0);
benson516 0:8dbe2a4687b8 23 zeros_q.assign(q, 0.0);
benson516 0:8dbe2a4687b8 24 //
benson516 0:8dbe2a4687b8 25 states = zeros_n;
benson516 0:8dbe2a4687b8 26 sys_inputs = zeros_p;
benson516 0:8dbe2a4687b8 27 sys_outputs = zeros_q;
benson516 0:8dbe2a4687b8 28
benson516 2:0544e16ea933 29 // Command (equalibrium state)
benson516 2:0544e16ea933 30 // states_d = zeros_n;
benson516 2:0544e16ea933 31 // inputs_d = zeros_p;
benson516 2:0544e16ea933 32 sys_inputs_compensate = zeros_p;
benson516 2:0544e16ea933 33 command = zeros_q; // q = p
benson516 2:0544e16ea933 34
benson516 2:0544e16ea933 35
benson516 0:8dbe2a4687b8 36 }
benson516 1:cdd434f6aa9a 37 // Assign Parameters
benson516 1:cdd434f6aa9a 38 void STATE_FEEDBACK::assign_K_full(float* K_full_in, size_t p_in, size_t n_in){
benson516 1:cdd434f6aa9a 39 // K_full_in is the pointer of a mutidimentional array with size p_in by n_in
benson516 1:cdd434f6aa9a 40 if (n != n_in || p != p_in){
benson516 1:cdd434f6aa9a 41 n = n_in;
benson516 1:cdd434f6aa9a 42 p = p_in;
benson516 1:cdd434f6aa9a 43 zeros_n.resize(n, 0.0);
benson516 1:cdd434f6aa9a 44 zeros_p.resize(p, 0.0);
benson516 1:cdd434f6aa9a 45 K_full.assign(p, zeros_n);
benson516 1:cdd434f6aa9a 46 }
benson516 1:cdd434f6aa9a 47 //
benson516 1:cdd434f6aa9a 48 for (size_t i = 0; i < p; ++i){
benson516 1:cdd434f6aa9a 49 for (size_t j = 0; j < n; ++j){
benson516 1:cdd434f6aa9a 50 // K_full[i][j] = K_full_in[i][j];
benson516 1:cdd434f6aa9a 51 K_full[i][j] = *K_full_in;
benson516 1:cdd434f6aa9a 52 K_full_in++;
benson516 1:cdd434f6aa9a 53 }
benson516 1:cdd434f6aa9a 54 }
benson516 1:cdd434f6aa9a 55 }
benson516 2:0544e16ea933 56 /*
benson516 2:0544e16ea933 57 void STATE_FEEDBACK::assign_N_xd(float* N_xd_in, size_t n_in, size_t q_in){
benson516 2:0544e16ea933 58 // N_xd_in is the pointer of a mutidimentional array with size n_in by q_in
benson516 2:0544e16ea933 59 if (n != n_in || q != q_in){
benson516 2:0544e16ea933 60 n = n_in;
benson516 2:0544e16ea933 61 q = q_in;
benson516 2:0544e16ea933 62 zeros_n.resize(n, 0.0);
benson516 2:0544e16ea933 63 zeros_q.resize(q, 0.0);
benson516 2:0544e16ea933 64 N_xd.assign(n, zeros_q);
benson516 2:0544e16ea933 65 }
benson516 2:0544e16ea933 66 //
benson516 2:0544e16ea933 67 for (size_t i = 0; i < n; ++i){
benson516 2:0544e16ea933 68 for (size_t j = 0; j < q; ++j){
benson516 2:0544e16ea933 69 // N_xd[i][j] = N_xd_in[i][j];
benson516 2:0544e16ea933 70 N_xd[i][j] = *N_xd_in;
benson516 2:0544e16ea933 71 N_xd_in++;
benson516 2:0544e16ea933 72 }
benson516 2:0544e16ea933 73 }
benson516 2:0544e16ea933 74 }
benson516 2:0544e16ea933 75 void STATE_FEEDBACK::assign_N_ud(float* N_ud_in, size_t p_in, size_t q_in){
benson516 2:0544e16ea933 76 // N_ud_in is the pointer of a mutidimentional array with size p_in by q_in
benson516 2:0544e16ea933 77 if (p != p_in || q != q_in){
benson516 2:0544e16ea933 78 p = p_in;
benson516 2:0544e16ea933 79 q = q_in;
benson516 2:0544e16ea933 80 zeros_p.resize(p, 0.0);
benson516 2:0544e16ea933 81 zeros_q.resize(q, 0.0);
benson516 2:0544e16ea933 82 N_ud.assign(p, zeros_q);
benson516 2:0544e16ea933 83 }
benson516 2:0544e16ea933 84 //
benson516 2:0544e16ea933 85 for (size_t i = 0; i < p; ++i){
benson516 2:0544e16ea933 86 for (size_t j = 0; j < q; ++j){
benson516 2:0544e16ea933 87 // N_ud[i][j] = N_ud_in[i][j];
benson516 2:0544e16ea933 88 N_ud[i][j] = *N_ud_in;
benson516 2:0544e16ea933 89 N_ud_in++;
benson516 2:0544e16ea933 90 }
benson516 2:0544e16ea933 91 }
benson516 2:0544e16ea933 92 }
benson516 2:0544e16ea933 93 */
benson516 2:0544e16ea933 94 void STATE_FEEDBACK::assign_N_total(float* N_total_in, size_t p_in, size_t q_in){
benson516 2:0544e16ea933 95 // N_total_in is the pointer of a mutidimentional array with size p_in by q_in
benson516 2:0544e16ea933 96 if (p != p_in || q != q_in){
benson516 2:0544e16ea933 97 p = p_in;
benson516 2:0544e16ea933 98 q = q_in;
benson516 2:0544e16ea933 99 zeros_p.resize(p, 0.0);
benson516 2:0544e16ea933 100 zeros_q.resize(q, 0.0);
benson516 2:0544e16ea933 101 N_total.assign(p, zeros_q);
benson516 2:0544e16ea933 102 }
benson516 2:0544e16ea933 103 //
benson516 2:0544e16ea933 104 for (size_t i = 0; i < p; ++i){
benson516 2:0544e16ea933 105 for (size_t j = 0; j < q; ++j){
benson516 2:0544e16ea933 106 // N_total[i][j] = N_total_in[i][j];
benson516 2:0544e16ea933 107 N_total[i][j] = *N_total_in;
benson516 2:0544e16ea933 108 N_total_in++;
benson516 2:0544e16ea933 109 }
benson516 2:0544e16ea933 110 }
benson516 2:0544e16ea933 111 }
benson516 4:958c25c1b151 112 void STATE_FEEDBACK::fullStateFeedBack_calc(bool enable){
benson516 4:958c25c1b151 113 if (!enable){
benson516 4:958c25c1b151 114 sys_inputs = zeros_p;
benson516 4:958c25c1b151 115 return;
benson516 4:958c25c1b151 116 }
benson516 2:0544e16ea933 117 // sys_inputs = Get_VectorScalarMultiply(Mat_multiply_Vec(K_full, states),-1.0);
benson516 2:0544e16ea933 118 //
benson516 2:0544e16ea933 119 // With command input
benson516 3:7ff53317e0a4 120 get_inputs_compensate();
benson516 2:0544e16ea933 121 // sys_inputs = Get_VectorPlus(inputs_d, Mat_multiply_Vec(K_full, Get_VectorPlus(states, states_d, true)), true); // minus
benson516 2:0544e16ea933 122 sys_inputs = Get_VectorPlus(sys_inputs_compensate, Mat_multiply_Vec(K_full, states), true); // minus
benson516 0:8dbe2a4687b8 123 }
benson516 1:cdd434f6aa9a 124
benson516 2:0544e16ea933 125 // Private functions
benson516 2:0544e16ea933 126 // Command (equalibrium state) related calculation
benson516 3:7ff53317e0a4 127 void STATE_FEEDBACK::get_inputs_compensate(void){ // Calculate the compensation variable, states_d and sys_inputs_compensate
benson516 2:0544e16ea933 128 // Mat_multiply_Vec(states_d, N_xd, command);
benson516 2:0544e16ea933 129 // Mat_multiply_Vec(inputs_d, N_ud, command);
benson516 2:0544e16ea933 130 //
benson516 2:0544e16ea933 131 Mat_multiply_Vec(sys_inputs_compensate, N_total, command);
benson516 2:0544e16ea933 132
benson516 2:0544e16ea933 133 }
benson516 1:cdd434f6aa9a 134 // Utilities
benson516 0:8dbe2a4687b8 135 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
benson516 1:cdd434f6aa9a 136 static vector<float>::iterator it_out;
benson516 1:cdd434f6aa9a 137 static vector<const float>::iterator it_m_row;
benson516 1:cdd434f6aa9a 138 static vector<const float>::iterator it_v;
benson516 0:8dbe2a4687b8 139 //
benson516 0:8dbe2a4687b8 140 it_out = v_out.begin();
benson516 0:8dbe2a4687b8 141 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:8dbe2a4687b8 142 *it_out = 0.0;
benson516 0:8dbe2a4687b8 143 it_m_row = m_left[i].begin();
benson516 0:8dbe2a4687b8 144 it_v = v_right.begin();
benson516 0:8dbe2a4687b8 145 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:8dbe2a4687b8 146 // *it_out += m_left[i][j] * v_right[j];
benson516 2:0544e16ea933 147 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 2:0544e16ea933 148 (*it_out) += (*it_m_row) * (*it_v);
benson516 2:0544e16ea933 149 }else{
benson516 2:0544e16ea933 150 // (*it_out) += 0.0
benson516 2:0544e16ea933 151 }
benson516 2:0544e16ea933 152 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:8dbe2a4687b8 153 //
benson516 0:8dbe2a4687b8 154 it_m_row++;
benson516 0:8dbe2a4687b8 155 it_v++;
benson516 0:8dbe2a4687b8 156 }
benson516 0:8dbe2a4687b8 157 it_out++;
benson516 0:8dbe2a4687b8 158 }
benson516 0:8dbe2a4687b8 159 }
benson516 1:cdd434f6aa9a 160 vector<float> STATE_FEEDBACK::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 1:cdd434f6aa9a 161 static vector<float> v_out;
benson516 1:cdd434f6aa9a 162 // Size check
benson516 1:cdd434f6aa9a 163 if (v_out.size() != m_left.size()){
benson516 1:cdd434f6aa9a 164 v_out.resize(m_left.size());
benson516 1:cdd434f6aa9a 165 }
benson516 1:cdd434f6aa9a 166 // Iterators
benson516 1:cdd434f6aa9a 167 static vector<float>::iterator it_out;
benson516 1:cdd434f6aa9a 168 static vector<const float>::iterator it_m_row;
benson516 1:cdd434f6aa9a 169 static vector<const float>::iterator it_v;
benson516 1:cdd434f6aa9a 170 //
benson516 1:cdd434f6aa9a 171 it_out = v_out.begin();
benson516 1:cdd434f6aa9a 172 for (size_t i = 0; i < m_left.size(); ++i){
benson516 1:cdd434f6aa9a 173 *it_out = 0.0;
benson516 1:cdd434f6aa9a 174 it_m_row = m_left[i].begin();
benson516 1:cdd434f6aa9a 175 it_v = v_right.begin();
benson516 1:cdd434f6aa9a 176 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 1:cdd434f6aa9a 177 // *it_out += m_left[i][j] * v_right[j];
benson516 2:0544e16ea933 178 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 2:0544e16ea933 179 (*it_out) += (*it_m_row) * (*it_v);
benson516 2:0544e16ea933 180 }else{
benson516 2:0544e16ea933 181 // (*it_out) += 0.0
benson516 2:0544e16ea933 182 }
benson516 2:0544e16ea933 183 // (*it_out) += (*it_m_row) * (*it_v);
benson516 1:cdd434f6aa9a 184 //
benson516 1:cdd434f6aa9a 185 it_m_row++;
benson516 1:cdd434f6aa9a 186 it_v++;
benson516 1:cdd434f6aa9a 187 }
benson516 1:cdd434f6aa9a 188 it_out++;
benson516 1:cdd434f6aa9a 189 }
benson516 1:cdd434f6aa9a 190 return v_out;
benson516 1:cdd434f6aa9a 191 }
benson516 1:cdd434f6aa9a 192 vector<float> STATE_FEEDBACK::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
benson516 1:cdd434f6aa9a 193 {
benson516 1:cdd434f6aa9a 194 static vector<float> v_c;
benson516 1:cdd434f6aa9a 195 // Size check
benson516 1:cdd434f6aa9a 196 if (v_c.size() != v_a.size()){
benson516 1:cdd434f6aa9a 197 v_c.resize(v_a.size());
benson516 1:cdd434f6aa9a 198 }
benson516 1:cdd434f6aa9a 199 //
benson516 3:7ff53317e0a4 200 for (size_t i = 0; i < v_a.size(); ++i){
benson516 1:cdd434f6aa9a 201 if (is_minus){
benson516 1:cdd434f6aa9a 202 v_c[i] = v_a[i] - v_b[i];
benson516 1:cdd434f6aa9a 203 }else{
benson516 1:cdd434f6aa9a 204 v_c[i] = v_a[i] + v_b[i];
benson516 1:cdd434f6aa9a 205 }
benson516 1:cdd434f6aa9a 206 }
benson516 1:cdd434f6aa9a 207 return v_c;
benson516 1:cdd434f6aa9a 208 }
benson516 1:cdd434f6aa9a 209 vector<float> STATE_FEEDBACK::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 1:cdd434f6aa9a 210 {
benson516 1:cdd434f6aa9a 211 static vector<float> v_c;
benson516 1:cdd434f6aa9a 212 // Size check
benson516 1:cdd434f6aa9a 213 if (v_c.size() != v_a.size()){
benson516 1:cdd434f6aa9a 214 v_c.resize(v_a.size());
benson516 1:cdd434f6aa9a 215 }
benson516 1:cdd434f6aa9a 216 // for pure negative
benson516 1:cdd434f6aa9a 217 if (scale == -1.0){
benson516 3:7ff53317e0a4 218 for (size_t i = 0; i < v_a.size(); ++i){
benson516 1:cdd434f6aa9a 219 v_c[i] = -v_a[i];
benson516 1:cdd434f6aa9a 220 }
benson516 1:cdd434f6aa9a 221 return v_c;
benson516 1:cdd434f6aa9a 222 }
benson516 1:cdd434f6aa9a 223 // else
benson516 3:7ff53317e0a4 224 for (size_t i = 0; i < v_a.size(); ++i){
benson516 1:cdd434f6aa9a 225 v_c[i] = scale*v_a[i];
benson516 1:cdd434f6aa9a 226
benson516 1:cdd434f6aa9a 227 }
benson516 1:cdd434f6aa9a 228 return v_c;
benson516 1:cdd434f6aa9a 229 }