Project_WIPV_antiSlip / STATE_FEEDBACK_INTEGRAL

The library for full-state feedback control with integral action

Fork of STATE_FEEDBACK_INTEGRAL by Project_WIPV_antiSlip

STATE_FEEDBACK_INTEGRAL.cpp@1:f19909200517, 2017-02-07 (annotated)

Committer:
benson516
Date:
Tue Feb 07 09:53:58 2017 +0000
Revision:
1:f19909200517
Parent:
0:37e27f2930a3 
test succeed

Who changed what in which revision?

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