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"
 
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 : uint8_t
    {
        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