CObservationGasSensors.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "obs-precomp.h"  // Precompiled headers

#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/obs/CObservationGasSensors.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>
#include <fstream>
#include <iostream>

using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CObservationGasSensors, CObservation, mrpt::obs)

/** Constructor
 */
CObservationGasSensors::CObservationGasSensors() : m_readings() {}
uint8_t CObservationGasSensors::serializeGetVersion() const { return 5; }
void CObservationGasSensors::serializeTo(
    mrpt::serialization::CArchive& out) const
{
    uint32_t i, n = m_readings.size();
    out << n;

    for (i = 0; i < n; i++)
    {
        out << CPose3D(m_readings[i].eNosePoseOnTheRobot);
        out << m_readings[i].readingsVoltage;
        out << m_readings[i].sensorTypes;
        out << m_readings[i].hasTemperature;
        if (m_readings[i].hasTemperature) out << m_readings[i].temperature;
    }

    out << sensorLabel << timestamp;
}

void CObservationGasSensors::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 2:
        case 3:
        case 4:
        case 5:
        {
            // A general set of e-nose descriptors:
            uint32_t i, n;

            in >> n;
            m_readings.resize(n);

            CPose3D aux;

            for (i = 0; i < n; i++)
            {
                in >> aux;
                m_readings[i].eNosePoseOnTheRobot = aux.asTPose();
                in >> m_readings[i].readingsVoltage;
                in >> m_readings[i].sensorTypes;
                if (version >= 3)
                {
                    in >> m_readings[i].hasTemperature;
                    if (m_readings[i].hasTemperature)
                        in >> m_readings[i].temperature;
                }
                else
                {
                    m_readings[i].hasTemperature = false;
                    m_readings[i].temperature = 0;
                }
            }

            if (version >= 4)
                in >> sensorLabel;
            else
                sensorLabel = "";

            if (version >= 5)
                in >> timestamp;
            else
                timestamp = INVALID_TIMESTAMP;
        }
        break;
        case 0:
        case 1:
        {
            TObservationENose eNose;

            m_readings.clear();

            // There was a single set of 16 values from "Sancho" (DEC-2006)
            CVectorFloat readings;
            in >> readings;

            ASSERT_(readings.size() == 16);

            // There was TWO e-noses:
            // (1)
            eNose.eNosePoseOnTheRobot =
                TPose3D(0.20, -0.15, 0.10, 0, 0, 0);  // (x,y,z) only
            eNose.readingsVoltage.resize(4);
            eNose.readingsVoltage[0] = readings[2];
            eNose.readingsVoltage[1] = readings[4];
            eNose.readingsVoltage[2] = readings[5];
            eNose.readingsVoltage[3] = readings[6];

            eNose.sensorTypes.clear();
            eNose.sensorTypes.resize(4, 0);
            m_readings.push_back(eNose);

            // (2)
            eNose.eNosePoseOnTheRobot =
                TPose3D(0.20, 0.15, 0.10, .0, .0, .0);  // (x,y,z) only
            eNose.readingsVoltage.resize(4);
            eNose.readingsVoltage[0] = readings[8];
            eNose.readingsVoltage[1] = readings[10];
            eNose.readingsVoltage[2] = readings[12];
            eNose.readingsVoltage[3] = readings[14];

            eNose.sensorTypes.clear();
            eNose.sensorTypes.resize(4, 0);
            m_readings.push_back(eNose);
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
}

/*---------------------------------------------------------------
                     getSensorPose
 ---------------------------------------------------------------*/
void CObservationGasSensors::getSensorPose(CPose3D& out_sensorPose) const
{
    if (!m_readings.empty())
        out_sensorPose = CPose3D(m_readings[0].eNosePoseOnTheRobot);
    else
        out_sensorPose = CPose3D(0, 0, 0);
}

void CObservationGasSensors::setSensorPose(const CPose3D& newSensorPose)
{
    for (auto& r : m_readings) r.eNosePoseOnTheRobot = newSensorPose.asTPose();
}

bool CObservationGasSensors::CMOSmodel::get_GasDistribution_estimation(
    float& reading, mrpt::system::TTimeStamp& timestamp)
{
    try
    {
        // Noise filtering
        noise_filtering(reading, timestamp);

        // Decimate
        if (decimate_count != decimate_value)
        {
            decimate_count++;
            return false;
        }

        // Gas concentration estimation based on FIRST ORDER + NONLINEAR
        // COMPENSATIONS DYNAMICS
        inverse_MOSmodeling(
            m_antiNoise_window[winNoise_size / 2].reading_filtered,
            m_antiNoise_window[winNoise_size / 2].timestamp);
        decimate_count = 1;

        // update (output)
        reading = last_Obs.estimation;
        timestamp = last_Obs.timestamp;

        // Save data map in log file for Matlab visualization
        if (save_maplog)
            save_log_map(
                last_Obs.timestamp, last_Obs.reading, last_Obs.estimation,
                last_Obs.tau);

        return true;
    }
    catch (...)
    {
        cout << "Error when decimating \n";
        mrpt::system::pause();
        return false;
    }
}

/*---------------------------------------------------------------
                     noise_filtering (smooth)
 ---------------------------------------------------------------*/
