Chun Feng Huang / STATES_OBSERVER_DELAY

An state-observer that deals with delay in measurements, also compatible with the standard state-observer

STATES_OBSERVER_DELAY.cpp@0:0699f8e638ca, 2017-01-11 (annotated)

Committer:
benson516
Date:
Wed Jan 11 09:30:18 2017 +0000
Revision:
0:0699f8e638ca
Child:
1:085d41355949
The first compilation of this library

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 0:0699f8e638ca 21 }else{
benson516 0:0699f8e638ca 22 enable_Cd = true; // using Cd
benson516 0:0699f8e638ca 23 Cd.assign(q,zeros_n);
benson516 0:0699f8e638ca 24 }
benson516 0:0699f8e638ca 25 // Gain matrix
benson516 0:0699f8e638ca 26 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 27
benson516 0:0699f8e638ca 28 // Input-signals of the observer
benson516 0:0699f8e638ca 29 sys_inputs = zeros_p;
benson516 0:0699f8e638ca 30 sys_outputs = zeros_q;
benson516 0:0699f8e638ca 31 sys_extraDisturbance = zeros_n;
benson516 0:0699f8e638ca 32 // Variables of the observer
benson516 0:0699f8e638ca 33 states_est_buffer.Reset(zeros_n);
benson516 0:0699f8e638ca 34 state_est = zeros_n;
benson516 0:0699f8e638ca 35 // states_est_delay_d = zeros_n;
benson516 0:0699f8e638ca 36 y_est = zeros_q;
benson516 0:0699f8e638ca 37 est_error = zeros_q;
benson516 0:0699f8e638ca 38 }
benson516 0:0699f8e638ca 39 void STATES_OBSERVER_DELAY::start(){
benson516 0:0699f8e638ca 40 enable = true;
benson516 0:0699f8e638ca 41 }
benson516 0:0699f8e638ca 42 void STATES_OBSERVER_DELAY::stop(){
benson516 0:0699f8e638ca 43 if (!enable){
benson516 0:0699f8e638ca 44 return;
benson516 0:0699f8e638ca 45 }
benson516 0:0699f8e638ca 46 enable = false;
benson516 0:0699f8e638ca 47 flag_reset = true;
benson516 0:0699f8e638ca 48 }
benson516 0:0699f8e638ca 49 void STATES_OBSERVER_DELAY::reset(){
benson516 0:0699f8e638ca 50 //
benson516 0:0699f8e638ca 51 // Input-signals of the observer
benson516 0:0699f8e638ca 52 sys_inputs = zeros_p;
benson516 0:0699f8e638ca 53 sys_outputs = zeros_q;
benson516 0:0699f8e638ca 54 // Variables of the observer
benson516 0:0699f8e638ca 55 states_est_buffer.Reset(zeros_n);
benson516 0:0699f8e638ca 56 state_est = zeros_n;
benson516 0:0699f8e638ca 57 // states_est_delay_d = zeros_n;
benson516 0:0699f8e638ca 58 y_est = zeros_q;
benson516 0:0699f8e638ca 59 est_error = zeros_q;
benson516 0:0699f8e638ca 60 }
benson516 0:0699f8e638ca 61
benson516 0:0699f8e638ca 62
benson516 0:0699f8e638ca 63 // Assignments for observer
benson516 0:0699f8e638ca 64 // Assign continuous-time version of system matrices
benson516 0:0699f8e638ca 65 void STATES_OBSERVER_DELAY::assign_At(float* At_in, size_t n_in){ // Continuous-time version
benson516 0:0699f8e638ca 66 // At_in is the pointer of a mutidimentional array with size n_in by n_in
benson516 0:0699f8e638ca 67 if (n != n_in){
benson516 0:0699f8e638ca 68 n = n_in;
benson516 0:0699f8e638ca 69 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 70 Ad.assign(n, zeros_n);
benson516 0:0699f8e638ca 71 }
benson516 0:0699f8e638ca 72 //
benson516 0:0699f8e638ca 73 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 74 for (size_t j = 0; j < n; ++j){
benson516 0:0699f8e638ca 75 // Ad[i][j] = At_in[i][j];
benson516 0:0699f8e638ca 76 Ad[i][j] = (*At_in)*Ts;
benson516 0:0699f8e638ca 77 //
benson516 0:0699f8e638ca 78 if (i == j){
benson516 0:0699f8e638ca 79 Ad[i][j] += 1.0;
benson516 0:0699f8e638ca 80 }
benson516 0:0699f8e638ca 81 //
benson516 0:0699f8e638ca 82 At_in++;
benson516 0:0699f8e638ca 83 }
benson516 0:0699f8e638ca 84 }
benson516 0:0699f8e638ca 85 }
benson516 0:0699f8e638ca 86 void STATES_OBSERVER_DELAY::assign_Bt(float* Bt_in, size_t n_in, size_t p_in){ // Continuous-time version
benson516 0:0699f8e638ca 87 // Bt_in is the pointer of a mutidimentional array with size n_in by p_in
benson516 0:0699f8e638ca 88 if (n != n_in || p != p_in){
benson516 0:0699f8e638ca 89 n = n_in;
benson516 0:0699f8e638ca 90 p = p_in;
benson516 0:0699f8e638ca 91 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 92 zeros_p.resize(p, 0.0);
benson516 0:0699f8e638ca 93 Bd.assign(n, zeros_p);
benson516 0:0699f8e638ca 94 }
benson516 0:0699f8e638ca 95 //
benson516 0:0699f8e638ca 96 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 97 for (size_t j = 0; j < p; ++j){
benson516 0:0699f8e638ca 98 // Bd[i][j] = Bt_in[i][j];
benson516 0:0699f8e638ca 99 Bd[i][j] = (*Bt_in)*Ts;
benson516 0:0699f8e638ca 100 Bt_in++;
benson516 0:0699f8e638ca 101 }
benson516 0:0699f8e638ca 102 }
benson516 0:0699f8e638ca 103 }
benson516 0:0699f8e638ca 104 // ** Assign the continuous-time version of system matrices by matrices
benson516 0:0699f8e638ca 105 void STATES_OBSERVER_DELAY::assign_At(const vector<vector<float> > &At_in){
benson516 0:0699f8e638ca 106 size_t n_in = At_in.size();
benson516 0:0699f8e638ca 107 if (n != n_in){
benson516 0:0699f8e638ca 108 n = n_in;
benson516 0:0699f8e638ca 109 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 110 Ad.assign(n, zeros_n);
benson516 0:0699f8e638ca 111 }
benson516 0:0699f8e638ca 112 // Ad
benson516 0:0699f8e638ca 113 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 114 for (size_t j = 0; j < n; ++j){
benson516 0:0699f8e638ca 115 //
benson516 0:0699f8e638ca 116 Ad[i][j] = Ts*At_in[i][j];
benson516 0:0699f8e638ca 117 //
benson516 0:0699f8e638ca 118 if (i == j){
benson516 0:0699f8e638ca 119 Ad[i][j] += 1.0;
benson516 0:0699f8e638ca 120 }
benson516 0:0699f8e638ca 121 }
benson516 0:0699f8e638ca 122 }
benson516 0:0699f8e638ca 123 }
benson516 0:0699f8e638ca 124 void STATES_OBSERVER_DELAY::assign_Bt(const vector<vector<float> > &Bt_in){
benson516 0:0699f8e638ca 125 size_t n_in = Bt_in.size();
benson516 0:0699f8e638ca 126 size_t p_in = Bt_in[0].size();
benson516 0:0699f8e638ca 127 if (n != n_in || p != p_in){
benson516 0:0699f8e638ca 128 n = n_in;
benson516 0:0699f8e638ca 129 p = p_in;
benson516 0:0699f8e638ca 130 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 131 zeros_p.resize(p, 0.0);
benson516 0:0699f8e638ca 132 Bd.assign(n, zeros_p);
benson516 0:0699f8e638ca 133 }
benson516 0:0699f8e638ca 134 // Bd
benson516 0:0699f8e638ca 135 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 136 for (size_t j = 0; j < p; ++j){
benson516 0:0699f8e638ca 137 // Bd[i][j] = Bt_in[i][j];
benson516 0:0699f8e638ca 138 Bd[i][j] = Ts*Bt_in[i][j];
benson516 0:0699f8e638ca 139 }
benson516 0:0699f8e638ca 140 }
benson516 0:0699f8e638ca 141 }
benson516 0:0699f8e638ca 142 // Assign discrete-time version of system matrices directly
benson516 0:0699f8e638ca 143 void STATES_OBSERVER_DELAY::assign_Ad(float* Ad_in, size_t n_in){ // Discrete-time version
benson516 0:0699f8e638ca 144 // Ad_in is the pointer of a mutidimentional array with size n_in by n_in
benson516 0:0699f8e638ca 145 if (n != n_in){
benson516 0:0699f8e638ca 146 n = n_in;
benson516 0:0699f8e638ca 147 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 148 Ad.assign(n, zeros_n);
benson516 0:0699f8e638ca 149 }
benson516 0:0699f8e638ca 150 //
benson516 0:0699f8e638ca 151 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 152 for (size_t j = 0; j < n; ++j){
benson516 0:0699f8e638ca 153 // Ad[i][j] = Ad_in[i][j];
benson516 0:0699f8e638ca 154 Ad[i][j] = *Ad_in;
benson516 0:0699f8e638ca 155 Ad_in++;
benson516 0:0699f8e638ca 156 }
benson516 0:0699f8e638ca 157 }
benson516 0:0699f8e638ca 158 }
benson516 0:0699f8e638ca 159 void STATES_OBSERVER_DELAY::assign_Bd(float* Bd_in, size_t n_in, size_t p_in){ // Discrete-time version
benson516 0:0699f8e638ca 160 // Bd_in is the pointer of a mutidimentional array with size n_in by p_in
benson516 0:0699f8e638ca 161 if (n != n_in || p != p_in){
benson516 0:0699f8e638ca 162 n = n_in;
benson516 0:0699f8e638ca 163 p = p_in;
benson516 0:0699f8e638ca 164 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 165 zeros_p.resize(p, 0.0);
benson516 0:0699f8e638ca 166 Bd.assign(n, zeros_p);
benson516 0:0699f8e638ca 167 }
benson516 0:0699f8e638ca 168 //
benson516 0:0699f8e638ca 169 for (size_t i = 0; i < n; ++i){
benson516 0:0699f8e638ca 170 for (size_t j = 0; j < p; ++j){
benson516 0:0699f8e638ca 171 // Bd[i][j] = Bd_in[i][j];
benson516 0:0699f8e638ca 172 Bd[i][j] = *Bd_in;
benson516 0:0699f8e638ca 173 Bd_in++;
benson516 0:0699f8e638ca 174 }
benson516 0:0699f8e638ca 175 }
benson516 0:0699f8e638ca 176 }
benson516 0:0699f8e638ca 177 //
benson516 0:0699f8e638ca 178 void STATES_OBSERVER_DELAY::assign_Cd(float* Cd_in, size_t q_in, size_t n_in){
benson516 0:0699f8e638ca 179 // Cd_in is the pointer of a mutidimentional array with size q_in by n_in
benson516 0:0699f8e638ca 180 if (n != n_in || q != q_in){
benson516 0:0699f8e638ca 181 n = n_in;
benson516 0:0699f8e638ca 182 q = q_in;
benson516 0:0699f8e638ca 183 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 184 zeros_q.resize(q, 0.0);
benson516 0:0699f8e638ca 185 Cd.assign(q, zeros_n);
benson516 0:0699f8e638ca 186 }
benson516 0:0699f8e638ca 187 //
benson516 0:0699f8e638ca 188 for (size_t i = 0; i < q; ++i){
benson516 0:0699f8e638ca 189 for (size_t j = 0; j < n; ++j){
benson516 0:0699f8e638ca 190 // Cd[i][j] = Cd_in[i][j];
benson516 0:0699f8e638ca 191 Cd[i][j] = *Cd_in;
benson516 0:0699f8e638ca 192 Cd_in++;
benson516 0:0699f8e638ca 193 }
benson516 0:0699f8e638ca 194 }
benson516 0:0699f8e638ca 195 // q = n if not using Cd
benson516 0:0699f8e638ca 196 if (q == 0){
benson516 0:0699f8e638ca 197 enable_Cd = false; // Default: not using Cd
benson516 0:0699f8e638ca 198 q = n;
benson516 0:0699f8e638ca 199 zeros_q.resize(q, 0.0);
benson516 0:0699f8e638ca 200 }else{
benson516 0:0699f8e638ca 201 enable_Cd = true; // using Cd
benson516 0:0699f8e638ca 202 Cd.assign(q,zeros_n);
benson516 0:0699f8e638ca 203 }
benson516 0:0699f8e638ca 204 }
benson516 0:0699f8e638ca 205 // Assignment for observer Gain
benson516 0:0699f8e638ca 206 void STATES_OBSERVER_DELAY::assign_Ld(float* Ld_in, size_t n_in, size_t d_in, size_t q_in){
benson516 0:0699f8e638ca 207 // Ld_in is the pointer of a mutidimentional array with size n_in*(d_in+1) by q_in
benson516 0:0699f8e638ca 208 if (n != n_in || d != d_in || q != q_in){
benson516 0:0699f8e638ca 209 n = n_in;
benson516 0:0699f8e638ca 210 q = q_in;
benson516 0:0699f8e638ca 211 zeros_n.resize(n, 0.0);
benson516 0:0699f8e638ca 212 zeros_q.resize(q, 0.0);
benson516 0:0699f8e638ca 213 //
benson516 0:0699f8e638ca 214 if (d != d_in){
benson516 0:0699f8e638ca 215 d = d_in;
benson516 0:0699f8e638ca 216 states_est_buffer.Init((d+1), zeros_n);
benson516 0:0699f8e638ca 217 }
benson516 0:0699f8e638ca 218 Ld.assign(n*(d+1), zeros_q);
benson516 0:0699f8e638ca 219 }
benson516 0:0699f8e638ca 220 //
benson516 0:0699f8e638ca 221 for (size_t i = 0; i < n*(d+1); ++i){
benson516 0:0699f8e638ca 222 for (size_t j = 0; j < q; ++j){
benson516 0:0699f8e638ca 223 // Ld[i][j] = Ld_in[i][j];
benson516 0:0699f8e638ca 224 Ld[i][j] = *Ld_in;
benson516 0:0699f8e638ca 225 Ld_in++;
benson516 0:0699f8e638ca 226 }
benson516 0:0699f8e638ca 227 }
benson516 0:0699f8e638ca 228 }
benson516 0:0699f8e638ca 229
benson516 0:0699f8e638ca 230
benson516 0:0699f8e638ca 231 void STATES_OBSERVER_DELAY::iterateOnce(void){
benson516 0:0699f8e638ca 232 if(!enable){
benson516 0:0699f8e638ca 233 if (flag_reset){
benson516 0:0699f8e638ca 234 reset();
benson516 0:0699f8e638ca 235 flag_reset = false;
benson516 0:0699f8e638ca 236 }
benson516 0:0699f8e638ca 237 return;
benson516 0:0699f8e638ca 238 }
benson516 0:0699f8e638ca 239
benson516 0:0699f8e638ca 240 // Inputs of the observer: sys_inputs, sys_outputs
benson516 0:0699f8e638ca 241 //
benson516 0:0699f8e638ca 242 // Get the delayed estimation of states
benson516 0:0699f8e638ca 243 // states_est_delay_d = states_est_buffer.Get(d); // Get the element that is d samples ago
benson516 0:0699f8e638ca 244 //
benson516 0:0699f8e638ca 245 if (enable_Cd){
benson516 0:0699f8e638ca 246 // y_est = Cd*states_est_delay_d
benson516 0:0699f8e638ca 247 y_est = Mat_multiply_Vec(Cd, states_est_buffer.Get(d));
benson516 0:0699f8e638ca 248 // est_error = y_est - sys_outputs
benson516 0:0699f8e638ca 249 est_error = Get_VectorPlus(y_est,sys_outputs,true); // minus
benson516 0:0699f8e638ca 250 }else{
benson516 0:0699f8e638ca 251 // est_error = y_est - sys_outputs
benson516 0:0699f8e638ca 252 est_error = Get_VectorPlus(states_est_buffer.Get(d), sys_outputs,true); // minus
benson516 0:0699f8e638ca 253 }
benson516 0:0699f8e638ca 254
benson516 0:0699f8e638ca 255 // Get estimation
benson516 0:0699f8e638ca 256 state_est = states_est_buffer.Get(0);
benson516 0:0699f8e638ca 257
benson516 0:0699f8e638ca 258 //-------------------------------Update-----------------------------//
benson516 0:0699f8e638ca 259 static vector<vector<float> >::iterator it_Ld;
benson516 0:0699f8e638ca 260 it_Ld = Ld.begin(); // it_Ld points to L0
benson516 0:0699f8e638ca 261 //
benson516 0:0699f8e638ca 262 static vector<float> states_new;
benson516 0:0699f8e638ca 263 // x_(k+1) = (Ad*x_k + Bd*sys_inputs + sys_extraDisturbance) - L*est_error)
benson516 0:0699f8e638ca 264 states_new = Get_VectorPlus(Mat_multiply_Vec(Ad,state_est), Mat_multiply_Vec(Bd,sys_inputs), false); // plus
benson516 0:0699f8e638ca 265 Get_VectorIncrement(states_new, sys_extraDisturbance, false); // +=, extra disturbances
benson516 0:0699f8e638ca 266 Get_VectorIncrement(states_new, Partial_Mat_multiply_Vec(it_Ld, n, est_error), true); // -=
benson516 0:0699f8e638ca 267 // states_new is equal to x_(k+1) and will be used to .Insert() the buffer
benson516 0:0699f8e638ca 268
benson516 0:0699f8e638ca 269 // Update all the other states
benson516 0:0699f8e638ca 270 // from k to (k-d)+1, totally d samples
benson516 0:0699f8e638ca 271 // Currently it_Ld is pointing to "L1"
benson516 0:0699f8e638ca 272 for (size_t i = 0; i < d; ++i){
benson516 0:0699f8e638ca 273 // it_Ld will be automatically updated during iteration
benson516 0:0699f8e638ca 274 states_est_buffer.Increase(i, Partial_Mat_multiply_Vec(it_Ld, n, est_error), true); // -=
benson516 0:0699f8e638ca 275 }
benson516 0:0699f8e638ca 276
benson516 0:0699f8e638ca 277 // Rotate the buffer
benson516 0:0699f8e638ca 278 states_est_buffer.Insert(states_new);
benson516 0:0699f8e638ca 279 }
benson516 0:0699f8e638ca 280
benson516 0:0699f8e638ca 281 // Utilities
benson516 0:0699f8e638ca 282 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 0:0699f8e638ca 283 static vector<float>::iterator it_out;
benson516 0:0699f8e638ca 284 static vector<const float>::iterator it_m_row;
benson516 0:0699f8e638ca 285 static vector<const float>::iterator it_v;
benson516 0:0699f8e638ca 286 // Size check
benson516 0:0699f8e638ca 287 if (v_out.size() != m_left.size()){
benson516 0:0699f8e638ca 288 v_out.resize(m_left.size());
benson516 0:0699f8e638ca 289 }
benson516 0:0699f8e638ca 290 //
benson516 0:0699f8e638ca 291 it_out = v_out.begin();
benson516 0:0699f8e638ca 292 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:0699f8e638ca 293 *it_out = 0.0;
benson516 0:0699f8e638ca 294 it_m_row = m_left[i].begin();
benson516 0:0699f8e638ca 295 it_v = v_right.begin();
benson516 0:0699f8e638ca 296 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:0699f8e638ca 297 // *it_out += m_left[i][j] * v_right[j];
benson516 0:0699f8e638ca 298 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:0699f8e638ca 299 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 300 }else{
benson516 0:0699f8e638ca 301 // (*it_out) += 0.0
benson516 0:0699f8e638ca 302 }
benson516 0:0699f8e638ca 303 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 304 //
benson516 0:0699f8e638ca 305 it_m_row++;
benson516 0:0699f8e638ca 306 it_v++;
benson516 0:0699f8e638ca 307 }
benson516 0:0699f8e638ca 308 it_out++;
benson516 0:0699f8e638ca 309 }
benson516 0:0699f8e638ca 310 }
benson516 0:0699f8e638ca 311 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 312 static vector<float> v_out;
benson516 0:0699f8e638ca 313 // Size check
benson516 0:0699f8e638ca 314 if (v_out.size() != m_left.size()){
benson516 0:0699f8e638ca 315 v_out.resize(m_left.size());
benson516 0:0699f8e638ca 316 }
benson516 0:0699f8e638ca 317 // Iterators
benson516 0:0699f8e638ca 318 static vector<float>::iterator it_out;
benson516 0:0699f8e638ca 319 static vector<const float>::iterator it_m_row;
benson516 0:0699f8e638ca 320 static vector<const float>::iterator it_v;
benson516 0:0699f8e638ca 321 //
benson516 0:0699f8e638ca 322 it_out = v_out.begin();
benson516 0:0699f8e638ca 323 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:0699f8e638ca 324 *it_out = 0.0;
benson516 0:0699f8e638ca 325 it_m_row = m_left[i].begin();
benson516 0:0699f8e638ca 326 it_v = v_right.begin();
benson516 0:0699f8e638ca 327 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:0699f8e638ca 328 // *it_out += m_left[i][j] * v_right[j];
benson516 0:0699f8e638ca 329 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:0699f8e638ca 330 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 331 }else{
benson516 0:0699f8e638ca 332 // (*it_out) += 0.0
benson516 0:0699f8e638ca 333 }
benson516 0:0699f8e638ca 334 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 335 //
benson516 0:0699f8e638ca 336 it_m_row++;
benson516 0:0699f8e638ca 337 it_v++;
benson516 0:0699f8e638ca 338 }
benson516 0:0699f8e638ca 339 it_out++;
benson516 0:0699f8e638ca 340 }
benson516 0:0699f8e638ca 341 return v_out;
benson516 0:0699f8e638ca 342 }
benson516 0:0699f8e638ca 343 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 344 {
benson516 0:0699f8e638ca 345 static vector<float> v_c;
benson516 0:0699f8e638ca 346 // Size check
benson516 0:0699f8e638ca 347 if (v_c.size() != v_a.size()){
benson516 0:0699f8e638ca 348 v_c.resize(v_a.size());
benson516 0:0699f8e638ca 349 }
benson516 0:0699f8e638ca 350 //
benson516 0:0699f8e638ca 351 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:0699f8e638ca 352 if (is_minus){
benson516 0:0699f8e638ca 353 v_c[i] = v_a[i] - v_b[i];
benson516 0:0699f8e638ca 354 }else{
benson516 0:0699f8e638ca 355 v_c[i] = v_a[i] + v_b[i];
benson516 0:0699f8e638ca 356 }
benson516 0:0699f8e638ca 357 }
benson516 0:0699f8e638ca 358 return v_c;
benson516 0:0699f8e638ca 359 }
benson516 0:0699f8e638ca 360 vector<float> STATES_OBSERVER_DELAY::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 0:0699f8e638ca 361 {
benson516 0:0699f8e638ca 362 static vector<float> v_c;
benson516 0:0699f8e638ca 363 // Size check
benson516 0:0699f8e638ca 364 if (v_c.size() != v_a.size()){
benson516 0:0699f8e638ca 365 v_c.resize(v_a.size());
benson516 0:0699f8e638ca 366 }
benson516 0:0699f8e638ca 367 // for pure negative
benson516 0:0699f8e638ca 368 if (scale == -1.0){
benson516 0:0699f8e638ca 369 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:0699f8e638ca 370 v_c[i] = -v_a[i];
benson516 0:0699f8e638ca 371 }
benson516 0:0699f8e638ca 372 return v_c;
benson516 0:0699f8e638ca 373 }
benson516 0:0699f8e638ca 374 // else
benson516 0:0699f8e638ca 375 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:0699f8e638ca 376 v_c[i] = scale*v_a[i];
benson516 0:0699f8e638ca 377
benson516 0:0699f8e638ca 378 }
benson516 0:0699f8e638ca 379 return v_c;
benson516 0:0699f8e638ca 380 }
benson516 0:0699f8e638ca 381 // Increment
benson516 0:0699f8e638ca 382 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 383 // Size check
benson516 0:0699f8e638ca 384 if (v_a.size() != v_b.size()){
benson516 0:0699f8e638ca 385 v_a.resize(v_b.size());
benson516 0:0699f8e638ca 386 }
benson516 0:0699f8e638ca 387 //
benson516 0:0699f8e638ca 388 if (is_minus){ // -=
benson516 0:0699f8e638ca 389 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:0699f8e638ca 390 v_a[i] -= v_b[i];
benson516 0:0699f8e638ca 391 }
benson516 0:0699f8e638ca 392 }else{ // +=
benson516 0:0699f8e638ca 393 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:0699f8e638ca 394 v_a[i] += v_b[i];
benson516 0:0699f8e638ca 395 }
benson516 0:0699f8e638ca 396 }
benson516 0:0699f8e638ca 397
benson516 0:0699f8e638ca 398 }
benson516 0:0699f8e638ca 399 // Partial matrix multiplication
benson516 0:0699f8e638ca 400 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 401 static vector<float> v_out;
benson516 0:0699f8e638ca 402 // Size check
benson516 0:0699f8e638ca 403 if (v_out.size() != numRow_m_left){
benson516 0:0699f8e638ca 404 v_out.resize(numRow_m_left);
benson516 0:0699f8e638ca 405 }
benson516 0:0699f8e638ca 406 // Iterators
benson516 0:0699f8e638ca 407 static vector<float>::iterator it_out;
benson516 0:0699f8e638ca 408 static vector<float>::iterator it_m_row;
benson516 0:0699f8e638ca 409 static vector<const float>::iterator it_v;
benson516 0:0699f8e638ca 410 //
benson516 0:0699f8e638ca 411 it_out = v_out.begin();
benson516 0:0699f8e638ca 412 for (size_t i = 0; i < numRow_m_left; ++i){
benson516 0:0699f8e638ca 413 *it_out = 0.0;
benson516 0:0699f8e638ca 414 it_m_row = (*it_m_left).begin();
benson516 0:0699f8e638ca 415 it_v = v_right.begin();
benson516 0:0699f8e638ca 416 for (size_t j = 0; j < (*it_m_left).size(); ++j){
benson516 0:0699f8e638ca 417 // *it_out += m_left[i][j] * v_right[j];
benson516 0:0699f8e638ca 418 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:0699f8e638ca 419 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 420 }else{
benson516 0:0699f8e638ca 421 // (*it_out) += 0.0
benson516 0:0699f8e638ca 422 }
benson516 0:0699f8e638ca 423 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:0699f8e638ca 424 //
benson516 0:0699f8e638ca 425 it_m_row++;
benson516 0:0699f8e638ca 426 it_v++;
benson516 0:0699f8e638ca 427 }
benson516 0:0699f8e638ca 428 it_out++;
benson516 0:0699f8e638ca 429 it_m_left++;
benson516 0:0699f8e638ca 430 }
benson516 0:0699f8e638ca 431 return v_out;
benson516 0:0699f8e638ca 432 }