A new type of anti-slip controller based on TS fuzzy models
ANTI_SLIP_FUZZY_CONTROL.cpp@1:773d8ae11c1a, 2017-02-26 (annotated)
- Committer:
- benson516
- Date:
- Sun Feb 26 05:36:16 2017 +0000
- Revision:
- 1:773d8ae11c1a
- Parent:
- 0:bfcd2371f3dc
Ver. 1.00
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benson516 | 0:bfcd2371f3dc | 1 | #include "ANTI_SLIP_FUZZY_CONTROL.h" |
benson516 | 0:bfcd2371f3dc | 2 | |
benson516 | 0:bfcd2371f3dc | 3 | //------------------------------------------// |
benson516 | 0:bfcd2371f3dc | 4 | // The template for building a library |
benson516 | 0:bfcd2371f3dc | 5 | // for control system apllication |
benson516 | 0:bfcd2371f3dc | 6 | //------------------------------------------// |
benson516 | 0:bfcd2371f3dc | 7 | |
benson516 | 0:bfcd2371f3dc | 8 | // The plant is a (p, n, q) system |
benson516 | 0:bfcd2371f3dc | 9 | // Dimensions: |
benson516 | 0:bfcd2371f3dc | 10 | // |
benson516 | 0:bfcd2371f3dc | 11 | // Inputs, u | States, x | outputs, y |
benson516 | 0:bfcd2371f3dc | 12 | // p --> n --> q |
benson516 | 0:bfcd2371f3dc | 13 | // |
benson516 | 0:bfcd2371f3dc | 14 | |
benson516 | 0:bfcd2371f3dc | 15 | // |
benson516 | 0:bfcd2371f3dc | 16 | // The number of vertex systems is m_vertex. |
benson516 | 0:bfcd2371f3dc | 17 | // |
benson516 | 0:bfcd2371f3dc | 18 | |
benson516 | 0:bfcd2371f3dc | 19 | ANTI_SLIP_FUZZY_CONTROL::ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime): |
benson516 | 0:bfcd2371f3dc | 20 | n(num_state), p(num_in), q(num_out), m_vertex(num_vertex), Ts(samplingTime), |
benson516 | 0:bfcd2371f3dc | 21 | E_out(num_out, vector<float>(num_state,0.0)), |
benson516 | 1:773d8ae11c1a | 22 | Nxd(num_state, vector<float>(num_in,0.0)), |
benson516 | 1:773d8ae11c1a | 23 | Nud(num_in, vector<float>(num_in,0.0)), |
benson516 | 0:bfcd2371f3dc | 24 | ver_K_matrix(num_vertex ,vector<vector<float> >(num_in, vector<float>( (num_state + num_out), 0.0)) ), |
benson516 | 0:bfcd2371f3dc | 25 | SA_r(1.0, 0.0), |
benson516 | 0:bfcd2371f3dc | 26 | SA_l(1.0, 0.0) |
benson516 | 0:bfcd2371f3dc | 27 | { |
benson516 | 1:773d8ae11c1a | 28 | |
benson516 | 1:773d8ae11c1a | 29 | // To enble, run *.start() function |
benson516 | 1:773d8ae11c1a | 30 | enable = false; |
benson516 | 1:773d8ae11c1a | 31 | // If is_usingFeedForward, Nxd and Nud are used to calculate the x_d and u_d |
benson516 | 1:773d8ae11c1a | 32 | is_usingFeedForward = false; |
benson516 | 1:773d8ae11c1a | 33 | |
benson516 | 1:773d8ae11c1a | 34 | |
benson516 | 0:bfcd2371f3dc | 35 | // Normally, q = p |
benson516 | 0:bfcd2371f3dc | 36 | if (q > p) |
benson516 | 0:bfcd2371f3dc | 37 | q = p; |
benson516 | 0:bfcd2371f3dc | 38 | // |
benson516 | 0:bfcd2371f3dc | 39 | zeros_n.assign(n, 0.0); |
benson516 | 0:bfcd2371f3dc | 40 | zeros_p.assign(p, 0.0); |
benson516 | 0:bfcd2371f3dc | 41 | zeros_q.assign(q, 0.0); |
benson516 | 0:bfcd2371f3dc | 42 | zeros_nPq.assign((n+q), 0.0); // (n+q) |
benson516 | 0:bfcd2371f3dc | 43 | zeros_m_vertex.assign(m_vertex, 0.0); |
benson516 | 0:bfcd2371f3dc | 44 | // |
benson516 | 0:bfcd2371f3dc | 45 | ones_p.assign(p, 1.0); |
benson516 | 0:bfcd2371f3dc | 46 | |
benson516 | 0:bfcd2371f3dc | 47 | // States |
benson516 | 0:bfcd2371f3dc | 48 | // Input signal --- |
benson516 | 0:bfcd2371f3dc | 49 | states = zeros_n; |
benson516 | 0:bfcd2371f3dc | 50 | command = zeros_q; // r, commands, q = p |
benson516 | 0:bfcd2371f3dc | 51 | // Output signal --- |
benson516 | 0:bfcd2371f3dc | 52 | sys_inputs = zeros_p; |
benson516 | 0:bfcd2371f3dc | 53 | |
benson516 | 0:bfcd2371f3dc | 54 | // Internal states --- |
benson516 | 0:bfcd2371f3dc | 55 | ver_u.assign(m_vertex, zeros_p); |
benson516 | 0:bfcd2371f3dc | 56 | sys_outputs = zeros_q; |
benson516 | 0:bfcd2371f3dc | 57 | // Integral state |
benson516 | 0:bfcd2371f3dc | 58 | state_int = zeros_q; |
benson516 | 0:bfcd2371f3dc | 59 | // Total states, [states; state_int] |
benson516 | 0:bfcd2371f3dc | 60 | state_total = zeros_nPq; |
benson516 | 0:bfcd2371f3dc | 61 | |
benson516 | 1:773d8ae11c1a | 62 | // Equalibrium states for command tracking |
benson516 | 1:773d8ae11c1a | 63 | x_d = zeros_n; |
benson516 | 1:773d8ae11c1a | 64 | u_d = zeros_p; |
benson516 | 0:bfcd2371f3dc | 65 | |
benson516 | 0:bfcd2371f3dc | 66 | // The composition ratio of each vertex system |
benson516 | 0:bfcd2371f3dc | 67 | ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1] |
benson516 | 1:773d8ae11c1a | 68 | ver_ratio[3] = 1.0; // Ratio for no-slip system |
benson516 | 0:bfcd2371f3dc | 69 | |
benson516 | 0:bfcd2371f3dc | 70 | } |
benson516 | 1:773d8ae11c1a | 71 | void ANTI_SLIP_FUZZY_CONTROL::start(){ |
benson516 | 1:773d8ae11c1a | 72 | enable = true; |
benson516 | 1:773d8ae11c1a | 73 | } |
benson516 | 1:773d8ae11c1a | 74 | void ANTI_SLIP_FUZZY_CONTROL::pause(){ |
benson516 | 1:773d8ae11c1a | 75 | enable = false; |
benson516 | 1:773d8ae11c1a | 76 | } |
benson516 | 1:773d8ae11c1a | 77 | void ANTI_SLIP_FUZZY_CONTROL::stop(){ |
benson516 | 1:773d8ae11c1a | 78 | if (!enable){ |
benson516 | 1:773d8ae11c1a | 79 | return; |
benson516 | 1:773d8ae11c1a | 80 | } |
benson516 | 1:773d8ae11c1a | 81 | enable = false; |
benson516 | 1:773d8ae11c1a | 82 | // Reset |
benson516 | 1:773d8ae11c1a | 83 | reset(); |
benson516 | 1:773d8ae11c1a | 84 | } |
benson516 | 1:773d8ae11c1a | 85 | void ANTI_SLIP_FUZZY_CONTROL::reset(){ |
benson516 | 1:773d8ae11c1a | 86 | // |
benson516 | 1:773d8ae11c1a | 87 | // States |
benson516 | 1:773d8ae11c1a | 88 | // Input signal --- |
benson516 | 1:773d8ae11c1a | 89 | states = zeros_n; |
benson516 | 1:773d8ae11c1a | 90 | command = zeros_q; // r, commands, q = p |
benson516 | 1:773d8ae11c1a | 91 | // Output signal --- |
benson516 | 1:773d8ae11c1a | 92 | sys_inputs = zeros_p; |
benson516 | 1:773d8ae11c1a | 93 | |
benson516 | 1:773d8ae11c1a | 94 | // Internal states --- |
benson516 | 1:773d8ae11c1a | 95 | // ver_u.assign(m_vertex, zeros_p); |
benson516 | 1:773d8ae11c1a | 96 | sys_outputs = zeros_q; |
benson516 | 1:773d8ae11c1a | 97 | // Integral state |
benson516 | 1:773d8ae11c1a | 98 | state_int = zeros_q; |
benson516 | 1:773d8ae11c1a | 99 | // Total states, [states; state_int] |
benson516 | 1:773d8ae11c1a | 100 | state_total = zeros_nPq; |
benson516 | 1:773d8ae11c1a | 101 | |
benson516 | 1:773d8ae11c1a | 102 | // Equalibrium states for command tracking |
benson516 | 1:773d8ae11c1a | 103 | x_d = zeros_n; |
benson516 | 1:773d8ae11c1a | 104 | u_d = zeros_p; |
benson516 | 1:773d8ae11c1a | 105 | |
benson516 | 1:773d8ae11c1a | 106 | // The composition ratio of each vertex system |
benson516 | 1:773d8ae11c1a | 107 | ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1] |
benson516 | 1:773d8ae11c1a | 108 | ver_ratio[3] = 1.0; // Ratio for no-slip system |
benson516 | 1:773d8ae11c1a | 109 | |
benson516 | 1:773d8ae11c1a | 110 | } |
benson516 | 1:773d8ae11c1a | 111 | void ANTI_SLIP_FUZZY_CONTROL::reset_integrator(){ // Reset the state_int only |
benson516 | 1:773d8ae11c1a | 112 | // Integral state |
benson516 | 1:773d8ae11c1a | 113 | state_int = zeros_q; |
benson516 | 1:773d8ae11c1a | 114 | } |
benson516 | 0:bfcd2371f3dc | 115 | // Assign Parameters |
benson516 | 0:bfcd2371f3dc | 116 | void ANTI_SLIP_FUZZY_CONTROL::assign_E_out(float* E_out_in){ |
benson516 | 0:bfcd2371f3dc | 117 | // E_out_in is the pointer of a mutidimentional array with size q by n |
benson516 | 0:bfcd2371f3dc | 118 | E_out.assign(q, zeros_n); |
benson516 | 0:bfcd2371f3dc | 119 | // |
benson516 | 0:bfcd2371f3dc | 120 | for (size_t i = 0; i < q; ++i){ |
benson516 | 0:bfcd2371f3dc | 121 | for (size_t j = 0; j < n; ++j){ |
benson516 | 0:bfcd2371f3dc | 122 | // E_out[i][j] = E_out_in[i][j]; |
benson516 | 0:bfcd2371f3dc | 123 | E_out[i][j] = *E_out_in; |
benson516 | 0:bfcd2371f3dc | 124 | E_out_in++; |
benson516 | 0:bfcd2371f3dc | 125 | } |
benson516 | 0:bfcd2371f3dc | 126 | } |
benson516 | 0:bfcd2371f3dc | 127 | } |
benson516 | 1:773d8ae11c1a | 128 | void ANTI_SLIP_FUZZY_CONTROL::assign_Nxd(float* Nxd_in){ |
benson516 | 1:773d8ae11c1a | 129 | // Nxd_in is the pointer of a mutidimentional array with size n by p |
benson516 | 1:773d8ae11c1a | 130 | Nxd.assign(n, zeros_p); |
benson516 | 1:773d8ae11c1a | 131 | // |
benson516 | 1:773d8ae11c1a | 132 | for (size_t i = 0; i < n; ++i){ |
benson516 | 1:773d8ae11c1a | 133 | for (size_t j = 0; j < p; ++j){ |
benson516 | 1:773d8ae11c1a | 134 | // Nxd[i][j] = Nxd_in[i][j]; |
benson516 | 1:773d8ae11c1a | 135 | Nxd[i][j] = *Nxd_in; |
benson516 | 1:773d8ae11c1a | 136 | Nxd_in++; |
benson516 | 1:773d8ae11c1a | 137 | } |
benson516 | 1:773d8ae11c1a | 138 | } |
benson516 | 1:773d8ae11c1a | 139 | // |
benson516 | 1:773d8ae11c1a | 140 | is_usingFeedForward = true; |
benson516 | 1:773d8ae11c1a | 141 | } |
benson516 | 1:773d8ae11c1a | 142 | void ANTI_SLIP_FUZZY_CONTROL::assign_Nud(float* Nud_in){ |
benson516 | 1:773d8ae11c1a | 143 | // Nxd_in is the pointer of a mutidimentional array with size p by p |
benson516 | 1:773d8ae11c1a | 144 | Nud.assign(p, zeros_p); |
benson516 | 1:773d8ae11c1a | 145 | // |
benson516 | 1:773d8ae11c1a | 146 | for (size_t i = 0; i < p; ++i){ |
benson516 | 1:773d8ae11c1a | 147 | for (size_t j = 0; j < p; ++j){ |
benson516 | 1:773d8ae11c1a | 148 | // Nud[i][j] = Nud_in[i][j]; |
benson516 | 1:773d8ae11c1a | 149 | Nud[i][j] = *Nud_in; |
benson516 | 1:773d8ae11c1a | 150 | Nud_in++; |
benson516 | 1:773d8ae11c1a | 151 | } |
benson516 | 1:773d8ae11c1a | 152 | } |
benson516 | 1:773d8ae11c1a | 153 | // |
benson516 | 1:773d8ae11c1a | 154 | is_usingFeedForward = true; |
benson516 | 1:773d8ae11c1a | 155 | } |
benson516 | 0:bfcd2371f3dc | 156 | // 1st controller Parameters (no-slip plant, sys_dVs) |
benson516 | 0:bfcd2371f3dc | 157 | void ANTI_SLIP_FUZZY_CONTROL::assign_ver_K_matrix(float* ver_K_matrix_in){ |
benson516 | 0:bfcd2371f3dc | 158 | // ver_K_matrix_in is the pointer of a list of mutidimentional array with size p by (n+q) |
benson516 | 0:bfcd2371f3dc | 159 | ver_K_matrix.assign(m_vertex, vector<vector<float> >(p, zeros_nPq) ); |
benson516 | 0:bfcd2371f3dc | 160 | // |
benson516 | 0:bfcd2371f3dc | 161 | for (size_t k = 0; k < m_vertex; ++k){ |
benson516 | 0:bfcd2371f3dc | 162 | // For each vertex |
benson516 | 0:bfcd2371f3dc | 163 | for (size_t i = 0; i < p; ++i){ |
benson516 | 0:bfcd2371f3dc | 164 | for (size_t j = 0; j < (n+q); ++j){ |
benson516 | 0:bfcd2371f3dc | 165 | // (ver_K_matrix[k])[i][j] = ver_K_matrix_in[i][j]; |
benson516 | 0:bfcd2371f3dc | 166 | (ver_K_matrix[k])[i][j] = *ver_K_matrix_in; |
benson516 | 0:bfcd2371f3dc | 167 | ver_K_matrix_in++; |
benson516 | 0:bfcd2371f3dc | 168 | } |
benson516 | 0:bfcd2371f3dc | 169 | } |
benson516 | 0:bfcd2371f3dc | 170 | // |
benson516 | 0:bfcd2371f3dc | 171 | } |
benson516 | 0:bfcd2371f3dc | 172 | // |
benson516 | 0:bfcd2371f3dc | 173 | } |
benson516 | 0:bfcd2371f3dc | 174 | |
benson516 | 0:bfcd2371f3dc | 175 | |
benson516 | 0:bfcd2371f3dc | 176 | // |
benson516 | 0:bfcd2371f3dc | 177 | void ANTI_SLIP_FUZZY_CONTROL::set_ver_ratio(float ratio_ft_right, float ratio_ft_left){ |
benson516 | 0:bfcd2371f3dc | 178 | float one_ratio_ft_right; |
benson516 | 0:bfcd2371f3dc | 179 | float one_ratio_ft_left; |
benson516 | 0:bfcd2371f3dc | 180 | // Input Saturation |
benson516 | 0:bfcd2371f3dc | 181 | ratio_ft_right = SA_r.filter(ratio_ft_right); |
benson516 | 0:bfcd2371f3dc | 182 | ratio_ft_left = SA_l.filter(ratio_ft_left); |
benson516 | 0:bfcd2371f3dc | 183 | // |
benson516 | 0:bfcd2371f3dc | 184 | one_ratio_ft_right = 1.0 - ratio_ft_right; |
benson516 | 0:bfcd2371f3dc | 185 | one_ratio_ft_left = 1.0 - ratio_ft_left; |
benson516 | 0:bfcd2371f3dc | 186 | // |
benson516 | 0:bfcd2371f3dc | 187 | ver_ratio[0] = one_ratio_ft_right * one_ratio_ft_left; |
benson516 | 0:bfcd2371f3dc | 188 | ver_ratio[1] = ratio_ft_right * one_ratio_ft_left; |
benson516 | 0:bfcd2371f3dc | 189 | ver_ratio[2] = one_ratio_ft_right * ratio_ft_left; |
benson516 | 0:bfcd2371f3dc | 190 | ver_ratio[3] = ratio_ft_right * ratio_ft_left; |
benson516 | 0:bfcd2371f3dc | 191 | } |
benson516 | 0:bfcd2371f3dc | 192 | // |
benson516 | 1:773d8ae11c1a | 193 | void ANTI_SLIP_FUZZY_CONTROL::iterateOnce(void){ |
benson516 | 0:bfcd2371f3dc | 194 | |
benson516 | 1:773d8ae11c1a | 195 | if(!enable){ |
benson516 | 1:773d8ae11c1a | 196 | return; |
benson516 | 1:773d8ae11c1a | 197 | } |
benson516 | 1:773d8ae11c1a | 198 | |
benson516 | 1:773d8ae11c1a | 199 | // |
benson516 | 1:773d8ae11c1a | 200 | if (is_usingFeedForward){ |
benson516 | 1:773d8ae11c1a | 201 | get_equilibriumState(); |
benson516 | 0:bfcd2371f3dc | 202 | // Get the state_total |
benson516 | 0:bfcd2371f3dc | 203 | get_state_total(); |
benson516 | 1:773d8ae11c1a | 204 | // u = u_d + yc; |
benson516 | 1:773d8ae11c1a | 205 | sys_inputs = u_d; |
benson516 | 0:bfcd2371f3dc | 206 | }else{ |
benson516 | 1:773d8ae11c1a | 207 | // Get the state_total |
benson516 | 1:773d8ae11c1a | 208 | get_state_total(); |
benson516 | 1:773d8ae11c1a | 209 | // u = yc; |
benson516 | 0:bfcd2371f3dc | 210 | sys_inputs = zeros_p; |
benson516 | 0:bfcd2371f3dc | 211 | } |
benson516 | 0:bfcd2371f3dc | 212 | |
benson516 | 1:773d8ae11c1a | 213 | |
benson516 | 1:773d8ae11c1a | 214 | /* |
benson516 | 1:773d8ae11c1a | 215 | // Slower solution, record the outout of each vertex controller |
benson516 | 1:773d8ae11c1a | 216 | sys_inputs = zeros_p; |
benson516 | 1:773d8ae11c1a | 217 | // |
benson516 | 1:773d8ae11c1a | 218 | for (size_t k = 0; k < m_vertex; ++k){ |
benson516 | 1:773d8ae11c1a | 219 | ver_u[k] = Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), -1.0 ); |
benson516 | 1:773d8ae11c1a | 220 | Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( ver_u[k], ver_ratio[k]), false); // += |
benson516 | 1:773d8ae11c1a | 221 | } |
benson516 | 1:773d8ae11c1a | 222 | */ |
benson516 | 1:773d8ae11c1a | 223 | |
benson516 | 1:773d8ae11c1a | 224 | // Faster solution, no recording the output of each vertex controller |
benson516 | 1:773d8ae11c1a | 225 | // sys_inputs = zeros_p; |
benson516 | 1:773d8ae11c1a | 226 | // |
benson516 | 1:773d8ae11c1a | 227 | for (size_t k = 0; k < m_vertex; ++k){ |
benson516 | 1:773d8ae11c1a | 228 | // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total); |
benson516 | 1:773d8ae11c1a | 229 | Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -= |
benson516 | 1:773d8ae11c1a | 230 | } |
benson516 | 1:773d8ae11c1a | 231 | |
benson516 | 1:773d8ae11c1a | 232 | /* |
benson516 | 1:773d8ae11c1a | 233 | // Add the u_d |
benson516 | 1:773d8ae11c1a | 234 | if (is_usingFeedForward){ |
benson516 | 1:773d8ae11c1a | 235 | Get_VectorIncrement(sys_inputs, u_d, false); // += |
benson516 | 1:773d8ae11c1a | 236 | } |
benson516 | 1:773d8ae11c1a | 237 | */ |
benson516 | 1:773d8ae11c1a | 238 | |
benson516 | 0:bfcd2371f3dc | 239 | // Integral action |
benson516 | 1:773d8ae11c1a | 240 | get_integral(); |
benson516 | 0:bfcd2371f3dc | 241 | } |
benson516 | 0:bfcd2371f3dc | 242 | |
benson516 | 0:bfcd2371f3dc | 243 | // Private functions |
benson516 | 1:773d8ae11c1a | 244 | // Calculate the equilibrium states |
benson516 | 1:773d8ae11c1a | 245 | void ANTI_SLIP_FUZZY_CONTROL::get_equilibriumState(void){ |
benson516 | 1:773d8ae11c1a | 246 | Mat_multiply_Vec(x_d, Nxd, command); |
benson516 | 1:773d8ae11c1a | 247 | Mat_multiply_Vec(u_d, Nud, command); |
benson516 | 1:773d8ae11c1a | 248 | } |
benson516 | 0:bfcd2371f3dc | 249 | // Calculate the sys_outputs |
benson516 | 0:bfcd2371f3dc | 250 | void ANTI_SLIP_FUZZY_CONTROL::get_sys_outputs(void){ // Calculate the sys_outputs from states, by mutiplying E_out |
benson516 | 0:bfcd2371f3dc | 251 | // sys_outputs = E_out*states |
benson516 | 1:773d8ae11c1a | 252 | Mat_multiply_Vec(sys_outputs, E_out, states); |
benson516 | 1:773d8ae11c1a | 253 | // sys_outputs = Mat_multiply_Vec(E_out, states); |
benson516 | 0:bfcd2371f3dc | 254 | } |
benson516 | 0:bfcd2371f3dc | 255 | // Calculate the Integral |
benson516 | 1:773d8ae11c1a | 256 | void ANTI_SLIP_FUZZY_CONTROL::get_integral(){ // Calculate the state_int |
benson516 | 1:773d8ae11c1a | 257 | if(!enable){ |
benson516 | 1:773d8ae11c1a | 258 | return; |
benson516 | 1:773d8ae11c1a | 259 | } |
benson516 | 0:bfcd2371f3dc | 260 | // |
benson516 | 0:bfcd2371f3dc | 261 | // Calculate the sys_outputs |
benson516 | 0:bfcd2371f3dc | 262 | get_sys_outputs(); |
benson516 | 1:773d8ae11c1a | 263 | // |
benson516 | 0:bfcd2371f3dc | 264 | // Integral action |
benson516 | 0:bfcd2371f3dc | 265 | // state_int += sys_outputs - command |
benson516 | 1:773d8ae11c1a | 266 | Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // += |
benson516 | 0:bfcd2371f3dc | 267 | } |
benson516 | 0:bfcd2371f3dc | 268 | // Concatenate the state and state_int |
benson516 | 0:bfcd2371f3dc | 269 | void ANTI_SLIP_FUZZY_CONTROL::get_state_total(void){ // Total states, [states; state_int] |
benson516 | 0:bfcd2371f3dc | 270 | // |
benson516 | 0:bfcd2371f3dc | 271 | size_t idx = 0; |
benson516 | 1:773d8ae11c1a | 272 | |
benson516 | 0:bfcd2371f3dc | 273 | // states |
benson516 | 1:773d8ae11c1a | 274 | if (is_usingFeedForward){ |
benson516 | 1:773d8ae11c1a | 275 | for (size_t i = 0; i < n; ++i){ |
benson516 | 1:773d8ae11c1a | 276 | state_total[idx] = states[i] - x_d[i]; |
benson516 | 1:773d8ae11c1a | 277 | idx++; |
benson516 | 1:773d8ae11c1a | 278 | } |
benson516 | 1:773d8ae11c1a | 279 | }else{ |
benson516 | 1:773d8ae11c1a | 280 | for (size_t i = 0; i < n; ++i){ |
benson516 | 1:773d8ae11c1a | 281 | state_total[idx] = states[i]; |
benson516 | 1:773d8ae11c1a | 282 | idx++; |
benson516 | 1:773d8ae11c1a | 283 | } |
benson516 | 0:bfcd2371f3dc | 284 | } |
benson516 | 1:773d8ae11c1a | 285 | |
benson516 | 0:bfcd2371f3dc | 286 | // state_int |
benson516 | 0:bfcd2371f3dc | 287 | for (size_t i = 0; i < q; ++i){ |
benson516 | 0:bfcd2371f3dc | 288 | state_total[idx] = state_int[i]; |
benson516 | 0:bfcd2371f3dc | 289 | idx++; |
benson516 | 0:bfcd2371f3dc | 290 | } |
benson516 | 0:bfcd2371f3dc | 291 | } |
benson516 | 0:bfcd2371f3dc | 292 | |
benson516 | 0:bfcd2371f3dc | 293 | // Utilities |
benson516 | 0:bfcd2371f3dc | 294 | void ANTI_SLIP_FUZZY_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:bfcd2371f3dc | 295 | |
benson516 | 0:bfcd2371f3dc | 296 | // Size check |
benson516 | 0:bfcd2371f3dc | 297 | if (v_out.size() != m_left.size()){ |
benson516 | 0:bfcd2371f3dc | 298 | v_out.resize(m_left.size()); |
benson516 | 0:bfcd2371f3dc | 299 | } |
benson516 | 0:bfcd2371f3dc | 300 | |
benson516 | 0:bfcd2371f3dc | 301 | /* |
benson516 | 0:bfcd2371f3dc | 302 | // Iterators |
benson516 | 0:bfcd2371f3dc | 303 | static vector<float>::iterator it_out; |
benson516 | 0:bfcd2371f3dc | 304 | static vector<float>::iterator it_m_row; |
benson516 | 0:bfcd2371f3dc | 305 | static vector<float>::iterator it_v; |
benson516 | 0:bfcd2371f3dc | 306 | |
benson516 | 0:bfcd2371f3dc | 307 | // |
benson516 | 0:bfcd2371f3dc | 308 | it_out = v_out.begin(); |
benson516 | 0:bfcd2371f3dc | 309 | for (size_t i = 0; i < m_left.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 310 | *it_out = 0.0; |
benson516 | 0:bfcd2371f3dc | 311 | it_m_row = vector<vector<float> >(m_left)[i].begin(); |
benson516 | 0:bfcd2371f3dc | 312 | it_v = vector<float>(v_right).begin(); |
benson516 | 0:bfcd2371f3dc | 313 | for (size_t j = 0; j < m_left[i].size(); ++j){ |
benson516 | 0:bfcd2371f3dc | 314 | // *it_out += m_left[i][j] * v_right[j]; |
benson516 | 0:bfcd2371f3dc | 315 | if (*it_m_row != 0.0 && *it_v != 0.0){ |
benson516 | 0:bfcd2371f3dc | 316 | (*it_out) += (*it_m_row) * (*it_v); |
benson516 | 0:bfcd2371f3dc | 317 | }else{ |
benson516 | 0:bfcd2371f3dc | 318 | // (*it_out) += 0.0 |
benson516 | 0:bfcd2371f3dc | 319 | } |
benson516 | 0:bfcd2371f3dc | 320 | // (*it_out) += (*it_m_row) * (*it_v); |
benson516 | 0:bfcd2371f3dc | 321 | // |
benson516 | 0:bfcd2371f3dc | 322 | it_m_row++; |
benson516 | 0:bfcd2371f3dc | 323 | it_v++; |
benson516 | 0:bfcd2371f3dc | 324 | } |
benson516 | 0:bfcd2371f3dc | 325 | it_out++; |
benson516 | 0:bfcd2371f3dc | 326 | } |
benson516 | 0:bfcd2371f3dc | 327 | */ |
benson516 | 0:bfcd2371f3dc | 328 | |
benson516 | 0:bfcd2371f3dc | 329 | // Indexing |
benson516 | 0:bfcd2371f3dc | 330 | for (size_t i = 0; i < m_left.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 331 | // |
benson516 | 0:bfcd2371f3dc | 332 | v_out[i] = 0.0; |
benson516 | 0:bfcd2371f3dc | 333 | // |
benson516 | 0:bfcd2371f3dc | 334 | for (size_t j = 0; j < v_right.size(); ++j){ |
benson516 | 0:bfcd2371f3dc | 335 | if (m_left[i][j] != 0.0 && v_right[j] != 0.0) |
benson516 | 0:bfcd2371f3dc | 336 | v_out[i] += m_left[i][j]*v_right[j]; |
benson516 | 0:bfcd2371f3dc | 337 | } |
benson516 | 0:bfcd2371f3dc | 338 | } |
benson516 | 0:bfcd2371f3dc | 339 | |
benson516 | 0:bfcd2371f3dc | 340 | } |
benson516 | 0:bfcd2371f3dc | 341 | vector<float> ANTI_SLIP_FUZZY_CONTROL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right |
benson516 | 0:bfcd2371f3dc | 342 | vector<float> v_out; |
benson516 | 0:bfcd2371f3dc | 343 | // Size check |
benson516 | 0:bfcd2371f3dc | 344 | if (v_out.size() != m_left.size()){ |
benson516 | 0:bfcd2371f3dc | 345 | v_out.resize(m_left.size()); |
benson516 | 0:bfcd2371f3dc | 346 | } |
benson516 | 0:bfcd2371f3dc | 347 | |
benson516 | 0:bfcd2371f3dc | 348 | /* |
benson516 | 0:bfcd2371f3dc | 349 | // Iterators |
benson516 | 0:bfcd2371f3dc | 350 | static vector<float>::iterator it_out; |
benson516 | 0:bfcd2371f3dc | 351 | static vector<const float>::iterator it_m_row; |
benson516 | 0:bfcd2371f3dc | 352 | static vector<const float>::iterator it_v; |
benson516 | 0:bfcd2371f3dc | 353 | // |
benson516 | 0:bfcd2371f3dc | 354 | it_out = v_out.begin(); |
benson516 | 0:bfcd2371f3dc | 355 | for (size_t i = 0; i < m_left.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 356 | *it_out = 0.0; |
benson516 | 0:bfcd2371f3dc | 357 | it_m_row = m_left[i].begin(); |
benson516 | 0:bfcd2371f3dc | 358 | it_v = v_right.begin(); |
benson516 | 0:bfcd2371f3dc | 359 | for (size_t j = 0; j < m_left[i].size(); ++j){ |
benson516 | 0:bfcd2371f3dc | 360 | // *it_out += m_left[i][j] * v_right[j]; |
benson516 | 0:bfcd2371f3dc | 361 | if (*it_m_row != 0.0 && *it_v != 0.0){ |
benson516 | 0:bfcd2371f3dc | 362 | (*it_out) += (*it_m_row) * (*it_v); |
benson516 | 0:bfcd2371f3dc | 363 | }else{ |
benson516 | 0:bfcd2371f3dc | 364 | // (*it_out) += 0.0 |
benson516 | 0:bfcd2371f3dc | 365 | } |
benson516 | 0:bfcd2371f3dc | 366 | // (*it_out) += (*it_m_row) * (*it_v); |
benson516 | 0:bfcd2371f3dc | 367 | // |
benson516 | 0:bfcd2371f3dc | 368 | it_m_row++; |
benson516 | 0:bfcd2371f3dc | 369 | it_v++; |
benson516 | 0:bfcd2371f3dc | 370 | } |
benson516 | 0:bfcd2371f3dc | 371 | it_out++; |
benson516 | 0:bfcd2371f3dc | 372 | } |
benson516 | 0:bfcd2371f3dc | 373 | */ |
benson516 | 0:bfcd2371f3dc | 374 | |
benson516 | 0:bfcd2371f3dc | 375 | // Indexing |
benson516 | 0:bfcd2371f3dc | 376 | for (size_t i = 0; i < m_left.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 377 | // |
benson516 | 0:bfcd2371f3dc | 378 | v_out[i] = 0.0; |
benson516 | 0:bfcd2371f3dc | 379 | // |
benson516 | 0:bfcd2371f3dc | 380 | for (size_t j = 0; j < v_right.size(); ++j){ |
benson516 | 0:bfcd2371f3dc | 381 | if (m_left[i][j] != 0.0 && v_right[j] != 0.0) |
benson516 | 0:bfcd2371f3dc | 382 | v_out[i] += m_left[i][j]*v_right[j]; |
benson516 | 0:bfcd2371f3dc | 383 | } |
benson516 | 0:bfcd2371f3dc | 384 | } |
benson516 | 0:bfcd2371f3dc | 385 | |
benson516 | 0:bfcd2371f3dc | 386 | return v_out; |
benson516 | 0:bfcd2371f3dc | 387 | } |
benson516 | 0:bfcd2371f3dc | 388 | vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b |
benson516 | 0:bfcd2371f3dc | 389 | { |
benson516 | 0:bfcd2371f3dc | 390 | static vector<float> v_c; |
benson516 | 0:bfcd2371f3dc | 391 | // Size check |
benson516 | 0:bfcd2371f3dc | 392 | if (v_c.size() != v_a.size()){ |
benson516 | 0:bfcd2371f3dc | 393 | v_c.resize(v_a.size()); |
benson516 | 0:bfcd2371f3dc | 394 | } |
benson516 | 0:bfcd2371f3dc | 395 | // |
benson516 | 0:bfcd2371f3dc | 396 | for (size_t i = 0; i < v_a.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 397 | if (is_minus){ |
benson516 | 0:bfcd2371f3dc | 398 | v_c[i] = v_a[i] - v_b[i]; |
benson516 | 0:bfcd2371f3dc | 399 | }else{ |
benson516 | 0:bfcd2371f3dc | 400 | v_c[i] = v_a[i] + v_b[i]; |
benson516 | 0:bfcd2371f3dc | 401 | } |
benson516 | 0:bfcd2371f3dc | 402 | } |
benson516 | 0:bfcd2371f3dc | 403 | return v_c; |
benson516 | 0:bfcd2371f3dc | 404 | } |
benson516 | 0:bfcd2371f3dc | 405 | vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a |
benson516 | 0:bfcd2371f3dc | 406 | { |
benson516 | 0:bfcd2371f3dc | 407 | static vector<float> v_c; |
benson516 | 0:bfcd2371f3dc | 408 | // Size check |
benson516 | 0:bfcd2371f3dc | 409 | if (v_c.size() != v_a.size()){ |
benson516 | 0:bfcd2371f3dc | 410 | v_c.resize(v_a.size()); |
benson516 | 0:bfcd2371f3dc | 411 | } |
benson516 | 0:bfcd2371f3dc | 412 | // for pure negative |
benson516 | 0:bfcd2371f3dc | 413 | if (scale == -1.0){ |
benson516 | 0:bfcd2371f3dc | 414 | for (size_t i = 0; i < v_a.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 415 | v_c[i] = -v_a[i]; |
benson516 | 0:bfcd2371f3dc | 416 | } |
benson516 | 0:bfcd2371f3dc | 417 | return v_c; |
benson516 | 0:bfcd2371f3dc | 418 | } |
benson516 | 0:bfcd2371f3dc | 419 | // else |
benson516 | 0:bfcd2371f3dc | 420 | for (size_t i = 0; i < v_a.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 421 | v_c[i] = scale*v_a[i]; |
benson516 | 0:bfcd2371f3dc | 422 | |
benson516 | 0:bfcd2371f3dc | 423 | } |
benson516 | 0:bfcd2371f3dc | 424 | return v_c; |
benson516 | 0:bfcd2371f3dc | 425 | } |
benson516 | 0:bfcd2371f3dc | 426 | // Increment |
benson516 | 0:bfcd2371f3dc | 427 | void ANTI_SLIP_FUZZY_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b |
benson516 | 0:bfcd2371f3dc | 428 | // Size check |
benson516 | 0:bfcd2371f3dc | 429 | if (v_a.size() != v_b.size()){ |
benson516 | 0:bfcd2371f3dc | 430 | v_a.resize(v_b.size()); |
benson516 | 0:bfcd2371f3dc | 431 | } |
benson516 | 0:bfcd2371f3dc | 432 | // |
benson516 | 0:bfcd2371f3dc | 433 | if (is_minus){ // -= |
benson516 | 0:bfcd2371f3dc | 434 | for (size_t i = 0; i < v_b.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 435 | v_a[i] -= v_b[i]; |
benson516 | 0:bfcd2371f3dc | 436 | } |
benson516 | 0:bfcd2371f3dc | 437 | }else{ // += |
benson516 | 0:bfcd2371f3dc | 438 | for (size_t i = 0; i < v_b.size(); ++i){ |
benson516 | 0:bfcd2371f3dc | 439 | v_a[i] += v_b[i]; |
benson516 | 0:bfcd2371f3dc | 440 | } |
benson516 | 0:bfcd2371f3dc | 441 | } |
benson516 | 0:bfcd2371f3dc | 442 | |
benson516 | 0:bfcd2371f3dc | 443 | } |