InertialIntegrator.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 <functional>
#include <optional>
#include <memory>
 
#include "NodeData/IMU/ImuPos.hpp"
#include "NodeData/State/PosVelAtt.hpp"
 
#include "Navigation/Gravity/Gravity.hpp"
#include "Navigation/Math/NumericalIntegration.hpp"
#include "Navigation/Time/InsTime.hpp"
#include "Navigation/INS/Mechanization.hpp"
#include "Navigation/INS/LocalNavFrame/Mechanization.hpp"
#include "Navigation/INS/EcefFrame/Mechanization.hpp"
 
#include "util/Container/ScrollingBuffer.hpp"
#include "util/Eigen.hpp"
#include "util/Json.hpp"
#include "util/Logger.hpp"
 
namespace NAV
{

class InertialIntegrator
{
  public:
    enum class IntegrationAlgorithm : uint8_t
    {
        SingleStepRungeKutta1, 
        SingleStepRungeKutta2, 
        SingleStepHeun2,       
        SingleStepRungeKutta3, 
        SingleStepHeun3,       
        SingleStepRungeKutta4, 
        MultiStepRK3,          
        MultiStepRK4,          
        COUNT,                 
    };

    enum class IntegrationFrame : uint8_t
    {
        ECEF, 
        NED,  
    };

    struct Measurement
    {
        bool averagedMeasurement = false; 
        double dt = 0.0;                  
        Eigen::Vector3d p_acceleration;   
        Eigen::Vector3d p_angularRate;    
    };

    template<typename T>
    struct GenericState
    {
        InsTime epoch; 
 
        Eigen::Vector3<T> position;    
        Eigen::Vector3<T> velocity;    
        Eigen::Quaternion<T> attitude; 
 
        Measurement m; 
 
        Eigen::Vector3<T> p_biasAcceleration = Eigen::Vector3<T>::Zero(); 
        Eigen::Vector3<T> p_biasAngularRate = Eigen::Vector3<T>::Zero();  
 
        Eigen::Vector3<T> scaleFactorAccel = Eigen::Vector3<T>::Ones(); 
        Eigen::Vector3<T> scaleFactorGyro = Eigen::Vector3<T>::Ones();  
 
        Eigen::Quaternion<T> misalignmentAccel = Eigen::Quaternion<T>::Identity(); 
        Eigen::Quaternion<T> misalignmentGyro = Eigen::Quaternion<T>::Identity();  
    };

    using State = GenericState<double>;

    InertialIntegrator() = default;

    explicit InertialIntegrator(IntegrationFrame integrationFrame);

    void reset();

    [[nodiscard]] bool hasInitialPosition() const;

    void setInitialState(const PosVelAtt& state, const char* nameId);

    void addState(const PosVelAtt& state, const char* nameId);

    void setTotalSensorBiases(const Eigen::Vector3d& p_biasAcceleration, const Eigen::Vector3d& p_biasAngularRate);

    void applySensorBiasesIncrements(const Eigen::Vector3d& p_deltaBiasAcceleration, const Eigen::Vector3d& p_deltaBiasAngularRate);

    void applyStateErrors_n(const Eigen::Vector3d& lla_positionError, const Eigen::Vector3d& n_velocityError, const Eigen::Vector3d& n_attitudeError_b);

    void applyStateErrors_e(const Eigen::Vector3d& e_positionError, const Eigen::Vector3d& e_velocityError, const Eigen::Vector3d& e_attitudeError_b);

    [[nodiscard]] std::optional<std::reference_wrapper<const State>> getLatestState() const;

    [[nodiscard]] const Eigen::Vector3d& p_getLastAccelerationBias() const;

    [[nodiscard]] const Eigen::Vector3d& p_getLastAngularRateBias() const;

    [[nodiscard]] IntegrationFrame getIntegrationFrame() const;

    [[nodiscard]] const PosVelAttDerivativeConstants& getConstants() const;

    [[nodiscard]] bool areAccelerationsAveragedMeasurements() const;

    [[nodiscard]] std::optional<Eigen::Vector3d> p_calcCurrentAcceleration() const;

    [[nodiscard]] std::optional<Eigen::Vector3d> p_calcCurrentAngularRate() const;

    std::shared_ptr<PosVelAtt> calcInertialSolution(const InsTime& obsTime,
                                                    const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate,
                                                    const ImuPos& imuPos, const char* nameId);

