Project_WIPV_antiSlip / STATE_FEEDBACK

The generic full-state feedback control libery

Fork of STATE_FEEDBACK by Project_WIPV_antiSlip

STATE_FEEDBACK.cpp@2:0544e16ea933, 2016-12-29 (annotated)

Committer:
benson516
Date:
Thu Dec 29 16:40:52 2016 +0000
Revision:
2:0544e16ea933
Parent:
1:cdd434f6aa9a
Child:
3:7ff53317e0a4
Add the capability of tracking 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 0:8dbe2a4687b8 112 void STATE_FEEDBACK::fullStateFeedBack_calc(){
benson516 2:0544e16ea933 113 // sys_inputs = Get_VectorScalarMultiply(Mat_multiply_Vec(K_full, states),-1.0);
benson516 2:0544e16ea933 114 //
benson516 2:0544e16ea933 115 // With command input
benson516 2:0544e16ea933 116 get_Xd_ud();
benson516 2:0544e16ea933 117 // sys_inputs = Get_VectorPlus(inputs_d, Mat_multiply_Vec(K_full, Get_VectorPlus(states, states_d, true)), true); // minus
benson516 2:0544e16ea933 118 sys_inputs = Get_VectorPlus(sys_inputs_compensate, Mat_multiply_Vec(K_full, states), true); // minus
benson516 0:8dbe2a4687b8 119 }
benson516 1:cdd434f6aa9a 120
benson516 2:0544e16ea933 121 // Private functions
benson516 2:0544e16ea933 122 // Command (equalibrium state) related calculation
benson516 2:0544e16ea933 123 void STATE_FEEDBACK::get_Xd_ud(void){ // Calculate the compensation variable, states_d and sys_inputs_compensate
benson516 2:0544e16ea933 124 // Mat_multiply_Vec(states_d, N_xd, command);
benson516 2:0544e16ea933 125 // Mat_multiply_Vec(inputs_d, N_ud, command);
benson516 2:0544e16ea933 126 //
benson516 2:0544e16ea933 127 Mat_multiply_Vec(sys_inputs_compensate, N_total, command);
benson516 2:0544e16ea933 128
benson516 2:0544e16ea933 129 }
benson516 1:cdd434f6aa9a 130 // Utilities
benson516 0:8dbe2a4687b8 131 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 132 static vector<float>::iterator it_out;
benson516 1:cdd434f6aa9a 133 static vector<const float>::iterator it_m_row;
benson516 1:cdd434f6aa9a 134 static vector<const float>::iterator it_v;
benson516 0:8dbe2a4687b8 135 //
benson516 0:8dbe2a4687b8 136 it_out = v_out.begin();
benson516 0:8dbe2a4687b8 137 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:8dbe2a4687b8 138 *it_out = 0.0;
benson516 0:8dbe2a4687b8 139 it_m_row = m_left[i].begin();
benson516 0:8dbe2a4687b8 140 it_v = v_right.begin();
benson516 0:8dbe2a4687b8 141 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:8dbe2a4687b8 142 // *it_out += m_left[i][j] * v_right[j];
benson516 2:0544e16ea933 143 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 2:0544e16ea933 144 (*it_out) += (*it_m_row) * (*it_v);
benson516 2:0544e16ea933 145 }else{
benson516 2:0544e16ea933 146 // (*it_out) += 0.0
benson516 2:0544e16ea933 147 }
benson516 2:0544e16ea933 148 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:8dbe2a4687b8 149 //
benson516 0:8dbe2a4687b8 150 it_m_row++;
benson516 0:8dbe2a4687b8 151 it_v++;
benson516 0:8dbe2a4687b8 152 }
benson516 0:8dbe2a4687b8 153 it_out++;
benson516 0:8dbe2a4687b8 154 }
benson516 0:8dbe2a4687b8 155 }
benson516 1:cdd434f6aa9a 156 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 157 static vector<float> v_out;
benson516 1:cdd434f6aa9a 158 // Size check
benson516 1:cdd434f6aa9a 159 if (v_out.size() != m_left.size()){
benson516 1:cdd434f6aa9a 160 v_out.resize(m_left.size());
benson516 1:cdd434f6aa9a 161 }
benson516 1:cdd434f6aa9a 162 // Iterators
benson516 1:cdd434f6aa9a 163 static vector<float>::iterator it_out;
benson516 1:cdd434f6aa9a 164 static vector<const float>::iterator it_m_row;
benson516 1:cdd434f6aa9a 165 static vector<const float>::iterator it_v;
benson516 1:cdd434f6aa9a 166 //
benson516 1:cdd434f6aa9a 167 it_out = v_out.begin();
benson516 1:cdd434f6aa9a 168 for (size_t i = 0; i < m_left.size(); ++i){
benson516 1:cdd434f6aa9a 169 *it_out = 0.0;
benson516 1:cdd434f6aa9a 170 it_m_row = m_left[i].begin();
benson516 1:cdd434f6aa9a 171 it_v = v_right.begin();
benson516 1:cdd434f6aa9a 172 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 1:cdd434f6aa9a 173 // *it_out += m_left[i][j] * v_right[j];
benson516 2:0544e16ea933 174 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 2:0544e16ea933 175 (*it_out) += (*it_m_row) * (*it_v);
benson516 2:0544e16ea933 176 }else{
benson516 2:0544e16ea933 177 // (*it_out) += 0.0
benson516 2:0544e16ea933 178 }
benson516 2:0544e16ea933 179 // (*it_out) += (*it_m_row) * (*it_v);
benson516 1:cdd434f6aa9a 180 //
benson516 1:cdd434f6aa9a 181 it_m_row++;
benson516 1:cdd434f6aa9a 182 it_v++;
benson516 1:cdd434f6aa9a 183 }
benson516 1:cdd434f6aa9a 184 it_out++;
benson516 1:cdd434f6aa9a 185 }
benson516 1:cdd434f6aa9a 186 return v_out;
benson516 1:cdd434f6aa9a 187 }
benson516 1:cdd434f6aa9a 188 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 189 {
benson516 1:cdd434f6aa9a 190 static vector<float> v_c;
benson516 1:cdd434f6aa9a 191 // Size check
benson516 1:cdd434f6aa9a 192 if (v_c.size() != v_a.size()){
benson516 1:cdd434f6aa9a 193 v_c.resize(v_a.size());
benson516 1:cdd434f6aa9a 194 }
benson516 1:cdd434f6aa9a 195 //
benson516 1:cdd434f6aa9a 196 for (size_t i = 0; i < n; ++i){
benson516 1:cdd434f6aa9a 197 if (is_minus){
benson516 1:cdd434f6aa9a 198 v_c[i] = v_a[i] - v_b[i];
benson516 1:cdd434f6aa9a 199 }else{
benson516 1:cdd434f6aa9a 200 v_c[i] = v_a[i] + v_b[i];
benson516 1:cdd434f6aa9a 201 }
benson516 1:cdd434f6aa9a 202 }
benson516 1:cdd434f6aa9a 203 return v_c;
benson516 1:cdd434f6aa9a 204 }
benson516 1:cdd434f6aa9a 205 vector<float> STATE_FEEDBACK::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 1:cdd434f6aa9a 206 {
benson516 1:cdd434f6aa9a 207 static vector<float> v_c;
benson516 1:cdd434f6aa9a 208 // Size check
benson516 1:cdd434f6aa9a 209 if (v_c.size() != v_a.size()){
benson516 1:cdd434f6aa9a 210 v_c.resize(v_a.size());
benson516 1:cdd434f6aa9a 211 }
benson516 1:cdd434f6aa9a 212 // for pure negative
benson516 1:cdd434f6aa9a 213 if (scale == -1.0){
benson516 1:cdd434f6aa9a 214 for (size_t i = 0; i < n; ++i){
benson516 1:cdd434f6aa9a 215 v_c[i] = -v_a[i];
benson516 1:cdd434f6aa9a 216 }
benson516 1:cdd434f6aa9a 217 return v_c;
benson516 1:cdd434f6aa9a 218 }
benson516 1:cdd434f6aa9a 219 // else
benson516 1:cdd434f6aa9a 220 for (size_t i = 0; i < n; ++i){
benson516 1:cdd434f6aa9a 221 v_c[i] = scale*v_a[i];
benson516 1:cdd434f6aa9a 222
benson516 1:cdd434f6aa9a 223 }
benson516 1:cdd434f6aa9a 224 return v_c;
benson516 1:cdd434f6aa9a 225 }