Project_WIPV_antiSlip / STATES_OBSERVER_DELAY

An observer that can estimate the states from delayed signals

Fork of STATES_OBSERVER_DELAY by Project_WIPV_antiSlip

STATES_OBSERVER_DELAY.cpp@1:085d41355949, 2017-02-10 (annotated)

Committer:
benson516
Date:
Fri Feb 10 18:27:14 2017 +0000
Revision:
1:085d41355949
Parent:
0:0699f8e638ca 
Fix some serious bugs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:0699f8e638ca 1 #include "STATES_OBSERVER_DELAY.h"
benson516 0:0699f8e638ca 2
benson516 0:0699f8e638ca 3 STATES_OBSERVER_DELAY::STATES_OBSERVER_DELAY(size_t num_state, size_t num_in, size_t num_out, size_t delay_sample, float samplingTime):
benson516 0:0699f8e638ca 4 n(num_state), p(num_in), q(num_out), d(delay_sample), Ts(samplingTime),
benson516 0:0699f8e638ca 5 states_est_buffer((delay_sample+1),num_state)
benson516 0:0699f8e638ca 6 {
benson516 0:0699f8e638ca 7 // To enble, run *.start() function
benson516 0:0699f8e638ca 8 enable = false;
benson516 0:0699f8e638ca 9 flag_reset = false;
benson516 0:0699f8e638ca 10 //
benson516 0:0699f8e638ca 11 zeros_n.assign(n, 0.0);
benson516 0:0699f8e638ca 12 zeros_p.assign(p, 0.0);
benson516 0:0699f8e638ca 13 zeros_q.assign(q, 0.0);
benson516 0:0699f8e638ca 14 // Parameters for observer
benson516 0:0699f8e638ca 15 Ad.assign(n,zeros_n);
benson516 0:0699f8e638ca 16 Bd.assign(n,zeros_p);
benson516 0:0699f8e638ca 17 if (num_out == 0){
benson516 0:0699f8e638ca 18 enable_Cd = false; // Default: not using Cd
benson516 0:0699f8e638ca 19 q = n;
benson516 0:0699f8e638ca 20 zeros_q.resize(q, 0.0);
benson516 1:085d41355949 21 Cd.assign(q,zeros_n);
benson516 0:0699f8e638ca 22 }else{
benson516 0:0699f8e638ca 23 enable_Cd = true; // using Cd
benson516 0:0699f8e638ca 24 Cd.assign(q,zeros_n);
benson516 0:0699f8e638ca 25 }
benson516 0:0699f8e638ca 26 // Gain matrix
benson516 0:0699f8e638ca 27 Ld.assign(n*(d+1),zeros_q); // Ld is an n(d+1) by q matrix (or n(d+1) by n if Cd is not used)
benson516 0:0699f8e638ca 28
benson516 0:0699f8e638ca 29 // Input-signals of the observer
benson516 0:0699f8e638ca 30 sys_inputs = zeros_p;
benson516 0:0699f8e638ca 31 sys_outputs = zeros_q;
benson516 0:0699f8e638ca 32 sys_extraDisturbance = zeros_n;
benson516 0:0699f8e638ca 33 // Variables of the observer
benson516 0:0699f8e638ca 34 states_est_buffer.Reset(zeros_n);
benson516 0:0699f8e638ca 35 state_est = zeros_n;
benson516 0:0699f8e638ca 36 // states_est_delay_d = zeros_n;
benson516 0:0699f8e638ca 37 y_est = zeros_q;
benson516 0:0699f8e638ca 38 est_error = zeros_q;
benson516 0:0699f8e638ca 39 }
benson516 0:0699f8e638ca 40 void STATES_OBSERVER_DELAY::start(){
benson516 0:0699f8e638ca 41 enable = true;
benson516 0:0699f8e638ca 42 }
benson516 0:0699f8e638ca 43 void STATES_OBSERVER_DELAY::stop(){
benson516 0:0699f8e638ca 44 if (!enable){
benson516 0:0699f8e638ca 45 return;
benson516 0:0699f8e638ca 46 }
benson516 0:0699f8e638ca 47 enable = false;
benson516 0:0699f8e638ca 48 flag_reset = true;
benson516 0:0699f8e638ca 49 }
benson516 0:0699f8e638ca 50 void STATES_OBSERVER_DELAY::reset(){
benson516 0:0699f8e638ca 51 //
benson516 0:0699f8e638ca 52 // Input-signals of the observer
benson516 0:0699f8e638ca 53 sys_inputs = zeros_p;
benson516 0:0699f8e638ca 54 sys_outputs = zeros_q;
benson516 0:0699f8e638ca 55 // Variables of the observer
benson516 0:0699f8e638ca 56 states_est_buffer.Reset(zeros_n);
benson516 0:0699f8e638ca 57 state_est = zeros_n;
benson516 0:0699f8e638ca 58 // states_est_delay_d = zeros_n;
benson516 0:0699f8e638ca 59 y_est = zeros_q;
benson516 0:0699f8e638ca 60 est_error = zeros_q;
benson516 0:0699f8e638ca 61 }
benson516 0:0699f8e638ca 62
benson516 0:0699f8e638ca 63
benson516 0:0699f8e638ca 64 // Assignments for observer
benson516 0:0699f8e638ca 65 // Assign continuous-time version of system matrices
benson516 0:0699f8e638ca 66 void STATES_OBSERVER_DELAY::assign_At(float* At_in, size_t n_in){ // Continuous-time version
benson516 0:0699f8e638ca 67 // At_in is the pointer of a mutidimentional array with size n_in by n_in
benson516 0:0699f8e638ca 68 if (n != n_in){
benson516 0:0699f8e638ca 69 n = n_in;
benson516 0:0699f8e638ca 70 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 71 Ad.assign(n, zeros_n);
benson516 0:0699f8e638ca 72 }
benson516 0:0699f8e638ca 73 //
benson516 0:0699f8e638ca 74 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 75 for (size_t j = 0; j < n; ++j){
benson516 0:0699f8e638ca 76 // Ad[i][j] = At_in[i][j];
benson516 0:0699f8e638ca 77 Ad[i][j] = (*At_in)*Ts;
benson516 0:0699f8e638ca 78 //
benson516 0:0699f8e638ca 79 if (i == j){
benson516 0:0699f8e638ca 80 Ad[i][j] += 1.0;
benson516 0:0699f8e638ca 81 }
benson516 0:0699f8e638ca 82 //
benson516 0:0699f8e638ca 83 At_in++;
benson516 0:0699f8e638ca 84 }
benson516 0:0699f8e638ca 85 }
benson516 0:0699f8e638ca 86 }
benson516 0:0699f8e638ca 87 void STATES_OBSERVER_DELAY::assign_Bt(float* Bt_in, size_t n_in, size_t p_in){ // Continuous-time version
benson516 0:0699f8e638ca 88 // Bt_in is the pointer of a mutidimentional array with size n_in by p_in
benson516 0:0699f8e638ca 89 if (n != n_in || p != p_in){
benson516 0:0699f8e638ca 90 n = n_in;
benson516 0:0699f8e638ca 91 p = p_in;
benson516 0:0699f8e638ca 92 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 93 zeros_p.resize(p, 0.0);
benson516 0:0699f8e638ca 94 Bd.assign(n, zeros_p);
benson516 0:0699f8e638ca 95 }
benson516 0:0699f8e638ca 96 //
benson516 0:0699f8e638ca 97 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 98 for (size_t j = 0; j < p; ++j){
benson516 0:0699f8e638ca 99 // Bd[i][j] = Bt_in[i][j];
benson516 0:0699f8e638ca 100 Bd[i][j] = (*Bt_in)*Ts;
benson516 0:0699f8e638ca 101 Bt_in++;
benson516 0:0699f8e638ca 102 }
benson516 0:0699f8e638ca 103 }
benson516 0:0699f8e638ca 104 }
benson516 1:085d41355949 105 void STATES_OBSERVER_DELAY::assign_Lt(float* Lt_in, size_t n_in, size_t d_in, size_t q_in){ // Continuous-time version
benson516 1:085d41355949 106 // Lt_in is the pointer of a mutidimentional array with size n_in*(d_in+1) by q_in
benson516 1:085d41355949 107 if (n != n_in || d != d_in || q != q_in){
benson516 1:085d41355949 108 n = n_in;
benson516 1:085d41355949 109 q = q_in;
benson516 1:085d41355949 110 zeros_n.resize(n, 0.0);
benson516 1:085d41355949 111 zeros_q.resize(q, 0.0);
benson516 1:085d41355949 112 //
benson516 1:085d41355949 113 if (d != d_in){
benson516 1:085d41355949 114 d = d_in;
benson516 1:085d41355949 115 states_est_buffer.Init((d+1), zeros_n);
benson516 1:085d41355949 116 }
benson516 1:085d41355949 117 Ld.assign(n*(d+1), zeros_q);
benson516 1:085d41355949 118 }
benson516 1:085d41355949 119 //
benson516 1:085d41355949 120 for (size_t i = 0; i < n*(d+1); ++i){
benson516 1:085d41355949 121 for (size_t j = 0; j < q; ++j){
benson516 1:085d41355949 122 // Ld[i][j] = Lt_in[i][j];
benson516 1:085d41355949 123 Ld[i][j] = (*Lt_in)*Ts;
benson516 1:085d41355949 124 Lt_in++;
benson516 1:085d41355949 125 }
benson516 1:085d41355949 126 }
benson516 1:085d41355949 127 }
benson516 0:0699f8e638ca 128 // ** Assign the continuous-time version of system matrices by matrices
benson516 0:0699f8e638ca 129 void STATES_OBSERVER_DELAY::assign_At(const vector<vector<float> > &At_in){
benson516 0:0699f8e638ca 130 size_t n_in = At_in.size();
benson516 0:0699f8e638ca 131 if (n != n_in){
benson516 0:0699f8e638ca 132 n = n_in;
benson516 0:0699f8e638ca 133 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 134 Ad.assign(n, zeros_n);
benson516 0:0699f8e638ca 135 }
benson516 0:0699f8e638ca 136 // Ad
benson516 0:0699f8e638ca 137 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 138 for (size_t j = 0; j < n; ++j){
benson516 0:0699f8e638ca 139 //
benson516 0:0699f8e638ca 140 Ad[i][j] = Ts*At_in[i][j];
benson516 0:0699f8e638ca 141 //
benson516 0:0699f8e638ca 142 if (i == j){
benson516 0:0699f8e638ca 143 Ad[i][j] += 1.0;
benson516 0:0699f8e638ca 144 }
benson516 0:0699f8e638ca 145 }
benson516 0:0699f8e638ca 146 }
benson516 0:0699f8e638ca 147 }
benson516 0:0699f8e638ca 148 void STATES_OBSERVER_DELAY::assign_Bt(const vector<vector<float> > &Bt_in){
benson516 0:0699f8e638ca 149 size_t n_in = Bt_in.size();
benson516 0:0699f8e638ca 150 size_t p_in = Bt_in[0].size();
benson516 0:0699f8e638ca 151 if (n != n_in || p != p_in){
benson516 0:0699f8e638ca 152 n = n_in;
benson516 0:0699f8e638ca 153 p = p_in;
benson516 0:0699f8e638ca 154 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 155 zeros_p.resize(p, 0.0);
benson516 0:0699f8e638ca 156 Bd.assign(n, zeros_p);
benson516 0:0699f8e638ca 157 }
benson516 0:0699f8e638ca 158 // Bd
benson516 0:0699f8e638ca 159 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 160 for (size_t j = 0; j < p; ++j){
benson516 0:0699f8e638ca 161 // Bd[i][j] = Bt_in[i][j];
benson516 0:0699f8e638ca 162 Bd[i][j] = Ts*Bt_in[i][j];
benson516 0:0699f8e638ca 163 }
benson516 0:0699f8e638ca 164 }
benson516 0:0699f8e638ca 165 }
benson516 1:085d41355949 166 void STATES_OBSERVER_DELAY::assign_Lt(const vector<vector<float> > &Lt_in){
benson516 1:085d41355949 167 size_t ndp1_in = Lt_in.size();
benson516 1:085d41355949 168 size_t q_in = Lt_in[0].size();
benson516 1:085d41355949 169 //
benson516 1:085d41355949 170 if ( n*(d+1) != ndp1_in || q != q_in){
benson516 1:085d41355949 171 n = ndp1_in/(d+1);
benson516 1:085d41355949 172 q = q_in;
benson516 1:085d41355949 173 zeros_n.resize(n, 0.0);
benson516 1:085d41355949 174 zeros_q.resize(q, 0.0);
benson516 1:085d41355949 175 Ld.assign(n*(d+1), zeros_q);
benson516 1:085d41355949 176 }
benson516 1:085d41355949 177 // Ld
benson516 1:085d41355949 178 for (size_t i = 0; i < n*(d+1); ++i){
benson516 1:085d41355949 179 for (size_t j = 0; j < q; ++j){
benson516 1:085d41355949 180 //
benson516 1:085d41355949 181 Ld[i][j] = Ts*Lt_in[i][j];
benson516 1:085d41355949 182 }
benson516 1:085d41355949 183 }
benson516 1:085d41355949 184 }
benson516 0:0699f8e638ca 185 // Assign discrete-time version of system matrices directly
benson516 0:0699f8e638ca 186 void STATES_OBSERVER_DELAY::assign_Ad(float* Ad_in, size_t n_in){ // Discrete-time version
benson516 0:0699f8e638ca 187 // Ad_in is the pointer of a mutidimentional array with size n_in by n_in
benson516 0:0699f8e638ca 188 if (n != n_in){
benson516 0:0699f8e638ca 189 n = n_in;
benson516 0:0699f8e638ca 190 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 191 Ad.assign(n, zeros_n);
benson516 0:0699f8e638ca 192 }
benson516 0:0699f8e638ca 193 //
benson516 0:0699f8e638ca 194 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 195 for (size_t j = 0; j < n; ++j){
benson516 0:0699f8e638ca 196 // Ad[i][j] = Ad_in[i][j];
benson516 0:0699f8e638ca 197 Ad[i][j] = *Ad_in;
benson516 0:0699f8e638ca 198 Ad_in++;
benson516 0:0699f8e638ca 199 }
benson516 0:0699f8e638ca 200 }
benson516 0:0699f8e638ca 201 }
benson516 0:0699f8e638ca 202 void STATES_OBSERVER_DELAY::assign_Bd(float* Bd_in, size_t n_in, size_t p_in){ // Discrete-time version
benson516 0:0699f8e638ca 203 // Bd_in is the pointer of a mutidimentional array with size n_in by p_in
benson516 0:0699f8e638ca 204 if (n != n_in || p != p_in){
benson516 0:0699f8e638ca 205 n = n_in;
benson516 0:0699f8e638ca 206 p = p_in;
benson516 0:0699f8e638ca 207 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 208 zeros_p.resize(p, 0.0);
benson516 0:0699f8e638ca 209 Bd.assign(n, zeros_p);
benson516 0:0699f8e638ca 210 }
benson516 0:0699f8e638ca 211 //
benson516 0:0699f8e638ca 212 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 213 for (size_t j = 0; j < p; ++j){
benson516 0:0699f8e638ca 214 // Bd[i][j] = Bd_in[i][j];
benson516 0:0699f8e638ca 215 Bd[i][j] = *Bd_in;
benson516 0:0699f8e638ca 216 Bd_in++;
benson516 0:0699f8e638ca 217 }
benson516 0:0699f8e638ca 218 }
benson516 0:0699f8e638ca 219 }
benson516 0:0699f8e638ca 220 //
benson516 0:0699f8e638ca 221 void STATES_OBSERVER_DELAY::assign_Cd(float* Cd_in, size_t q_in, size_t n_in){
benson516 0:0699f8e638ca 222 // Cd_in is the pointer of a mutidimentional array with size q_in by n_in
benson516 1:085d41355949 223 // q = n if not using Cd
benson516 1:085d41355949 224 if (q_in == 0){
benson516 1:085d41355949 225 enable_Cd = false; // Default: not using Cd
benson516 1:085d41355949 226 q_in = n_in;
benson516 1:085d41355949 227 //
benson516 1:085d41355949 228 q = q_in;
benson516 1:085d41355949 229 n = n_in;
benson516 1:085d41355949 230 zeros_n.resize(n,0.0);
benson516 1:085d41355949 231 zeros_q.resize(q, 0.0);
benson516 1:085d41355949 232 Cd.assign(q,zeros_n);
benson516 1:085d41355949 233 //
benson516 1:085d41355949 234 return;
benson516 1:085d41355949 235 }else{
benson516 1:085d41355949 236 enable_Cd = true; // using Cd
benson516 1:085d41355949 237 // Cd.assign(q,zeros_n);
benson516 1:085d41355949 238 }
benson516 1:085d41355949 239
benson516 0:0699f8e638ca 240 if (n != n_in || q != q_in){
benson516 0:0699f8e638ca 241 n = n_in;
benson516 0:0699f8e638ca 242 q = q_in;
benson516 0:0699f8e638ca 243 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 244 zeros_q.resize(q, 0.0);
benson516 0:0699f8e638ca 245 Cd.assign(q, zeros_n);
benson516 0:0699f8e638ca 246 }
benson516 0:0699f8e638ca 247 //
benson516 0:0699f8e638ca 248 for (size_t i = 0; i < q; ++i){
benson516 0:0699f8e638ca 249 for (size_t j = 0; j < n; ++j){
benson516 0:0699f8e638ca 250 // Cd[i][j] = Cd_in[i][j];
benson516 0:0699f8e638ca 251 Cd[i][j] = *Cd_in;
benson516 0:0699f8e638ca 252 Cd_in++;
benson516 0:0699f8e638ca 253 }
benson516 0:0699f8e638ca 254 }
benson516 1:085d41355949 255
benson516 0:0699f8e638ca 256 }
benson516 0:0699f8e638ca 257 // Assignment for observer Gain
benson516 0:0699f8e638ca 258 void STATES_OBSERVER_DELAY::assign_Ld(float* Ld_in, size_t n_in, size_t d_in, size_t q_in){
benson516 0:0699f8e638ca 259 // Ld_in is the pointer of a mutidimentional array with size n_in*(d_in+1) by q_in
benson516 0:0699f8e638ca 260 if (n != n_in || d != d_in || q != q_in){
benson516 0:0699f8e638ca 261 n = n_in;
benson516 0:0699f8e638ca 262 q = q_in;
benson516 0:0699f8e638ca 263 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 264 zeros_q.resize(q, 0.0);
benson516 0:0699f8e638ca 265 //
benson516 0:0699f8e638ca 266 if (d != d_in){
benson516 0:0699f8e638ca 267 d = d_in;
benson516 0:0699f8e638ca 268 states_est_buffer.Init((d+1), zeros_n);
benson516 0:0699f8e638ca 269 }
benson516 0:0699f8e638ca 270 Ld.assign(n*(d+1), zeros_q);
benson516 0:0699f8e638ca 271 }
benson516 0:0699f8e638ca 272 //
benson516 0:0699f8e638ca 273 for (size_t i = 0; i < n*(d+1); ++i){
benson516 0:0699f8e638ca 274 for (size_t j = 0; j < q; ++j){
benson516 0:0699f8e638ca 275 // Ld[i][j] = Ld_in[i][j];
benson516 0:0699f8e638ca 276 Ld[i][j] = *Ld_in;
benson516 0:0699f8e638ca 277 Ld_in++;
benson516 0:0699f8e638ca 278 }
benson516 0:0699f8e638ca 279 }
benson516 0:0699f8e638ca 280 }
benson516 0:0699f8e638ca 281
benson516 0:0699f8e638ca 282
benson516 0:0699f8e638ca 283 void STATES_OBSERVER_DELAY::iterateOnce(void){
benson516 0:0699f8e638ca 284 if(!enable){
benson516 0:0699f8e638ca 285 if (flag_reset){
benson516 0:0699f8e638ca 286 reset();
benson516 0:0699f8e638ca 287 flag_reset = false;
benson516 0:0699f8e638ca 288 }
benson516 0:0699f8e638ca 289 return;
benson516 0:0699f8e638ca 290 }
benson516 0:0699f8e638ca 291
benson516 1:085d41355949 292
benson516 0:0699f8e638ca 293 // Inputs of the observer: sys_inputs, sys_outputs
benson516 0:0699f8e638ca 294 //
benson516 0:0699f8e638ca 295 // Get the delayed estimation of states
benson516 0:0699f8e638ca 296 // states_est_delay_d = states_est_buffer.Get(d); // Get the element that is d samples ago
benson516 0:0699f8e638ca 297 //
benson516 0:0699f8e638ca 298 if (enable_Cd){
benson516 0:0699f8e638ca 299 // y_est = Cd*states_est_delay_d
benson516 0:0699f8e638ca 300 y_est = Mat_multiply_Vec(Cd, states_est_buffer.Get(d));
benson516 0:0699f8e638ca 301 // est_error = y_est - sys_outputs
benson516 0:0699f8e638ca 302 est_error = Get_VectorPlus(y_est,sys_outputs,true); // minus
benson516 0:0699f8e638ca 303 }else{
benson516 0:0699f8e638ca 304 // est_error = y_est - sys_outputs
benson516 0:0699f8e638ca 305 est_error = Get_VectorPlus(states_est_buffer.Get(d), sys_outputs,true); // minus
benson516 0:0699f8e638ca 306 }
benson516 0:0699f8e638ca 307
benson516 1:085d41355949 308
benson516 0:0699f8e638ca 309 // Get estimation
benson516 0:0699f8e638ca 310 state_est = states_est_buffer.Get(0);
benson516 0:0699f8e638ca 311
benson516 0:0699f8e638ca 312 //-------------------------------Update-----------------------------//
benson516 0:0699f8e638ca 313 static vector<vector<float> >::iterator it_Ld;
benson516 0:0699f8e638ca 314 it_Ld = Ld.begin(); // it_Ld points to L0
benson516 0:0699f8e638ca 315 //
benson516 0:0699f8e638ca 316 static vector<float> states_new;
benson516 0:0699f8e638ca 317 // x_(k+1) = (Ad*x_k + Bd*sys_inputs + sys_extraDisturbance) - L*est_error)
benson516 0:0699f8e638ca 318 states_new = Get_VectorPlus(Mat_multiply_Vec(Ad,state_est), Mat_multiply_Vec(Bd,sys_inputs), false); // plus
benson516 0:0699f8e638ca 319 Get_VectorIncrement(states_new, sys_extraDisturbance, false); // +=, extra disturbances
benson516 0:0699f8e638ca 320 Get_VectorIncrement(states_new, Partial_Mat_multiply_Vec(it_Ld, n, est_error), true); // -=
benson516 0:0699f8e638ca 321 // states_new is equal to x_(k+1) and will be used to .Insert() the buffer
benson516 0:0699f8e638ca 322
benson516 0:0699f8e638ca 323 // Update all the other states
benson516 0:0699f8e638ca 324 // from k to (k-d)+1, totally d samples
benson516 0:0699f8e638ca 325 // Currently it_Ld is pointing to "L1"
benson516 0:0699f8e638ca 326 for (size_t i = 0; i < d; ++i){
benson516 0:0699f8e638ca 327 // it_Ld will be automatically updated during iteration
benson516 0:0699f8e638ca 328 states_est_buffer.Increase(i, Partial_Mat_multiply_Vec(it_Ld, n, est_error), true); // -=
benson516 0:0699f8e638ca 329 }
benson516 0:0699f8e638ca 330
benson516 0:0699f8e638ca 331 // Rotate the buffer
benson516 0:0699f8e638ca 332 states_est_buffer.Insert(states_new);
benson516 1:085d41355949 333
benson516 1:085d41355949 334
benson516 1:085d41355949 335 /*
benson516 1:085d41355949 336 // No-delay version
benson516 1:085d41355949 337 //----------------------------------------//
benson516 1:085d41355949 338 if (enable_Cd){
benson516 1:085d41355949 339 // y_est = Cd*states_est_delay_d
benson516 1:085d41355949 340 y_est = Mat_multiply_Vec(Cd, state_est);
benson516 1:085d41355949 341 // est_error = y_est - sys_outputs
benson516 1:085d41355949 342 est_error = Get_VectorPlus(y_est,sys_outputs, true); // minus
benson516 1:085d41355949 343 }else{
benson516 1:085d41355949 344 // est_error = y_est - sys_outputs
benson516 1:085d41355949 345 est_error = Get_VectorPlus(state_est, sys_outputs, true); // minus
benson516 1:085d41355949 346 }
benson516 1:085d41355949 347 //
benson516 1:085d41355949 348 state_est = Get_VectorPlus(Mat_multiply_Vec(Ad,state_est), Mat_multiply_Vec(Bd,sys_inputs), false); // plus
benson516 1:085d41355949 349 // Get_VectorIncrement(state_est, sys_extraDisturbance, false); // +=, extra disturbances
benson516 1:085d41355949 350 Get_VectorIncrement(state_est, Mat_multiply_Vec(Ld, est_error), true); // -=
benson516 1:085d41355949 351 */
benson516 0:0699f8e638ca 352 }
benson516 0:0699f8e638ca 353
benson516 0:0699f8e638ca 354 // Utilities
benson516 0:0699f8e638ca 355 void STATES_OBSERVER_DELAY::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:085d41355949 356
benson516 0:0699f8e638ca 357 // Size check
benson516 0:0699f8e638ca 358 if (v_out.size() != m_left.size()){
benson516 0:0699f8e638ca 359 v_out.resize(m_left.size());
benson516 0:0699f8e638ca 360 }
benson516 1:085d41355949 361
benson516 1:085d41355949 362 /*
benson516 1:085d41355949 363 //
benson516 1:085d41355949 364 static vector<float>::iterator it_out;
benson516 1:085d41355949 365 static vector<float>::iterator it_m_row;
benson516 1:085d41355949 366 static vector<float>::iterator it_v;
benson516 0:0699f8e638ca 367 //
benson516 0:0699f8e638ca 368 it_out = v_out.begin();
benson516 0:0699f8e638ca 369 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:0699f8e638ca 370 *it_out = 0.0;
benson516 1:085d41355949 371 it_m_row = vector<vector<float> >(m_left)[i].begin();
benson516 1:085d41355949 372 it_v = vector<float>(v_right).begin();
benson516 0:0699f8e638ca 373 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:0699f8e638ca 374 // *it_out += m_left[i][j] * v_right[j];
benson516 0:0699f8e638ca 375 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:0699f8e638ca 376 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 377 }else{
benson516 0:0699f8e638ca 378 // (*it_out) += 0.0
benson516 0:0699f8e638ca 379 }
benson516 0:0699f8e638ca 380 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 381 //
benson516 0:0699f8e638ca 382 it_m_row++;
benson516 0:0699f8e638ca 383 it_v++;
benson516 0:0699f8e638ca 384 }
benson516 0:0699f8e638ca 385 it_out++;
benson516 0:0699f8e638ca 386 }
benson516 1:085d41355949 387 */
benson516 1:085d41355949 388
benson516 1:085d41355949 389 // Indexing
benson516 1:085d41355949 390 for (size_t i = 0; i < m_left.size(); ++i){
benson516 1:085d41355949 391 //
benson516 1:085d41355949 392 v_out[i] = 0.0;
benson516 1:085d41355949 393 //
benson516 1:085d41355949 394 for (size_t j = 0; j < v_right.size(); ++j){
benson516 1:085d41355949 395 if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
benson516 1:085d41355949 396 v_out[i] += m_left[i][j]*v_right[j];
benson516 1:085d41355949 397 }
benson516 1:085d41355949 398 }
benson516 1:085d41355949 399
benson516 0:0699f8e638ca 400 }
benson516 0:0699f8e638ca 401 vector<float> STATES_OBSERVER_DELAY::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:0699f8e638ca 402 static vector<float> v_out;
benson516 0:0699f8e638ca 403 // Size check
benson516 0:0699f8e638ca 404 if (v_out.size() != m_left.size()){
benson516 0:0699f8e638ca 405 v_out.resize(m_left.size());
benson516 0:0699f8e638ca 406 }
benson516 1:085d41355949 407 /*
benson516 0:0699f8e638ca 408 // Iterators
benson516 0:0699f8e638ca 409 static vector<float>::iterator it_out;
benson516 1:085d41355949 410 static vector<float>::iterator it_m_row;
benson516 1:085d41355949 411 static vector<float>::iterator it_v;
benson516 0:0699f8e638ca 412 //
benson516 0:0699f8e638ca 413 it_out = v_out.begin();
benson516 0:0699f8e638ca 414 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:0699f8e638ca 415 *it_out = 0.0;
benson516 1:085d41355949 416 it_m_row = vector<vector<float> >(m_left)[i].begin();
benson516 1:085d41355949 417 it_v = vector<float>(v_right).begin();
benson516 0:0699f8e638ca 418 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:0699f8e638ca 419 // *it_out += m_left[i][j] * v_right[j];
benson516 0:0699f8e638ca 420 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:0699f8e638ca 421 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 422 }else{
benson516 0:0699f8e638ca 423 // (*it_out) += 0.0
benson516 0:0699f8e638ca 424 }
benson516 0:0699f8e638ca 425 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 426 //
benson516 0:0699f8e638ca 427 it_m_row++;
benson516 0:0699f8e638ca 428 it_v++;
benson516 0:0699f8e638ca 429 }
benson516 0:0699f8e638ca 430 it_out++;
benson516 0:0699f8e638ca 431 }
benson516 1:085d41355949 432 */
benson516 1:085d41355949 433
benson516 1:085d41355949 434 // Indexing
benson516 1:085d41355949 435 for (size_t i = 0; i < m_left.size(); ++i){
benson516 1:085d41355949 436 //
benson516 1:085d41355949 437 v_out[i] = 0.0;
benson516 1:085d41355949 438 //
benson516 1:085d41355949 439 for (size_t j = 0; j < v_right.size(); ++j){
benson516 1:085d41355949 440 if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
benson516 1:085d41355949 441 v_out[i] += m_left[i][j]*v_right[j];
benson516 1:085d41355949 442 }
benson516 1:085d41355949 443 }
benson516 1:085d41355949 444
benson516 0:0699f8e638ca 445 return v_out;
benson516 0:0699f8e638ca 446 }
benson516 0:0699f8e638ca 447 vector<float> STATES_OBSERVER_DELAY::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
benson516 0:0699f8e638ca 448 {
benson516 0:0699f8e638ca 449 static vector<float> v_c;
benson516 0:0699f8e638ca 450 // Size check
benson516 0:0699f8e638ca 451 if (v_c.size() != v_a.size()){
benson516 0:0699f8e638ca 452 v_c.resize(v_a.size());
benson516 0:0699f8e638ca 453 }
benson516 0:0699f8e638ca 454 //
benson516 0:0699f8e638ca 455 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:0699f8e638ca 456 if (is_minus){
benson516 0:0699f8e638ca 457 v_c[i] = v_a[i] - v_b[i];
benson516 0:0699f8e638ca 458 }else{
benson516 0:0699f8e638ca 459 v_c[i] = v_a[i] + v_b[i];
benson516 0:0699f8e638ca 460 }
benson516 0:0699f8e638ca 461 }
benson516 0:0699f8e638ca 462 return v_c;
benson516 0:0699f8e638ca 463 }
benson516 0:0699f8e638ca 464 vector<float> STATES_OBSERVER_DELAY::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 0:0699f8e638ca 465 {
benson516 0:0699f8e638ca 466 static vector<float> v_c;
benson516 0:0699f8e638ca 467 // Size check
benson516 0:0699f8e638ca 468 if (v_c.size() != v_a.size()){
benson516 0:0699f8e638ca 469 v_c.resize(v_a.size());
benson516 0:0699f8e638ca 470 }
benson516 0:0699f8e638ca 471 // for pure negative
benson516 0:0699f8e638ca 472 if (scale == -1.0){
benson516 0:0699f8e638ca 473 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:0699f8e638ca 474 v_c[i] = -v_a[i];
benson516 0:0699f8e638ca 475 }
benson516 0:0699f8e638ca 476 return v_c;
benson516 0:0699f8e638ca 477 }
benson516 0:0699f8e638ca 478 // else
benson516 0:0699f8e638ca 479 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:0699f8e638ca 480 v_c[i] = scale*v_a[i];
benson516 0:0699f8e638ca 481
benson516 0:0699f8e638ca 482 }
benson516 0:0699f8e638ca 483 return v_c;
benson516 0:0699f8e638ca 484 }
benson516 0:0699f8e638ca 485 // Increment
benson516 0:0699f8e638ca 486 void STATES_OBSERVER_DELAY::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
benson516 0:0699f8e638ca 487 // Size check
benson516 0:0699f8e638ca 488 if (v_a.size() != v_b.size()){
benson516 0:0699f8e638ca 489 v_a.resize(v_b.size());
benson516 0:0699f8e638ca 490 }
benson516 0:0699f8e638ca 491 //
benson516 0:0699f8e638ca 492 if (is_minus){ // -=
benson516 0:0699f8e638ca 493 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:0699f8e638ca 494 v_a[i] -= v_b[i];
benson516 0:0699f8e638ca 495 }
benson516 0:0699f8e638ca 496 }else{ // +=
benson516 0:0699f8e638ca 497 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:0699f8e638ca 498 v_a[i] += v_b[i];
benson516 0:0699f8e638ca 499 }
benson516 0:0699f8e638ca 500 }
benson516 0:0699f8e638ca 501
benson516 0:0699f8e638ca 502 }
benson516 0:0699f8e638ca 503 // Partial matrix multiplication
benson516 0:0699f8e638ca 504 vector<float> STATES_OBSERVER_DELAY::Partial_Mat_multiply_Vec(vector<vector<float> >::iterator &it_m_left, size_t numRow_m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:0699f8e638ca 505 static vector<float> v_out;
benson516 0:0699f8e638ca 506 // Size check
benson516 0:0699f8e638ca 507 if (v_out.size() != numRow_m_left){
benson516 0:0699f8e638ca 508 v_out.resize(numRow_m_left);
benson516 0:0699f8e638ca 509 }
benson516 1:085d41355949 510 /*
benson516 0:0699f8e638ca 511 // Iterators
benson516 0:0699f8e638ca 512 static vector<float>::iterator it_out;
benson516 0:0699f8e638ca 513 static vector<float>::iterator it_m_row;
benson516 1:085d41355949 514 static vector<float>::iterator it_v;
benson516 0:0699f8e638ca 515 //
benson516 0:0699f8e638ca 516 it_out = v_out.begin();
benson516 0:0699f8e638ca 517 for (size_t i = 0; i < numRow_m_left; ++i){
benson516 0:0699f8e638ca 518 *it_out = 0.0;
benson516 0:0699f8e638ca 519 it_m_row = (*it_m_left).begin();
benson516 1:085d41355949 520 it_v = vector<float>(v_right).begin();
benson516 0:0699f8e638ca 521 for (size_t j = 0; j < (*it_m_left).size(); ++j){
benson516 0:0699f8e638ca 522 // *it_out += m_left[i][j] * v_right[j];
benson516 0:0699f8e638ca 523 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:0699f8e638ca 524 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 525 }else{
benson516 0:0699f8e638ca 526 // (*it_out) += 0.0
benson516 0:0699f8e638ca 527 }
benson516 0:0699f8e638ca 528 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 529 //
benson516 0:0699f8e638ca 530 it_m_row++;
benson516 0:0699f8e638ca 531 it_v++;
benson516 0:0699f8e638ca 532 }
benson516 0:0699f8e638ca 533 it_out++;
benson516 0:0699f8e638ca 534 it_m_left++;
benson516 0:0699f8e638ca 535 }
benson516 1:085d41355949 536 */
benson516 1:085d41355949 537
benson516 1:085d41355949 538 //
benson516 1:085d41355949 539 // Indexing
benson516 1:085d41355949 540 for (size_t i = 0; i < numRow_m_left; ++i){
benson516 1:085d41355949 541 //
benson516 1:085d41355949 542 v_out[i] = 0.0;
benson516 1:085d41355949 543 //
benson516 1:085d41355949 544 for (size_t j = 0; j < v_right.size(); ++j){
benson516 1:085d41355949 545 if ( (*it_m_left)[j] != 0.0 && v_right[j] != 0.0)
benson516 1:085d41355949 546 v_out[i] += (*it_m_left)[j]*v_right[j];
benson516 1:085d41355949 547 }
benson516 1:085d41355949 548 //
benson516 1:085d41355949 549 it_m_left++;
benson516 1:085d41355949 550 }
benson516 1:085d41355949 551
benson516 1:085d41355949 552
benson516 0:0699f8e638ca 553 return v_out;
benson516 0:0699f8e638ca 554 }