Chun Feng Huang / Observer_ftRatio

An observer for estimating the friction-force ratio

Observer_ftRatio.cpp@1:fbae330f0e71, 2017-02-26 (annotated)

Committer:
benson516
Date:
Sun Feb 26 05:36:01 2017 +0000
Revision:
1:fbae330f0e71
Parent:
0:a936477fcd4a 
Ver. 1.00

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:a936477fcd4a 1 #include "Observer_ftRatio.h"
benson516 0:a936477fcd4a 2
benson516 0:a936477fcd4a 3 //
benson516 0:a936477fcd4a 4 float Dimension_ftOb[] = {
benson516 0:a936477fcd4a 5 #include "Dimensions_ftOb.txt" // Load the parameter Dimensions_ftOb
benson516 0:a936477fcd4a 6 };
benson516 0:a936477fcd4a 7 //
benson516 0:a936477fcd4a 8
benson516 0:a936477fcd4a 9 Observer_ftRatio::Observer_ftRatio(float samplingTime):
benson516 0:a936477fcd4a 10 Ts(samplingTime),
benson516 0:a936477fcd4a 11 OB(Dimension_ftOb[3], Dimension_ftOb[4], Dimension_ftOb[5], 0, samplingTime),
benson516 0:a936477fcd4a 12 KFID_ratio_ft_right(samplingTime, 1.0, 0.0, 1.0, 1.0, 1.0, false), // For identifing the ratio_ft of the right wheel
benson516 0:a936477fcd4a 13 KFID_ratio_ft_left(samplingTime, 1.0, 0.0, 1.0, 1.0, 1.0, false), // For identifing the ratio_ft of the left wheel
benson516 0:a936477fcd4a 14 SAT_ratio_ft(Dimension_ftOb[1], 1.0, 0.0),
benson516 0:a936477fcd4a 15 LPF_ratio_ft(Dimension_ftOb[1], samplingTime, 10.0, 1) // fc = 10 Hz
benson516 0:a936477fcd4a 16 {
benson516 0:a936477fcd4a 17 // To enble, run *.start() function
benson516 0:a936477fcd4a 18 enable = false;
benson516 0:a936477fcd4a 19 // Dimensions of the original system
benson516 0:a936477fcd4a 20 n = Dimension_ftOb[0];
benson516 0:a936477fcd4a 21 p = Dimension_ftOb[1];
benson516 0:a936477fcd4a 22 q = Dimension_ftOb[2];
benson516 0:a936477fcd4a 23 // Dimensions of the observer
benson516 0:a936477fcd4a 24 n_OB = Dimension_ftOb[3];
benson516 0:a936477fcd4a 25 p_OB = Dimension_ftOb[4];
benson516 0:a936477fcd4a 26 q_OB = Dimension_ftOb[5];
benson516 0:a936477fcd4a 27 n_ME_OB = Dimension_ftOb[6]; // Number of the measurement errors in the PI observer
benson516 0:a936477fcd4a 28 //
benson516 0:a936477fcd4a 29 zeros_n.assign(n, 0.0);
benson516 0:a936477fcd4a 30 zeros_p.assign(p, 0.0);
benson516 0:a936477fcd4a 31 zeros_q.assign(q, 0.0);
benson516 0:a936477fcd4a 32 zeros_n_ME_OB.assign(n_ME_OB, 0.0);
benson516 0:a936477fcd4a 33 //
benson516 0:a936477fcd4a 34 ones_p.assign(p, 1.0);
benson516 0:a936477fcd4a 35
benson516 0:a936477fcd4a 36 // Assign parameters
benson516 0:a936477fcd4a 37 // init();
benson516 0:a936477fcd4a 38
benson516 0:a936477fcd4a 39 // Indexes
benson516 0:a936477fcd4a 40 // idx_x_real = 0; // Index for x_real_est
benson516 0:a936477fcd4a 41 idx_x_ns = n;
benson516 0:a936477fcd4a 42 idx_ft_error_est = 2*n; // Index for ft_error_est
benson516 0:a936477fcd4a 43
benson516 0:a936477fcd4a 44 // Input signals
benson516 0:a936477fcd4a 45 yc = zeros_p;
benson516 0:a936477fcd4a 46 // States
benson516 0:a936477fcd4a 47 ft_est = zeros_p;
benson516 0:a936477fcd4a 48 ft_error_est = zeros_p;
benson516 0:a936477fcd4a 49 ft_ideal_est = zeros_p;
benson516 0:a936477fcd4a 50 //
benson516 0:a936477fcd4a 51 x_real_est = zeros_n; // Estimation of the real states
benson516 0:a936477fcd4a 52 // x_ns_est = zeros_n; // Estimation of the states of the no-slip system
benson516 0:a936477fcd4a 53
benson516 0:a936477fcd4a 54 // The estimation of measurement errors in PI observer
benson516 0:a936477fcd4a 55 Measurement_error_est = zeros_n_ME_OB;
benson516 0:a936477fcd4a 56
benson516 0:a936477fcd4a 57 // The degree of no-slip
benson516 0:a936477fcd4a 58 // ratio_ft = zeros_p;
benson516 0:a936477fcd4a 59 ratio_ft = ones_p; // No slip
benson516 0:a936477fcd4a 60 ratio_ft_filtered = ones_p; // No slip
benson516 0:a936477fcd4a 61
benson516 0:a936477fcd4a 62 }
benson516 0:a936477fcd4a 63 void Observer_ftRatio::start(){
benson516 0:a936477fcd4a 64 enable = true;
benson516 0:a936477fcd4a 65 //
benson516 0:a936477fcd4a 66 OB.start();
benson516 0:a936477fcd4a 67 }
benson516 0:a936477fcd4a 68 void Observer_ftRatio::pause(){
benson516 0:a936477fcd4a 69 enable = false;
benson516 0:a936477fcd4a 70 //
benson516 0:a936477fcd4a 71 OB.enable = false;
benson516 0:a936477fcd4a 72 }
benson516 0:a936477fcd4a 73 void Observer_ftRatio::stop(){
benson516 0:a936477fcd4a 74 if (!enable){
benson516 0:a936477fcd4a 75 return;
benson516 0:a936477fcd4a 76 }
benson516 0:a936477fcd4a 77 enable = false;
benson516 0:a936477fcd4a 78 // Reset
benson516 0:a936477fcd4a 79 reset();
benson516 0:a936477fcd4a 80
benson516 0:a936477fcd4a 81 //
benson516 0:a936477fcd4a 82 OB.stop();
benson516 0:a936477fcd4a 83 }
benson516 0:a936477fcd4a 84 void Observer_ftRatio::reset(){
benson516 0:a936477fcd4a 85 //
benson516 0:a936477fcd4a 86 // Input signals
benson516 0:a936477fcd4a 87 yc = zeros_p;
benson516 0:a936477fcd4a 88 // States
benson516 0:a936477fcd4a 89 ft_est = zeros_p;
benson516 0:a936477fcd4a 90 ft_error_est = zeros_p;
benson516 0:a936477fcd4a 91 ft_ideal_est = zeros_p;
benson516 0:a936477fcd4a 92 //
benson516 0:a936477fcd4a 93 x_real_est = zeros_n; // Estimation of the real states
benson516 0:a936477fcd4a 94 // x_ns_est = zeros_n; // Estimation of the states of the no-slip system
benson516 0:a936477fcd4a 95
benson516 0:a936477fcd4a 96 // The estimation of measurement errors in PI observer
benson516 0:a936477fcd4a 97 Measurement_error_est.assign(2, 0.0);
benson516 0:a936477fcd4a 98
benson516 0:a936477fcd4a 99 // Reset the identifier for ratio_ft(s)
benson516 0:a936477fcd4a 100 KFID_ratio_ft_right.reset(1.0); // 1.0 for no-slip (full friction-force)
benson516 0:a936477fcd4a 101 KFID_ratio_ft_left.reset(1.0); // 1.0 for no-slip (full friction-force)
benson516 0:a936477fcd4a 102
benson516 0:a936477fcd4a 103 // The degree of no-slip
benson516 0:a936477fcd4a 104 // ratio_ft = zeros_p;
benson516 0:a936477fcd4a 105 ratio_ft = ones_p; // No slip
benson516 0:a936477fcd4a 106 ratio_ft_filtered = ones_p;// No slip
benson516 0:a936477fcd4a 107
benson516 0:a936477fcd4a 108 // Reset the low-pass filter for ratio_ft
benson516 0:a936477fcd4a 109 LPF_ratio_ft.reset(ratio_ft);
benson516 0:a936477fcd4a 110
benson516 0:a936477fcd4a 111 //
benson516 0:a936477fcd4a 112 OB.reset();
benson516 0:a936477fcd4a 113 }
benson516 0:a936477fcd4a 114 //
benson516 0:a936477fcd4a 115 void Observer_ftRatio::init(void){
benson516 0:a936477fcd4a 116 //
benson516 0:a936477fcd4a 117 float _A_ftOb[] = {
benson516 0:a936477fcd4a 118 #include "A_ftOb.txt" // Load the parameter A_ftOb
benson516 0:a936477fcd4a 119 };
benson516 0:a936477fcd4a 120 float _By_ftOb[] = {
benson516 0:a936477fcd4a 121 #include "By_ftOb.txt" // Load the parameter By_ftOb
benson516 0:a936477fcd4a 122 };
benson516 0:a936477fcd4a 123 /*
benson516 0:a936477fcd4a 124 float _Bv1_ftOb[] = {
benson516 0:a936477fcd4a 125 #include "Bv1_ftOb.txt" // Load the parameter Bv1_ftOb
benson516 0:a936477fcd4a 126 };
benson516 0:a936477fcd4a 127 */
benson516 0:a936477fcd4a 128 float _C_ftOb[] = {
benson516 0:a936477fcd4a 129 #include "C_ftOb.txt" // Load the parameter C_ftOb
benson516 0:a936477fcd4a 130 };
benson516 0:a936477fcd4a 131 //
benson516 0:a936477fcd4a 132 float _F1_ftOb[] = {
benson516 0:a936477fcd4a 133 #include "F1_ftOb.txt" // Load the parameter F1_ftOb
benson516 0:a936477fcd4a 134 };
benson516 0:a936477fcd4a 135 float _F2_ftOb[] = {
benson516 0:a936477fcd4a 136 #include "F2_ftOb.txt" // Load the parameter F2_ftOb
benson516 0:a936477fcd4a 137 };
benson516 0:a936477fcd4a 138 //
benson516 0:a936477fcd4a 139 float _L_ftOb[] = {
benson516 0:a936477fcd4a 140 #include "L_ftOb.txt" // Load the parameter L_ftOb
benson516 0:a936477fcd4a 141 };
benson516 0:a936477fcd4a 142 //
benson516 0:a936477fcd4a 143 OB.assign_At(_A_ftOb, n_OB);
benson516 0:a936477fcd4a 144 OB.assign_Bt(_By_ftOb, n_OB, p_OB);
benson516 0:a936477fcd4a 145 OB.assign_Cd(_C_ftOb, q_OB, n_OB);
benson516 0:a936477fcd4a 146 //
benson516 0:a936477fcd4a 147 OB.assign_Lt(_L_ftOb, n_OB, 0, q_OB);
benson516 0:a936477fcd4a 148
benson516 0:a936477fcd4a 149 //-------------------------//
benson516 0:a936477fcd4a 150 // assign_Matrix(Bv1_ftOb, _Bv1_ftOb, n_OB, p_OB);
benson516 0:a936477fcd4a 151 //
benson516 0:a936477fcd4a 152 assign_Matrix(F1_ftOb, _F1_ftOb, p, n);
benson516 0:a936477fcd4a 153 assign_Matrix(F2_ftOb, _F2_ftOb, p, p);
benson516 0:a936477fcd4a 154
benson516 0:a936477fcd4a 155 }
benson516 0:a936477fcd4a 156
benson516 0:a936477fcd4a 157 //
benson516 0:a936477fcd4a 158 void Observer_ftRatio::iterateOnce(const vector<float> &yc_in, const vector<float> &x_real_in){
benson516 0:a936477fcd4a 159 if(!enable){
benson516 0:a936477fcd4a 160 return;
benson516 0:a936477fcd4a 161 }
benson516 0:a936477fcd4a 162 // Insert values
benson516 0:a936477fcd4a 163 insertSignals(yc_in, x_real_in);
benson516 0:a936477fcd4a 164
benson516 0:a936477fcd4a 165 // Progress one time slot
benson516 0:a936477fcd4a 166 OB.iterateOnce();
benson516 0:a936477fcd4a 167
benson516 0:a936477fcd4a 168
benson516 0:a936477fcd4a 169 // Clean up the estimation of w_ideal_avg and w_ideal_delta
benson516 0:a936477fcd4a 170
benson516 0:a936477fcd4a 171
benson516 0:a936477fcd4a 172 /*
benson516 0:a936477fcd4a 173 // for test
benson516 0:a936477fcd4a 174 return;
benson516 0:a936477fcd4a 175 //
benson516 0:a936477fcd4a 176 */
benson516 0:a936477fcd4a 177
benson516 0:a936477fcd4a 178
benson516 0:a936477fcd4a 179 // Get the results
benson516 0:a936477fcd4a 180 //--------------------------//
benson516 0:a936477fcd4a 181 size_t idx = 0;
benson516 0:a936477fcd4a 182 // x_real_est
benson516 0:a936477fcd4a 183 for (size_t i = 0; i < n; ++i){
benson516 0:a936477fcd4a 184 x_real_est[i] = OB.state_est[idx];
benson516 0:a936477fcd4a 185 //
benson516 0:a936477fcd4a 186 idx++;
benson516 0:a936477fcd4a 187 }
benson516 0:a936477fcd4a 188
benson516 0:a936477fcd4a 189 // ft_est
benson516 0:a936477fcd4a 190 for (size_t i = 0; i < p; ++i){
benson516 0:a936477fcd4a 191 ft_est[i] = OB.state_est[idx];
benson516 0:a936477fcd4a 192 //
benson516 0:a936477fcd4a 193 idx++;
benson516 0:a936477fcd4a 194 }
benson516 0:a936477fcd4a 195
benson516 0:a936477fcd4a 196 // Measurement_error_est
benson516 0:a936477fcd4a 197 for (size_t i = 0; i < n_ME_OB; ++i){
benson516 0:a936477fcd4a 198 Measurement_error_est[i] = OB.state_est[idx];
benson516 0:a936477fcd4a 199 //
benson516 0:a936477fcd4a 200 idx++;
benson516 0:a936477fcd4a 201 }
benson516 0:a936477fcd4a 202 //--------------------------//
benson516 0:a936477fcd4a 203
benson516 0:a936477fcd4a 204
benson516 0:a936477fcd4a 205
benson516 0:a936477fcd4a 206 // ft_ideal_est
benson516 0:a936477fcd4a 207 // ft_ideal_est = F1_ftOb*x_real_est + F2_ftOb*yc
benson516 0:a936477fcd4a 208 ft_ideal_est = Mat_multiply_Vec(F1_ftOb, x_real_est);
benson516 0:a936477fcd4a 209 Get_VectorIncrement(ft_ideal_est, Mat_multiply_Vec(F2_ftOb, yc), false); // +=
benson516 0:a936477fcd4a 210
benson516 0:a936477fcd4a 211 // ft_error_est = ft_est - ft_ideal_est;
benson516 0:a936477fcd4a 212 ft_error_est = Get_VectorPlus(ft_est, ft_ideal_est, true); // minus
benson516 0:a936477fcd4a 213
benson516 0:a936477fcd4a 214 /*
benson516 0:a936477fcd4a 215 // The ratio_ft
benson516 0:a936477fcd4a 216 for (size_t i = 0; i < p; ++i){
benson516 0:a936477fcd4a 217 //
benson516 0:a936477fcd4a 218 if (ft_ideal_est[i] < NUM_THRESHOLD && (-ft_ideal_est[i]) < NUM_THRESHOLD){
benson516 0:a936477fcd4a 219 ratio_ft[i] = 1.0; // no-slip
benson516 0:a936477fcd4a 220 }else{
benson516 0:a936477fcd4a 221 ratio_ft[i] = ft_est[i]/ft_ideal_est[i];
benson516 0:a936477fcd4a 222 }
benson516 0:a936477fcd4a 223 // Saturation
benson516 0:a936477fcd4a 224 if (ratio_ft[i] > 1.0){
benson516 0:a936477fcd4a 225 ratio_ft[i] = 1.0;
benson516 0:a936477fcd4a 226 }else if (ratio_ft[i] < 0.0){
benson516 0:a936477fcd4a 227 ratio_ft[i] = 0.0;
benson516 0:a936477fcd4a 228 }
benson516 0:a936477fcd4a 229 //
benson516 0:a936477fcd4a 230 }
benson516 0:a936477fcd4a 231 */
benson516 0:a936477fcd4a 232
benson516 0:a936477fcd4a 233 // Identify the ratio_ft
benson516 0:a936477fcd4a 234 //----------------------------------------------------//
benson516 0:a936477fcd4a 235 /*
benson516 0:a936477fcd4a 236 // Transform from (rifht, left) to (average, delta)
benson516 0:a936477fcd4a 237 T_ft_est.assign_R_L(ft_est);
benson516 0:a936477fcd4a 238 T_ft_ideal_est.assign_R_L(ft_ideal_est);
benson516 0:a936477fcd4a 239
benson516 0:a936477fcd4a 240 // ratio_ft_avg
benson516 0:a936477fcd4a 241 KFID_ratio_ft_avg.C = T_ft_ideal_est.get_avg();
benson516 0:a936477fcd4a 242 ratio_ft[0] = KFID_ratio_ft_avg.filter(0.0, T_ft_est.get_avg());
benson516 0:a936477fcd4a 243 // ratio_ft_delta
benson516 0:a936477fcd4a 244 KFID_ratio_ft_delta.C = T_ft_ideal_est.get_delta();
benson516 0:a936477fcd4a 245 ratio_ft[1] = KFID_ratio_ft_delta.filter(0.0, T_ft_est.get_delta());
benson516 0:a936477fcd4a 246 */
benson516 0:a936477fcd4a 247
benson516 0:a936477fcd4a 248 // Right-side ratio
benson516 0:a936477fcd4a 249 KFID_ratio_ft_right.C = ft_ideal_est[0];
benson516 0:a936477fcd4a 250 ratio_ft[0] = KFID_ratio_ft_right.filter(0.0, ft_est[0] );
benson516 0:a936477fcd4a 251 // Left-side ratio
benson516 0:a936477fcd4a 252 KFID_ratio_ft_left.C = ft_ideal_est[1];
benson516 0:a936477fcd4a 253 ratio_ft[1] = KFID_ratio_ft_left.filter(0.0, ft_est[1] );
benson516 0:a936477fcd4a 254
benson516 0:a936477fcd4a 255 // Saturation
benson516 0:a936477fcd4a 256 ratio_ft = SAT_ratio_ft.filter(ratio_ft);
benson516 0:a936477fcd4a 257 //----------------------------------------------------//
benson516 0:a936477fcd4a 258
benson516 0:a936477fcd4a 259 // Filtering
benson516 0:a936477fcd4a 260 ratio_ft_filtered = LPF_ratio_ft.filter(ratio_ft);
benson516 0:a936477fcd4a 261
benson516 0:a936477fcd4a 262 }
benson516 0:a936477fcd4a 263
benson516 0:a936477fcd4a 264 // Private methods
benson516 0:a936477fcd4a 265 //
benson516 0:a936477fcd4a 266 void Observer_ftRatio::insertSignals(const vector<float> &yc_in, const vector<float> &x_real_in){
benson516 0:a936477fcd4a 267 //
benson516 0:a936477fcd4a 268 yc = yc_in;
benson516 0:a936477fcd4a 269 // Insert values
benson516 0:a936477fcd4a 270 OB.sys_inputs = yc_in;
benson516 0:a936477fcd4a 271 OB.sys_outputs = x_real_in;
benson516 0:a936477fcd4a 272 //
benson516 0:a936477fcd4a 273 // OB.sys_extraDisturbance = Mat_multiply_Vec(Bv1_ftOb, v1_in);
benson516 0:a936477fcd4a 274 }
benson516 0:a936477fcd4a 275
benson516 0:a936477fcd4a 276
benson516 0:a936477fcd4a 277
benson516 0:a936477fcd4a 278 // Utilities
benson516 0:a936477fcd4a 279 void Observer_ftRatio::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:a936477fcd4a 280
benson516 0:a936477fcd4a 281 // Size check
benson516 0:a936477fcd4a 282 if (v_out.size() != m_left.size()){
benson516 0:a936477fcd4a 283 v_out.resize(m_left.size());
benson516 0:a936477fcd4a 284 }
benson516 0:a936477fcd4a 285
benson516 0:a936477fcd4a 286 /*
benson516 0:a936477fcd4a 287 // Iterators
benson516 0:a936477fcd4a 288 static vector<float>::iterator it_out;
benson516 0:a936477fcd4a 289 static vector<float>::iterator it_m_row;
benson516 0:a936477fcd4a 290 static vector<float>::iterator it_v;
benson516 0:a936477fcd4a 291
benson516 0:a936477fcd4a 292 //
benson516 0:a936477fcd4a 293 it_out = v_out.begin();
benson516 0:a936477fcd4a 294 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:a936477fcd4a 295 *it_out = 0.0;
benson516 0:a936477fcd4a 296 it_m_row = vector<vector<float> >(m_left)[i].begin();
benson516 0:a936477fcd4a 297 it_v = vector<float>(v_right).begin();
benson516 0:a936477fcd4a 298 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:a936477fcd4a 299 // *it_out += m_left[i][j] * v_right[j];
benson516 0:a936477fcd4a 300 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:a936477fcd4a 301 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:a936477fcd4a 302 }else{
benson516 0:a936477fcd4a 303 // (*it_out) += 0.0
benson516 0:a936477fcd4a 304 }
benson516 0:a936477fcd4a 305 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:a936477fcd4a 306 //
benson516 0:a936477fcd4a 307 it_m_row++;
benson516 0:a936477fcd4a 308 it_v++;
benson516 0:a936477fcd4a 309 }
benson516 0:a936477fcd4a 310 it_out++;
benson516 0:a936477fcd4a 311 }
benson516 0:a936477fcd4a 312 */
benson516 0:a936477fcd4a 313
benson516 0:a936477fcd4a 314 // Indexing
benson516 0:a936477fcd4a 315 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:a936477fcd4a 316 //
benson516 0:a936477fcd4a 317 v_out[i] = 0.0;
benson516 0:a936477fcd4a 318 //
benson516 0:a936477fcd4a 319 for (size_t j = 0; j < v_right.size(); ++j){
benson516 0:a936477fcd4a 320 if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
benson516 0:a936477fcd4a 321 v_out[i] += m_left[i][j]*v_right[j];
benson516 0:a936477fcd4a 322 }
benson516 0:a936477fcd4a 323 }
benson516 0:a936477fcd4a 324
benson516 0:a936477fcd4a 325 }
benson516 0:a936477fcd4a 326 vector<float> Observer_ftRatio::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:a936477fcd4a 327 static vector<float> v_out;
benson516 0:a936477fcd4a 328 // Size check
benson516 0:a936477fcd4a 329 if (v_out.size() != m_left.size()){
benson516 0:a936477fcd4a 330 v_out.resize(m_left.size());
benson516 0:a936477fcd4a 331 }
benson516 0:a936477fcd4a 332
benson516 0:a936477fcd4a 333 /*
benson516 0:a936477fcd4a 334 // Iterators
benson516 0:a936477fcd4a 335 static vector<float>::iterator it_out;
benson516 0:a936477fcd4a 336 static vector<float>::iterator it_m_row;
benson516 0:a936477fcd4a 337 static vector<float>::iterator it_v;
benson516 0:a936477fcd4a 338 //
benson516 0:a936477fcd4a 339 it_out = v_out.begin();
benson516 0:a936477fcd4a 340 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:a936477fcd4a 341 *it_out = 0.0;
benson516 0:a936477fcd4a 342 it_m_row = vector<vector<float> >(m_left)[i].begin();
benson516 0:a936477fcd4a 343 it_v = vector<float>(v_right).begin();
benson516 0:a936477fcd4a 344 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:a936477fcd4a 345 // *it_out += m_left[i][j] * v_right[j];
benson516 0:a936477fcd4a 346 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:a936477fcd4a 347 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:a936477fcd4a 348 }else{
benson516 0:a936477fcd4a 349 // (*it_out) += 0.0
benson516 0:a936477fcd4a 350 }
benson516 0:a936477fcd4a 351 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:a936477fcd4a 352 //
benson516 0:a936477fcd4a 353 it_m_row++;
benson516 0:a936477fcd4a 354 it_v++;
benson516 0:a936477fcd4a 355 }
benson516 0:a936477fcd4a 356 it_out++;
benson516 0:a936477fcd4a 357 }
benson516 0:a936477fcd4a 358 */
benson516 0:a936477fcd4a 359
benson516 0:a936477fcd4a 360 // Indexing
benson516 0:a936477fcd4a 361 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:a936477fcd4a 362 //
benson516 0:a936477fcd4a 363 v_out[i] = 0.0;
benson516 0:a936477fcd4a 364 //
benson516 0:a936477fcd4a 365 for (size_t j = 0; j < v_right.size(); ++j){
benson516 0:a936477fcd4a 366 if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
benson516 0:a936477fcd4a 367 v_out[i] += m_left[i][j]*v_right[j];
benson516 0:a936477fcd4a 368 }
benson516 0:a936477fcd4a 369 }
benson516 0:a936477fcd4a 370
benson516 0:a936477fcd4a 371 return v_out;
benson516 0:a936477fcd4a 372 }
benson516 0:a936477fcd4a 373 vector<float> Observer_ftRatio::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
benson516 0:a936477fcd4a 374 {
benson516 0:a936477fcd4a 375 static vector<float> v_c;
benson516 0:a936477fcd4a 376 // Size check
benson516 0:a936477fcd4a 377 if (v_c.size() != v_a.size()){
benson516 0:a936477fcd4a 378 v_c.resize(v_a.size());
benson516 0:a936477fcd4a 379 }
benson516 0:a936477fcd4a 380 //
benson516 0:a936477fcd4a 381 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:a936477fcd4a 382 if (is_minus){
benson516 0:a936477fcd4a 383 v_c[i] = v_a[i] - v_b[i];
benson516 0:a936477fcd4a 384 }else{
benson516 0:a936477fcd4a 385 v_c[i] = v_a[i] + v_b[i];
benson516 0:a936477fcd4a 386 }
benson516 0:a936477fcd4a 387 }
benson516 0:a936477fcd4a 388 return v_c;
benson516 0:a936477fcd4a 389 }
benson516 0:a936477fcd4a 390 vector<float> Observer_ftRatio::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 0:a936477fcd4a 391 {
benson516 0:a936477fcd4a 392 static vector<float> v_c;
benson516 0:a936477fcd4a 393 // Size check
benson516 0:a936477fcd4a 394 if (v_c.size() != v_a.size()){
benson516 0:a936477fcd4a 395 v_c.resize(v_a.size());
benson516 0:a936477fcd4a 396 }
benson516 0:a936477fcd4a 397 // for pure negative
benson516 0:a936477fcd4a 398 if (scale == -1.0){
benson516 0:a936477fcd4a 399 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:a936477fcd4a 400 v_c[i] = -v_a[i];
benson516 0:a936477fcd4a 401 }
benson516 0:a936477fcd4a 402 return v_c;
benson516 0:a936477fcd4a 403 }
benson516 0:a936477fcd4a 404 // else
benson516 0:a936477fcd4a 405 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:a936477fcd4a 406 v_c[i] = scale*v_a[i];
benson516 0:a936477fcd4a 407
benson516 0:a936477fcd4a 408 }
benson516 0:a936477fcd4a 409 return v_c;
benson516 0:a936477fcd4a 410 }
benson516 0:a936477fcd4a 411 // Increment
benson516 0:a936477fcd4a 412 void Observer_ftRatio::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
benson516 0:a936477fcd4a 413 // Size check
benson516 0:a936477fcd4a 414 if (v_a.size() != v_b.size()){
benson516 0:a936477fcd4a 415 v_a.resize(v_b.size());
benson516 0:a936477fcd4a 416 }
benson516 0:a936477fcd4a 417 //
benson516 0:a936477fcd4a 418 if (is_minus){ // -=
benson516 0:a936477fcd4a 419 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:a936477fcd4a 420 v_a[i] -= v_b[i];
benson516 0:a936477fcd4a 421 }
benson516 0:a936477fcd4a 422 }else{ // +=
benson516 0:a936477fcd4a 423 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:a936477fcd4a 424 v_a[i] += v_b[i];
benson516 0:a936477fcd4a 425 }
benson516 0:a936477fcd4a 426 }
benson516 0:a936477fcd4a 427
benson516 0:a936477fcd4a 428 }
benson516 0:a936477fcd4a 429
benson516 0:a936477fcd4a 430 //
benson516 0:a936477fcd4a 431 void Observer_ftRatio::assign_Matrix(vector<vector<float> > &M, float* M_in, size_t m_in, size_t n_in){ // M_in is a m_in by n_in array
benson516 0:a936477fcd4a 432 // M_in is the pointer of a mutidimentional array with size m_in by n_in
benson516 0:a936477fcd4a 433 M.resize(m_in, vector<float>(n_in, 0.0));
benson516 0:a936477fcd4a 434 //
benson516 0:a936477fcd4a 435 for (size_t i = 0; i < m_in; ++i){
benson516 0:a936477fcd4a 436 for (size_t j = 0; j < n_in; ++j){
benson516 0:a936477fcd4a 437 // M[i][j] = M_in[i][j];
benson516 0:a936477fcd4a 438 M[i][j] = *M_in;
benson516 0:a936477fcd4a 439 M_in++;
benson516 0:a936477fcd4a 440 }
benson516 0:a936477fcd4a 441 }
benson516 0:a936477fcd4a 442 }