    std::shared_ptr<PosVelAtt> calcInertialSolutionDelta(const InsTime& obsTime, double deltaTime,
                                                         const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta,
                                                         const ImuPos& imuPos, const char* nameId);
 
    // Forward declaring external function
    friend const char* to_string(InertialIntegrator::IntegrationAlgorithm algorithm);
    friend const char* to_string(InertialIntegrator::IntegrationFrame frame);

    template<typename T>
    [[nodiscard]] Eigen::Vector<T, 10> calcInertialSolution(const ImuPos& imuPos,
                                                            const GenericState<T>& state__t0,
                                                            const GenericState<T>& state__t1,
                                                            [[maybe_unused]] const char* nameId) const
    {
        // #if LOG_LEVEL <= LOG_LEVEL_DATA
        //         auto printState = [](const GenericState<T>& state, const char* t, const char* nameId) {
        //             LOG_DATA("{} [{}]:\n"
        //                      " - {}:\n"
        //                      "    Position [{}, {}, {}], Velocity [{}, {}, {}], Attitude [{}x, {}y, {}z, {}w]\n"
        //                      "    dt = {:.5f}, p_accel [{}, {}, {}], p_angRate [{}, {}, {}]\n"
        //                      "    p_biasAccel [{}, {}, {}], p_biasAngRate [{}, {}, {}]\n"
        //                      "    p_scaleFacAccel [{}, {}, {}], p_scaleFacAngRate [{}, {}, {}]\n"
        //                      "    p_misAlignAccel [{}x, {}y, {}z, {}w], p_misAlignAngRate [{}x, {}y, {}z, {}w]",
        //                      nameId, t, state.epoch.toYMDHMS(GPST),
        //                      state.position(0), state.position(1), state.position(2),
        //                      state.velocity(0), state.velocity(1), state.velocity(2),
        //                      state.attitude.x(), state.attitude.y(), state.attitude.z(), state.attitude.w(),
        //                      state.m.dt, state.m.p_acceleration(0), state.m.p_acceleration(1), state.m.p_acceleration(2),
        //                      state.m.p_angularRate(0), state.m.p_angularRate(1), state.m.p_angularRate(2),
        //                      state.p_biasAcceleration(0), state.p_biasAcceleration(1), state.p_biasAcceleration(2),
        //                      state.p_biasAngularRate(0), state.p_biasAngularRate(1), state.p_biasAngularRate(2),
        //                      state.scaleFactorAccel(0), state.scaleFactorAccel(1), state.scaleFactorAccel(2),
        //                      state.scaleFactorGyro(0), state.scaleFactorGyro(1), state.scaleFactorGyro(2),
        //                      state.misalignmentAccel.x(), state.misalignmentAccel.y(), state.misalignmentAccel.z(), state.misalignmentAccel.w(),
        //                      state.misalignmentGyro.x(), state.misalignmentGyro.y(), state.misalignmentGyro.z(), state.misalignmentGyro.w());
        //         };
        //         LOG_DATA("{}: Frame {}, Algorithm {}", nameId, to_string(_integrationFrame), to_string(_integrationAlgorithm));
        //         printState(state__t0, "t0", nameId);
        //         printState(state__t1, "t1", nameId);
        // #endif
 
        Eigen::Vector<T, 10> y;
        //        0  1  2   3    4    5     6       7       8       9
        // NED  [ 𝜙, λ, h, v_N, v_E, v_D, n_q_bx, n_q_by, n_q_bz, n_q_bw]^T
        // ECEF [ x, y, z, v_x, v_y, v_z, e_q_bx, e_q_by, e_q_bz, e_q_bw]^T
        y.template segment<3>(0) = state__t1.position;
        y.template segment<3>(3) = state__t1.velocity;
        y.template segment<4>(6) = state__t1.attitude.coeffs();
 
        auto p_accel = [](const GenericState<T>& state) -> Eigen::Vector3<T> {
            return (state.misalignmentAccel * state.m.p_acceleration.template cast<T>()).cwiseProduct(state.scaleFactorAccel) + state.p_biasAcceleration;
        };
        auto p_gyro = [](const GenericState<T>& state) -> Eigen::Vector3<T> {
            return (state.misalignmentGyro * state.m.p_angularRate.template cast<T>()).cwiseProduct(state.scaleFactorGyro) + state.p_biasAngularRate;
        };
 
        // LOG_DATA("{}: imuPos.b_quat_p() [{}]", nameId, imuPos.b_quat_p());
        Eigen::Vector3<T> b_accel__t0 = imuPos.b_quat_p().cast<T>() * p_accel(state__t0);
        Eigen::Vector3<T> b_accel__t1 = imuPos.b_quat_p().cast<T>() * p_accel(state__t1);
        Eigen::Vector3<T> b_gyro__t0 = imuPos.b_quat_p().cast<T>() * p_gyro(state__t0);
        Eigen::Vector3<T> b_gyro__t1 = imuPos.b_quat_p().cast<T>() * p_gyro(state__t1);
        // LOG_DATA("{}: b_accel__t0 [{}, {}, {}], b_accel__t1 [{}, {}, {}]", nameId, b_accel__t0(0), b_accel__t0(1), b_accel__t0(2), b_accel__t1(0), b_accel__t1(1), b_accel__t1(2));
        // LOG_DATA("{}: b_gyro__t0 [{}, {}, {}], b_gyro__t1 [{}, {}, {}]", nameId, b_gyro__t0(0), b_gyro__t0(1), b_gyro__t0(2), b_gyro__t1(0), b_gyro__t1(1), b_gyro__t1(2));
 
        // LOG_DATA("{}: integrationAlgorithm = {}", nameId, to_string(_integrationAlgorithm));
        switch (_integrationAlgorithm)
        {
        case IntegrationAlgorithm::SingleStepRungeKutta1:
        {
            std::array<Eigen::Vector<T, 6>, 1> z;
            if (state__t0.m.averagedMeasurement) { z[0] << b_accel__t0, b_gyro__t0; }
            else { z[0] << b_accel__t1, b_gyro__t1; }
            // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
 
            switch (_integrationFrame)
            {
            case IntegrationFrame::NED:
                y = RungeKutta1(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            case IntegrationFrame::ECEF:
                y = RungeKutta1(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            }
            break;
        }
        case IntegrationAlgorithm::SingleStepRungeKutta2:
        {
            std::array<Eigen::Vector<T, 6>, 2> z;
            if (state__t0.m.averagedMeasurement)
            {
                z[0] << b_accel__t0, b_gyro__t0;
                z[1] << b_accel__t0, b_gyro__t0;
            }
            else
            {
                z[0] << b_accel__t1, b_gyro__t1;
                z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
            }
            // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
            // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
            switch (_integrationFrame)
            {
            case IntegrationFrame::NED:
                y = RungeKutta2(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            case IntegrationFrame::ECEF:
                y = RungeKutta2(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            }
            break;
        }
        case IntegrationAlgorithm::SingleStepHeun2:
        {
            std::array<Eigen::Vector<T, 6>, 2> z;
            if (state__t0.m.averagedMeasurement)
            {
                z[0] << b_accel__t0, b_gyro__t0;
                z[1] << b_accel__t0, b_gyro__t0;
            }
            else
            {
                z[0] << b_accel__t1, b_gyro__t1;
                z[1] << b_accel__t0, b_gyro__t0;
            }
            // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
            // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
            switch (_integrationFrame)
            {
            case IntegrationFrame::NED:
                y = Heun2(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            case IntegrationFrame::ECEF:
                y = Heun2(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            }
            break;
        }
        case IntegrationAlgorithm::SingleStepRungeKutta3:
        {
            std::array<Eigen::Vector<T, 6>, 3> z;
            if (state__t0.m.averagedMeasurement)
            {
                z[0] << b_accel__t0, b_gyro__t0;
                z[1] << b_accel__t0, b_gyro__t0;
                z[2] << b_accel__t0, b_gyro__t0;
            }
            else
            {
                z[0] << b_accel__t1, b_gyro__t1;
                z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
                z[2] << b_accel__t0, b_gyro__t0;
            }
            // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
            // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
            // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5));
            switch (_integrationFrame)
            {
            case IntegrationFrame::NED:
                y = RungeKutta3(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            case IntegrationFrame::ECEF:
                y = RungeKutta3(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            }
            break;
        }
        case IntegrationAlgorithm::SingleStepHeun3:
        {
            std::array<Eigen::Vector<T, 6>, 3> z;
            if (state__t0.m.averagedMeasurement)
            {
                z[0] << b_accel__t0, b_gyro__t0;
                z[1] << b_accel__t0, b_gyro__t0;
                z[2] << b_accel__t0, b_gyro__t0;
            }
            else
            {
                z[0] << b_accel__t1, b_gyro__t1;
                z[1] << math::lerp(b_accel__t1, b_accel__t0, 1.0 / 3.0), math::lerp(b_gyro__t1, b_gyro__t0, 1.0 / 3.0);
                z[2] << math::lerp(b_accel__t1, b_accel__t0, 2.0 / 3.0), math::lerp(b_gyro__t1, b_gyro__t0, 2.0 / 3.0);
            }
            // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
            // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
            // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5));
            switch (_integrationFrame)
            {
            case IntegrationFrame::NED:
                y = RungeKutta3(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            case IntegrationFrame::ECEF:
                y = RungeKutta3(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            }
            break;
        }
        case IntegrationAlgorithm::SingleStepRungeKutta4:
        {
            std::array<Eigen::Vector<T, 6>, 4> z;
            if (state__t0.m.averagedMeasurement)
            {
                z[0] << b_accel__t0, b_gyro__t0;
                z[1] << b_accel__t0, b_gyro__t0;
                z[2] << b_accel__t0, b_gyro__t0;
                z[3] << b_accel__t0, b_gyro__t0;
            }
            else
            {
                z[0] << b_accel__t1, b_gyro__t1;
                z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
                z[2] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
                z[3] << b_accel__t0, b_gyro__t0;
            }
            // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
            // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
            // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5));
            // LOG_DATA("{}: z[3] = {}, {}, {}, {}, {}, {}", nameId, z[3](0), z[3](1), z[3](2), z[3](3), z[3](4), z[3](5));
            switch (_integrationFrame)
            {
            case IntegrationFrame::NED:
                y = RungeKutta4(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            case IntegrationFrame::ECEF:
                y = RungeKutta4(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants);
                break;
            }
            break;
        }
        case IntegrationAlgorithm::MultiStepRK3:
        {
            LOG_CRITICAL("Not implemented yet");
            break;
        }
        case IntegrationAlgorithm::MultiStepRK4:
        {
            LOG_CRITICAL("Not implemented yet");
            break;
        }
        case IntegrationAlgorithm::COUNT:
        {
            LOG_CRITICAL("Unreachable");
            break;
        }
        }
 
        y.template segment<4>(6) = Eigen::Quaternion<T>(y.template segment<4>(6)).normalized().coeffs();
 
        return y;
    }
 
  private:
    void addMeasurement(const InsTime& epoch, double dt,
                        const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate, const char* nameId);

    void addDeltaMeasurement(const InsTime& epoch, double dt, double deltaTime,
                             const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta, const char* nameId);

    [[nodiscard]] std::shared_ptr<PosVelAtt> lastStateAsPosVelAtt() const;

    std::shared_ptr<PosVelAtt> calcInertialSolutionFromMeasurementBuffer(const ImuPos& imuPos, const char* nameId);

    void setBufferSizes();

    ScrollingBuffer<State> _states = ScrollingBuffer<State>(1);
 
    Eigen::Vector3d _p_lastBiasAcceleration = Eigen::Vector3d::Zero(); 
    Eigen::Vector3d _p_lastBiasAngularRate = Eigen::Vector3d::Zero();  
 
    // #########################################################################################################################################

    IntegrationFrame _integrationFrame = IntegrationFrame::NED;

    bool _lockIntegrationFrame = false;

    IntegrationAlgorithm _integrationAlgorithm = IntegrationAlgorithm::SingleStepRungeKutta3;

    bool _accelerationsAreAveragedMeasurements = true;
 
    // #########################################################################################################################################

    PosVelAttDerivativeConstants _posVelAttDerivativeConstants;
 
    friend bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, bool& preferAccelerationOverDeltaMeasurements, float width);
    friend void to_json(json& j, const InertialIntegrator& data);
    friend void from_json(const json& j, InertialIntegrator& data);
};

bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, bool& preferAccelerationOverDeltaMeasurements, float width = 250.0F);

void to_json(json& j, const InertialIntegrator& data);
void from_json(const json& j, InertialIntegrator& data);

const char* to_string(InertialIntegrator::IntegrationAlgorithm algorithm);

const char* to_string(InertialIntegrator::IntegrationFrame frame);
 
} // namespace NAV