ImuIntegrator.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
/// @file ImuIntegrator.hpp
/// @brief Integrates ImuObs Data
/// @author T. Topp (topp@ins.uni-stuttgart.de)
/// @author M. Maier (marcel.maier@ins.uni-stuttgart.de)
/// @date 2020-05-18
 
#pragma once
 
#include "internal/Node/Node.hpp"
 
#include "Navigation/INS/InertialIntegrator.hpp"
#include "Navigation/INS/InertialPreIntegrator.hpp"
 
#include "NodeData/IMU/ImuObs.hpp"
#include "NodeData/IMU/ImuObsWDelta.hpp"
#include "NodeData/State/PosVelAtt.hpp"
 
namespace NAV
{
/// @brief Numerically integrates Imu data
class ImuIntegrator : public Node
{
  public:
    /// @brief Default constructor
    ImuIntegrator();
    /// @brief Destructor
    ~ImuIntegrator() override;
    /// @brief Copy constructor
    ImuIntegrator(const ImuIntegrator&) = delete;
    /// @brief Move constructor
    ImuIntegrator(ImuIntegrator&&) = delete;
    /// @brief Copy assignment operator
    ImuIntegrator& operator=(const ImuIntegrator&) = delete;
    /// @brief Move assignment operator
    ImuIntegrator& operator=(ImuIntegrator&&) = delete;
 
    /// @brief String representation of the Class Type
    [[nodiscard]] static std::string typeStatic();
 
    /// @brief String representation of the Class Type
    [[nodiscard]] std::string type() const override;
 
    /// @brief String representation of the Class Category
    [[nodiscard]] static std::string category();
 
    /// @brief ImGui config window which is shown on double click
    /// @attention Don't forget to set _hasConfig to true in the constructor of the node
    void guiConfig() override;
 
    /// @brief Saves the node into a json object
    [[nodiscard]] json save() const override;
 
    /// @brief Restores the node from a json object
    /// @param[in] j Json object with the node state
    void restore(const json& j) override;
 
  private:
    constexpr static size_t OUTPUT_PORT_INDEX_INERTIAL_NAV_SOL = 0; ///< @brief Flow (InertialNavSol)
    constexpr static size_t INPUT_PORT_INDEX_IMU_OBS = 0;           ///< @brief Flow (ImuObs)
    constexpr static size_t INPUT_PORT_INDEX_POS_VEL_ATT_INIT = 1;  ///< @brief Flow (PosVelAtt)
 
    /// @brief Initialize the node
    bool initialize() override;
 
    /// @brief Deinitialize the node
    void deinitialize() override;
 
    /// @brief Receive Function for the PosVelAtt initial values
    /// @param[in] queue Queue with all the received data messages
    /// @param[in] pinIdx Index of the pin the data is received on
    void recvPosVelAttInit(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    /// @brief Receive Function
    /// @param[in] queue Queue with all the received data messages
    /// @param[in] pinIdx Index of the pin the data is received on
    void recvObservation(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    /// Wether IMU preintegration should be used
    bool _imuPreintegration = false;
 
    /// Resetting the preintegrator every epoch makes it behave like a normal integrator
    bool _resetPreintegratorEveryEpoch = false;
 
    /// @brief Inertial Integrator
    InertialIntegrator _inertialIntegrator;
 
    /// @brief Inertial Preintegrator
    InertialPreIntegrator _inertialPreintegrator;
 
    /// @brief Prefer the raw acceleration measurements over the deltaVel & deltaTheta values
    bool _preferAccelerationOverDeltaMeasurements = false;
 
    // ################################################################################################################
 
    /// Last IMU measuremnt
    std::shared_ptr<const ImuObs> _lastImuObs = nullptr;
 
    /// Last position, velocity and attitude
    std::shared_ptr<const PosVelAtt> _lastPosVelAtt = nullptr;
};
 
} // namespace NAV