void CObservationGasSensors::CMOSmodel::noise_filtering(
    float reading, const mrpt::system::TTimeStamp& timestamp)
{
    try
    {
        // Store values in the temporal Observation
        temporal_Obs.reading = reading;
        temporal_Obs.timestamp = timestamp;

        // If first reading from E-nose
        if (m_antiNoise_window.empty())
        {
            // use default values
            temporal_Obs.reading_filtered = reading;

            // Populate the noise window
            m_antiNoise_window.assign(winNoise_size, temporal_Obs);
        }
        else
        {
            // Erase the first element (the oldest), and add the new one
            m_antiNoise_window.erase(m_antiNoise_window.begin());
            m_antiNoise_window.push_back(temporal_Obs);
        }

        // Average data to reduce noise (Noise Filtering)
        float partial_sum = 0;
        for (auto& i : m_antiNoise_window) partial_sum += i.reading;

        m_antiNoise_window.at(winNoise_size / 2).reading_filtered =
            partial_sum / winNoise_size;
    }
    catch (...)
    {
        cout << "Error when filtering noise from readings \n";
        mrpt::system::pause();
    }
}

/*---------------------------------------------------------------
                inverse_MOSmodeling
 ---------------------------------------------------------------*/
void CObservationGasSensors::CMOSmodel::inverse_MOSmodeling(
    float reading, const mrpt::system::TTimeStamp& timestamp)
{
    try
    {
        // Keep the minimum reading value as an approximation to the basline
        // level
        if (reading < min_reading) min_reading = reading;

        // Check if estimation posible (not possible in the first iteration)
        if (!first_iteration)
        {
            // Assure the samples are provided at constant rate (important for
            // the correct gas distribution estimation)
            double incT =
                mrpt::system::timeDifference(last_Obs.timestamp, timestamp);

            if ((incT > 0) & (!first_incT))
            {  // not the same sample due to initialization of buffers
                if (fixed_incT == 0)
                    fixed_incT = incT;
                else
                    // ASSERT_(fabs(incT - fixed_incT) < (double)(0.05));
                    if (fabs(incT - fixed_incT) > (double)(0.05))
                    cout << "IncT is not constant by HW." << endl;
            }
            else
            {
                if (incT > 0) first_incT = false;
            }

            // slope<0 -->Decay
            if (reading < last_Obs.reading)
            {
                last_Obs.tau =
                    a_decay * std::abs(reading - min_reading) + b_decay;
            }
            else  // slope>=0 -->rise
            {
                last_Obs.tau =
                    a_rise * std::abs(reading - min_reading) + b_rise;
            }  // end-if

            // New estimation values -- Ziegler-Nichols model --
            if (incT > 0)
                // Initially there may come repetetive values till
                // m_antiNoise_window is full populated.
                last_Obs.estimation =
                    d2f(((reading - last_Obs.reading) * last_Obs.tau / incT) +
                        reading);
            else
                last_Obs.estimation = reading;

            // Prepare the New observation
            last_Obs.timestamp = timestamp;
            last_Obs.reading = reading;
        }
        else
        {
            // First filtered reading (use default values)
            last_Obs.tau = b_rise;
            last_Obs.reading = reading;
            last_Obs.timestamp = timestamp;
            last_Obs.estimation =
                reading;  // No estimation possible at this step
            first_iteration = false;
        }  // end-if estimation values
    }
    catch (const exception& e)
    {
        cerr << "**Exception in "
                "CObservationGasSensors::CMOSmodel::inverse_MOSmodeling** "
             << e.what() << endl;
    }
}

/*---------------------------------------------------------------
                        save_log_map
  ---------------------------------------------------------------*/
void CObservationGasSensors::CMOSmodel::save_log_map(
    const mrpt::system::TTimeStamp& timestamp, float reading, float estimation,
    float tau)
{
    // function to save in a log file the information of the generated gas
    // distribution estimation

    double time = mrpt::system::timestampTotime_t(timestamp);
    char buffer[50];
    sprintf(buffer, "./log_MOSmodel_GasDistribution.txt");

    if (!m_debug_dump) m_debug_dump = new ofstream(buffer);

    if (m_debug_dump->is_open())
    {
        *m_debug_dump << format("%f \t", time);
        *m_debug_dump << format("%f \t", reading);
        *m_debug_dump << format("%f \t", estimation);
        *m_debug_dump << format("%f \t", tau);
        *m_debug_dump << "\n";
    }
    else
        cout << "Unable to open file";
}

void CObservationGasSensors::getDescriptionAsText(std::ostream& o) const
{
    using namespace std;
    CObservation::getDescriptionAsText(o);

    for (size_t j = 0; j < m_readings.size(); j++)
    {
        o << format("e-nose #%u:\n", (unsigned)j);

        vector<float>::const_iterator it;
        std::vector<int>::const_iterator itKind;

        ASSERT_(
            m_readings[j].readingsVoltage.size() ==
            m_readings[j].sensorTypes.size());

        for (it = m_readings[j].readingsVoltage.begin(),
            itKind = m_readings[j].sensorTypes.begin();
             it != m_readings[j].readingsVoltage.end(); it++, itKind++)
            o << format("%04X: %.03f ", *itKind, *it);

        o << endl;

        o << format(
            "  Sensor pose on robot: (x,y,z)=(%.02f,%.02f,%.02f)\n",
            m_readings[j].eNosePoseOnTheRobot.x,
            m_readings[j].eNosePoseOnTheRobot.y,
            m_readings[j].eNosePoseOnTheRobot.z);

        o << "Measured temperature: ";
        if (m_readings[j].hasTemperature)
            o << format("%.03f degC\n", m_readings[j].temperature);
        else
            o << "NOT AVAILABLE\n";
    }
}