34 uint32_t i, n = m_readings.size();
37 for (i = 0; i < n; i++)
39 out <<
CPose3D(m_readings[i].eNosePoseOnTheRobot);
40 out << m_readings[i].readingsVoltage;
41 out << m_readings[i].sensorTypes;
42 out << m_readings[i].hasTemperature;
43 if (m_readings[i].hasTemperature)
out << m_readings[i].temperature;
46 out << sensorLabel << timestamp;
67 for (i = 0; i < n; i++)
70 m_readings[i].eNosePoseOnTheRobot = aux.
asTPose();
71 in >> m_readings[i].readingsVoltage;
72 in >> m_readings[i].sensorTypes;
75 in >> m_readings[i].hasTemperature;
76 if (m_readings[i].hasTemperature)
77 in >> m_readings[i].temperature;
81 m_readings[i].hasTemperature =
false;
82 m_readings[i].temperature = 0;
108 ASSERT_(readings.size() == 16);
113 TPose3D(0.20, -0.15, 0.10, 0, 0, 0);
122 m_readings.push_back(eNose);
126 TPose3D(0.20, 0.15, 0.10, .0, .0, .0);
135 m_readings.push_back(eNose);
148 if (!m_readings.empty())
149 out_sensorPose =
CPose3D(m_readings[0].eNosePoseOnTheRobot);
151 out_sensorPose =
CPose3D(0, 0, 0);
156 for (
auto& r : m_readings) r.eNosePoseOnTheRobot = newSensorPose.
asTPose();
165 noise_filtering(reading, timestamp);
168 if (decimate_count != decimate_value)
177 m_antiNoise_window[winNoise_size / 2].reading_filtered,
178 m_antiNoise_window[winNoise_size / 2].timestamp);
182 reading = last_Obs.estimation;
183 timestamp = last_Obs.timestamp;
188 last_Obs.timestamp, last_Obs.reading, last_Obs.estimation,
195 cout <<
"Error when decimating \n";
210 temporal_Obs.reading = reading;
211 temporal_Obs.timestamp = timestamp;
214 if (m_antiNoise_window.empty())
217 temporal_Obs.reading_filtered = reading;
220 m_antiNoise_window.assign(winNoise_size, temporal_Obs);
225 m_antiNoise_window.erase(m_antiNoise_window.begin());
226 m_antiNoise_window.push_back(temporal_Obs);
230 float partial_sum = 0;
231 for (
auto& i : m_antiNoise_window) partial_sum += i.reading;
233 m_antiNoise_window.at(winNoise_size / 2).reading_filtered =
234 partial_sum / winNoise_size;
238 cout <<
"Error when filtering noise from readings \n";
253 if (reading < min_reading) min_reading = reading;
256 if (!first_iteration)
263 if ((incT > 0) & (!first_incT))
269 if (fabs(incT - fixed_incT) > (
double)(0.05))
270 cout <<
"IncT is not constant by HW." << endl;
274 if (incT > 0) first_incT =
false;
278 if (reading < last_Obs.reading)
281 a_decay * std::abs(reading - min_reading) + b_decay;
286 a_rise * std::abs(reading - min_reading) + b_rise;
293 last_Obs.estimation =
294 d2f(((reading - last_Obs.reading) * last_Obs.tau / incT) +
297 last_Obs.estimation = reading;
300 last_Obs.timestamp = timestamp;
301 last_Obs.reading = reading;
306 last_Obs.tau = b_rise;
307 last_Obs.reading = reading;
308 last_Obs.timestamp = timestamp;
309 last_Obs.estimation =
311 first_iteration =
false;
314 catch (
const exception& e)
316 cerr <<
"**Exception in "
317 "CObservationGasSensors::CMOSmodel::inverse_MOSmodeling** "
334 sprintf(buffer,
"./log_MOSmodel_GasDistribution.txt");
336 if (!m_debug_dump) m_debug_dump =
new ofstream(buffer);
338 if (m_debug_dump->is_open())
340 *m_debug_dump <<
format(
"%f \t", time);
341 *m_debug_dump <<
format(
"%f \t", reading);
342 *m_debug_dump <<
format(
"%f \t", estimation);
343 *m_debug_dump <<
format(
"%f \t", tau);
344 *m_debug_dump <<
"\n";
347 cout <<
"Unable to open file";
355 for (
size_t j = 0; j < m_readings.size(); j++)
357 o <<
format(
"e-nose #%u:\n", (
unsigned)j);
359 vector<float>::const_iterator it;
360 std::vector<int>::const_iterator itKind;
363 m_readings[j].readingsVoltage.size() ==
364 m_readings[j].sensorTypes.size());
366 for (it = m_readings[j].readingsVoltage.begin(),
367 itKind = m_readings[j].sensorTypes.begin();
368 it != m_readings[j].readingsVoltage.end(); it++, itKind++)
369 o <<
format(
"%04X: %.03f ", *itKind, *it);
374 " Sensor pose on robot: (x,y,z)=(%.02f,%.02f,%.02f)\n",
375 m_readings[j].eNosePoseOnTheRobot.x,
376 m_readings[j].eNosePoseOnTheRobot.y,
377 m_readings[j].eNosePoseOnTheRobot.z);
379 o <<
"Measured temperature: ";
380 if (m_readings[j].hasTemperature)
381 o <<
format(
"%.03f degC\n", m_readings[j].temperature);
383 o <<
"NOT AVAILABLE\n";