WiFiPositioning.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 "util/Eigen.hpp"
#include "util/CallbackTimer.hpp"
#include <vector>
 
#include "internal/Node/Node.hpp"
#include "NodeData/WiFi/WiFiObs.hpp"
#include "NodeData/State/Pos.hpp"
#include "NodeData/State/PosVel.hpp"
 
#include "Navigation/Math/KeyedKalmanFilter.hpp"
#include "Navigation/Math/KalmanFilter.hpp"
#include "Navigation/Math/LeastSquares.hpp"
 
namespace NAV
{
class WiFiPositioning : public Node
{
  public:
    WiFiPositioning();
    ~WiFiPositioning() override;
    WiFiPositioning(const WiFiPositioning&) = delete;
    WiFiPositioning(WiFiPositioning&&) = delete;
    WiFiPositioning& operator=(const WiFiPositioning&) = delete;
    WiFiPositioning& operator=(WiFiPositioning&&) = 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;
 
  private:
    constexpr static size_t INPUT_PORT_INDEX_WIFI_OBS = 0; 
    constexpr static size_t OUTPUT_PORT_INDEX_WIFISOL = 0; 
 
    uint8_t _numStates = 6;
 
    uint8_t _numMeasurements = 1;
 
    KalmanFilter _kalmanFilter{ _numStates, _numMeasurements }; // TODO: Change to KeyedKalmanFilter
 
    // --------------------------------------------------------------- Gui -----------------------------------------------------------------
 
    bool initialize() override;
 
    void deinitialize() override;
 
    size_t _nWifiInputPins = 1;
 
    void updateNumberOfInputPins();
 
    // ------------------------------------------------------------ Algorithm --------------------------------------------------------------
 
    enum class Frame : uint8_t
    {
        ECEF, 
        LLA,  
    };
    enum class Frame : uint8_t {…};
    Frame _frame = Frame::ECEF;
 
    enum class SolutionMode : uint8_t
    {
        LSQ, 
        KF,  
    };
    enum class SolutionMode : uint8_t {…};
    SolutionMode _solutionMode = SolutionMode::LSQ;
 
    bool _estimateBias = false;
 
    bool _weightedSolution = false;
 
    bool _useInitialValues = false;
 
    struct State
    {
        Eigen::Vector3d e_position = Eigen::Vector3d::Zero();
        Eigen::Vector3d e_velocity = Eigen::Vector3d::Zero();
        double bias = 0;
    };
    struct State {…};
 
    State _state;
 
    State _initialState;
 
    std::vector<std::string> _deviceMacAddresses{ 1 };
 
    std::vector<Eigen::Vector3d> _devicePositions{ 1 };
 
    std::vector<double> _deviceBias{ 1, 0.0 };
 
    std::vector<double> _deviceScale{ 1, 0.0 };
 
    size_t _numOfDevices = 0;
 
    struct Device
    {
        Eigen::Vector3d position{ Eigen::Vector3d::Zero() }; 
        InsTime time;                                        
        double distance{ 0.0 };                              
        double distanceStd{ 0.0 };                           
    };
    struct Device {…};
    std::vector<Device> _devices{ 1 };
 
    InsTime _lastPredictTime;
 
    void recvWiFiObs(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    LeastSquaresResult<Eigen::VectorXd, Eigen::MatrixXd> lsqSolution();
 
    void kfSolution();
 
    // ###########################################################################################################
 
    enum class MeasurementNoiseUnit : uint8_t
    {
        meter2, 
        meter,  
    };
    enum class MeasurementNoiseUnit : uint8_t {…};
    MeasurementNoiseUnit _measurementNoiseUnit = MeasurementNoiseUnit::meter;
 
    double _measurementNoise = 10;
 
    // ###########################################################################################################
 
    enum class ProcessNoiseUnit : uint8_t
    {
        meter2, 
        meter,  
    };
    enum class ProcessNoiseUnit : uint8_t {…};
    ProcessNoiseUnit _processNoiseUnit = ProcessNoiseUnit::meter;
 
    double _processNoise = 10;
 
    // ###########################################################################################################
 
    enum class InitCovariancePositionUnit : uint8_t
    {
        meter2, 
        meter,  
    };
    enum class InitCovariancePositionUnit : uint8_t {…};
    InitCovariancePositionUnit _initCovariancePositionUnit = InitCovariancePositionUnit::meter;
 
    Eigen::Vector3d _initCovariancePosition{ 100, 100, 100 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceVelocityUnit : uint8_t
    {
        m2_s2, 
        m_s,   
    };
    enum class InitCovarianceVelocityUnit : uint8_t {…};
    InitCovarianceVelocityUnit _initCovarianceVelocityUnit = InitCovarianceVelocityUnit::m_s;
 
    Eigen::Vector3d _initCovarianceVelocity{ 10, 10, 10 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceBiasUnit : uint8_t
    {
        meter2, 
        meter,  
    };
    enum class InitCovarianceBiasUnit : uint8_t {…};
    InitCovarianceBiasUnit _initCovarianceBiasUnit = InitCovarianceBiasUnit::meter;
 
    double _initCovarianceBias = 10;
};
class WiFiPositioning : public Node {…};
} // namespace NAV