ImuFusion.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/.
 
 
#pragma once
 
#include "Nodes/DataProvider/IMU/Imu.hpp"
#include "NodeData/IMU/ImuObs.hpp"
 
#include "Navigation/Math/KalmanFilter.hpp"
 
#include "Navigation/INS/SensorCombiner/PinData.hpp"
 
#include "internal/gui/widgets/TimeEdit.hpp"
 
#include <deque>
#include <utility>
 
namespace NAV
{
class ImuFusion : public Imu
{
  public:
    ImuFusion();
    ~ImuFusion() override;
    ImuFusion(const ImuFusion&) = delete;
    ImuFusion(ImuFusion&&) = delete;
    ImuFusion& operator=(const ImuFusion&) = delete;
    ImuFusion& operator=(ImuFusion&&) = delete;
 
    [[nodiscard]] static std::string typeStatic();
 
    [[nodiscard]] std::string type() const override;
 
    [[nodiscard]] static std::string category();
 
    void guiConfig() override;
 
    [[nodiscard]] json save() const override;
 
    void restore(const json& j) override;
 
  protected:
    ImuPos _imuPos;
 
  private:
    constexpr static size_t OUTPUT_PORT_INDEX_COMBINED_SIGNAL = 0; 
    constexpr static size_t OUTPUT_PORT_INDEX_BIASES = 1;          
 
    bool initialize() override;
 
    void deinitialize() override;
 
    void updateNumberOfInputPins();
 
    void initializeMountingAngles();
 
    void recvSignal(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    [[nodiscard]] static Eigen::MatrixXd measurementNoiseMatrix_R_adaptive(double alpha,
                                                                           const Eigen::MatrixXd& R,
                                                                           const Eigen::VectorXd& e,
                                                                           const Eigen::MatrixXd& H,
                                                                           const Eigen::MatrixXd& P);
 
    InsTime _lastFiltObs{};
 
    void measurementNoiseMatrix_R(Eigen::MatrixXd& R, size_t pinIndex = 0) const;
 
    void combineSignals(const std::shared_ptr<const ImuObs>& imuObs);
 
    // --------------------------------------- Kalman filter config ------------------------------------------
    size_t _nInputPins = 2;
 
    const uint8_t _numStatesEstIRWKF = 12;
 
    const uint8_t _numStatesEstBsplineKF = 18;
 
    const uint8_t _numBsplines = 9;
 
    const uint8_t _numStatesPerPin = 6;
 
    const uint8_t _numMeasurements = 6;
 
    uint8_t _numStatesEst{};
 
    uint8_t _numStates = 12;
 
    KalmanFilter _kalmanFilter{ _numStates, _numMeasurements };
 
    // ---------------------------------------- Kalman filter init -------------------------------------------
    double _imuFrequency{ 100 };
 
    double _averageEndTime{ 1 };
 
    InsTime _avgEndTime;
 
    bool _autoInitKF = true;
 
    bool _imuCharacteristicsIdentical = true;
 
    bool _imuBiasesIdentical = true;
 
    std::vector<std::shared_ptr<const NAV::ImuObs>> _cumulatedImuObs;
 
    std::vector<size_t> _cumulatedPinIds;
 
    bool _initJerkAngAcc = true;
 
    bool _kfInitialized = false;
 
    // -------------------------------------------- Sensor info ----------------------------------------------
    std::vector<PinData> _pinData;
    PinDataIRWKF _pinDataIRWKF;
    PinDataBsplineKF _pinDataBsplineKF;
 
    Eigen::Vector3d _initCoeffsAngRateTemp{};
    Eigen::Vector3d _initCoeffsAccelTemp{};
    Eigen::Vector3d _initCovarianceCoeffsAngRateTemp{};
    Eigen::Vector3d _initCovarianceCoeffsAccelTemp{};
    Eigen::Vector3d _procNoiseCoeffsAngRateTemp{};
    Eigen::Vector3d _procNoiseCoeffsAccelTemp{};
 
    std::vector<Eigen::Vector3d> _biasCovariances;
    std::vector<Eigen::VectorXd> _processNoiseVariances;
    std::vector<Eigen::Vector3d> _measurementNoiseVariances;
 
    std::vector<Eigen::Matrix3d> _imuRotations_accel;
 
    std::vector<Eigen::Matrix3d> _imuRotations_gyro;
 
    // ----------------------------------------------- Time --------------------------------------------------
    InsTime _latestTimestamp{};
 
    InsTime _firstTimestamp{};
 
    double _timeSinceStartup{};
 
    double _latestKnot{};
 
    double _splineSpacing = 1.0;
 
    // ------------------------------------------- Miscellaneous ---------------------------------------------
    bool _checkKalmanMatricesRanks = false;
 
    bool _imuPosSet = false;
 
    enum class ImuFusionType
    {
        IRWKF,   
        Bspline, 
        COUNT,   
    };
    friend constexpr const char* to_string(ImuFusionType value);
 
    ImuFusionType _imuFusionType = ImuFusionType::Bspline;
};
 
constexpr const char* to_string(NAV::ImuFusion::ImuFusionType value)
{
    switch (value)
    {
    case NAV::ImuFusion::ImuFusionType::IRWKF:
        return "IRW KF";
    case NAV::ImuFusion::ImuFusionType::Bspline:
        return "B-spline KF";
    case NAV::ImuFusion::ImuFusionType::COUNT:
        return "";
    }
    return "";
}
 
} // namespace NAV