Wifi positioning

CMakeLists.txt

@@ -200,6 +200,9 @@ include_directories(SYSTEM lib/vnproglib/cpp/include)
 set_target_properties(libvncxx PROPERTIES CXX_CLANG_TIDY "")
 set_target_properties(libvncxx PROPERTIES CXX_CPPCHECK "")
 
+# ----------------------------------------------- libssh ------------------------------------------------- */
+find_package(libssh REQUIRED)
+
 # ------------------------------------------------ Navio2 ------------------------------------------------ */
 
 if(NOT APPLE AND NOT WIN32)

README.md

@@ -37,6 +37,7 @@ Read the docs on
 
 ##### Build & run the main program
 ```shell
+sudo apt-get install libssh-dev
 conan install . --build=missing -s build_type=Release -s compiler.cppstd=20
 # Windows needs the argument -DCMAKE_TOOLCHAIN_FILE="build/generators/conan_toolchain.cmake"
 cmake -Bbuild/Release -S. -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE="build/Release/generators/conan_toolchain.cmake" -DENABLE_MAIN=ON -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=OFF -DENABLE_CLANG_TIDY=OFF -DENABLE_CPPCHECK=OFF -DLOG_LEVEL=INFO

cmake/modules/FindLibSSH.cmake

@@ -0,0 +1,68 @@
+# - Try to find LibSSH
+# Once done this will define
+#
+#  LIBSSH_FOUND - system has LibSSH
+#  LIBSSH_INCLUDE_DIRS - the LibSSH include directory
+#  LIBSSH_LIBRARIES - Link these to use LibSSH
+#  LIBSSH_DEFINITIONS - Compiler switches required for using LibSSH
+#
+#  Copyright (c) 2009 Andreas Schneider <[email protected]>
+#
+#  Redistribution and use is allowed according to the terms of the New
+#  BSD license.
+#  For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+#
+
+if(NOT (LIBSSH_INCLUDE_DIR AND LIBSSH_LIBRARY))
+  find_package(PkgConfig)
+  pkg_check_modules(PC_LIBSSH QUIET libssh)
+
+  find_path(LIBSSH_INCLUDE_DIR
+    NAMES
+      libssh/libssh.h
+    PATHS
+      /usr/include
+      /usr/local/include
+      /opt/local/include
+      /sw/include
+      ${CMAKE_INCLUDE_PATH}
+      ${CMAKE_INSTALL_PREFIX}/include
+  )
+
+  find_library(LIBSSH_LIBRARY
+    NAMES
+      ssh
+      libssh
+    PATHS
+      /usr/lib
+      /usr/local/lib
+      /opt/local/lib
+      /sw/lib
+      ${CMAKE_LIBRARY_PATH}
+      ${CMAKE_INSTALL_PREFIX}/lib
+  )
+
+  mark_as_advanced(LIBSSH_INCLUDE_DIR LIBSSH_LIBRARY)
+endif(NOT (LIBSSH_INCLUDE_DIR AND LIBSSH_LIBRARY))
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(LIBSSH
+    FOUND_VAR LIBSSH_FOUND
+    REQUIRED_VARS LIBSSH_LIBRARY LIBSSH_INCLUDE_DIR)
+
+if(LIBSSH_FOUND)
+  set(LIBSSH_LIBRARIES ${LIBSSH_LIBRARY})
+  set(LIBSSH_INCLUDE_DIRS ${LIBSSH_INCLUDE_DIR})
+  set(LIBSSH_DEFINITIONS ${PC_LIBSSH_CFLAGS_OTHER})
+endif()
+
+if(LIBSSH_FOUND AND NOT TARGET LIBSSH::LIBSSH)
+  add_library(LIBSSH::LIBSSH UNKNOWN IMPORTED)
+  set_target_properties(
+      LIBSSH::LIBSSH PROPERTIES
+      IMPORTED_LOCATION "${LIBSSH_LIBRARY}"
+      INTERFACE_COMPILE_OPTIONS "${PC_LIBSSH_CFLAGS_OTHER}"
+      INTERFACE_INCLUDE_DIRECTORIES "${LIBSSH_INCLUDE_DIR}")
+endif()
+
+

conanfile.txt

@@ -1,6 +1,6 @@
 [requires]
 spdlog/1.13.0
-fmt/10.2.1
+fmt/[<=10.2.1]
 boost/1.83.0
 eigen/3.4.0
 catch2/3.4.0

src/CMakeLists.txt

@@ -37,6 +37,7 @@ if(ENABLE_MAIN)
             ImGuiFileDialog
             implot
             application
+            ssh
             libvncxx
             libUartSensor)
 

src/NodeData/WiFi/WiFiObs.hpp

@@ -0,0 +1,52 @@
+// 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 WiFiObs.hpp
+/// @brief Espressif Observation Class
+/// @author R. Lintz ([email protected]) (master thesis)
+/// @date 2024-01-08
+
+#pragma once
+
+#include "NodeData/NodeData.hpp"
+#include "util/Vendor/VectorNav/BinaryOutputs/TimeOutputs.hpp"
+
+namespace NAV
+{
+/// Espressif Observation Class
+class WiFiObs : public NodeData
+{
+  public:
+    /// @brief Returns the type of the data class
+    /// @return The data type
+    [[nodiscard]] static std::string type()
+    {
+        return "WiFiObs";
+    }
+
+    /// @brief Returns the parent types of the data class
+    /// @return The parent data types
+    [[nodiscard]] static std::vector<std::string> parentTypes()
+    {
+        return { NodeData::type() };
+    }
+
+    /// Payload length in bytes
+    uint16_t payloadLength = 0;
+
+    /// MAC address of the device
+    std::string macAddress;
+    /// Distance to the device
+    double distance;
+    /// Standard deviation of the distance
+    double distanceStd;
+    /// Time of observation
+    NAV::vendor::vectornav::TimeOutputs timeOutputs;
+};
+
+} // namespace NAV

src/NodeData/WiFi/WiFiPositioningSolution.hpp

@@ -0,0 +1,141 @@
+// 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 WiFiPositioningSolution.hpp
+/// @brief WiFi Positioning Algorithm output
+/// @author R. Lintz ([email protected]) (master thesis)
+/// @author T. Topp ([email protected])
+/// @date 2024-03-12
+
+#pragma once
+
+#include "NodeData/State/PosVel.hpp"
+#include <Eigen/Dense>
+
+namespace NAV
+{
+class WiFiPositioningSolution : public PosVel
+{
+  public:
+    /// @brief Returns the type of the data class
+    /// @return The data type
+    [[nodiscard]] static std::string type()
+    {
+        return "WiFiPositioningSolution";
+    }
+
+    /// @brief Returns the parent types of the data class
+    /// @return The parent data types
+    [[nodiscard]] static std::vector<std::string> parentTypes()
+    {
+        auto parent = PosVel::parentTypes();
+        parent.push_back(PosVel::type());
+        return parent;
+    }
+
+    /// @brief Returns a vector of data descriptors
+    [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
+    {
+        auto desc = PosVel::GetStaticDataDescriptors();
+        desc.reserve(GetStaticDescriptorCount());
+        desc.emplace_back("Bias [m]");
+        desc.emplace_back("Bias StDev [m]");
+        return desc;
+    }
+
+    /// @brief Get the amount of descriptors
+    [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return 41; }
+
+    /// @brief Returns a vector of data descriptors
+    [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
+
+    /// @brief Get the amount of descriptors
+    [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
+
+    /// @brief Get the value at the index
+    /// @param idx Index corresponding to data descriptor order
+    /// @return Value if in the observation
+    [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
+    {
+        INS_ASSERT(idx < GetStaticDescriptorCount());
+        switch (idx)
+        {
+        case 0:  // Latitude [deg]
+        case 1:  // Longitude [deg]
+        case 2:  // Altitude [m]
+        case 3:  // North/South [m]
+        case 4:  // East/West [m]
+        case 5:  // X-ECEF [m]
+        case 6:  // Y-ECEF [m]
+        case 7:  // Z-ECEF [m]
+        case 8:  // X-ECEF StDev [m]
+        case 9:  // Y-ECEF StDev [m]
+        case 10: // Z-ECEF StDev [m]
+        case 11: // XY-ECEF StDev [m]
+        case 12: // XZ-ECEF StDev [m]
+        case 13: // YZ-ECEF StDev [m]
+        case 14: // North StDev [m]
+        case 15: // East StDev [m]
+        case 16: // Down StDev [m]
+        case 17: // NE StDev [m]
+        case 18: // ND StDev [m]
+        case 19: // ED StDev [m]
+        case 20: // Velocity norm [m/s]
+        case 21: // X velocity ECEF [m/s]
+        case 22: // Y velocity ECEF [m/s]
+        case 23: // Z velocity ECEF [m/s]
+        case 24: // North velocity [m/s]
+        case 25: // East velocity [m/s]
+        case 26: // Down velocity [m/s]
+        case 27: // X velocity ECEF StDev [m/s]
+        case 28: // Y velocity ECEF StDev [m/s]
+        case 29: // Z velocity ECEF StDev [m/s]
+        case 30: // XY velocity StDev [m]
+        case 31: // XZ velocity StDev [m]
+        case 32: // YZ velocity StDev [m]
+        case 33: // North velocity StDev [m/s]
+        case 34: // East velocity StDev [m/s]
+        case 35: // Down velocity StDev [m/s]
+        case 36: // NE velocity StDev [m]
+        case 37: // ND velocity StDev [m]
+        case 38: // ED velocity StDev [m]
+            return PosVel::getValueAt(idx);
+        case 39: // Bias [m]
+            return bias;
+        case 40: // Bias StDev [m]
+            return biasStdev;
+        default:
+            return std::nullopt;
+        }
+        return std::nullopt;
+    }
+
+    // --------------------------------------------------------- Public Members ------------------------------------------------------------
+    /// Bias [m]
+    double bias = std::nan("");
+    /// Standard deviation of Bias [m]
+    double biasStdev = std::nan("");
+
+  private:
+    /// Standard deviation of Position in ECEF coordinates [m]
+    Eigen::Vector3d _e_positionStdev = Eigen::Vector3d::Zero() * std::nan("");
+    /// Standard deviation of Position in local navigation frame coordinates [m]
+    Eigen::Vector3d _n_positionStdev = Eigen::Vector3d::Zero() * std::nan("");
+
+    /// Standard deviation of Velocity in earth coordinates [m/s]
+    Eigen::Vector3d _e_velocityStdev = Eigen::Vector3d::Zero() * std::nan("");
+    /// Standard deviation of Velocity in navigation coordinates [m/s]
+    Eigen::Vector3d _n_velocityStdev = Eigen::Vector3d::Zero() * std::nan("");
+
+    /// Covariance matrix in ECEF coordinates (Position, Velocity)
+    Eigen::MatrixXd _e_covarianceMatrix;
+    /// Covariance matrix in local navigation coordinates (Position, Velocity)
+    Eigen::MatrixXd _n_covarianceMatrix;
+};
+
+} // namespace NAV

src/NodeRegistry.cpp

@@ -190,6 +190,7 @@ std::vector<std::string> NAV::NodeRegistry::GetParentNodeDataTypes(const std::st
 #include "Nodes/DataLogger/GNSS/RinexObsLogger.hpp"
 #include "Nodes/DataLogger/GNSS/UartDataLogger.hpp"
 #include "Nodes/DataLogger/IMU/VectorNavDataLogger.hpp"
+#include "Nodes/DataLogger/WiFi/WiFiObsLogger.hpp"
 // Data Processor
 #include "Nodes/DataProcessor/ErrorModel/ErrorModel.hpp"
 #include "Nodes/DataProcessor/GNSS/GnssAnalyzer.hpp"
@@ -198,6 +199,7 @@ std::vector<std::string> NAV::NodeRegistry::GetParentNodeDataTypes(const std::st
 #include "Nodes/DataProcessor/KalmanFilter/LooselyCoupledKF.hpp"
 #include "Nodes/DataProcessor/KalmanFilter/TightlyCoupledKF.hpp"
 #include "Nodes/DataProcessor/SensorCombiner/ImuFusion.hpp"
+#include "Nodes/DataProcessor/WiFi/WiFiPositioning.hpp"
 // Data Provider
 #include "Nodes/DataProvider/CSV/CsvFile.hpp"
 #include "Nodes/DataProvider/GNSS/FileReader/RinexNavFile.hpp"
@@ -217,6 +219,9 @@ std::vector<std::string> NAV::NodeRegistry::GetParentNodeDataTypes(const std::st
 #include "Nodes/DataProvider/IMU/FileReader/UlogFile.hpp"
 #include "Nodes/DataProvider/State/PosVelAttFile.hpp"
 #include "Nodes/DataProvider/IMU/FileReader/MultiImuFile.hpp"
+#include "Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp"
+#include "Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp"
+#include "Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.hpp"
 // Data Simulator
 #include "Nodes/DataProvider/IMU/Simulators/ImuSimulator.hpp"
 // Plotting
@@ -257,6 +262,7 @@ void NAV::NodeRegistry::RegisterNodeTypes()
     registerNodeType<RinexObsLogger>();
     registerNodeType<UartDataLogger>();
     registerNodeType<VectorNavDataLogger>();
+    registerNodeType<WiFiObsLogger>();
     // Data Processor
     registerNodeType<ErrorModel>();
     registerNodeType<GnssAnalyzer>();
@@ -265,6 +271,7 @@ void NAV::NodeRegistry::RegisterNodeTypes()
     registerNodeType<LooselyCoupledKF>();
     // registerNodeType<TightlyCoupledKF>();
     registerNodeType<ImuFusion>();
+    registerNodeType<WiFiPositioning>();
     // Data Provider
     registerNodeType<CsvFile>();
     registerNodeType<RinexNavFile>();
@@ -284,6 +291,9 @@ void NAV::NodeRegistry::RegisterNodeTypes()
     registerNodeType<UlogFile>();
     registerNodeType<PosVelAttFile>();
     registerNodeType<MultiImuFile>();
+    registerNodeType<EspressifSensor>();
+    registerNodeType<ArubaSensor>();
+    registerNodeType<WiFiObsFile>();
     // Data Simulator
     registerNodeType<ImuSimulator>();
     // Experimental
@@ -316,6 +326,8 @@ void NAV::NodeRegistry::RegisterNodeTypes()
 #include "NodeData/State/Pos.hpp"
 #include "NodeData/State/PosVel.hpp"
 #include "NodeData/State/PosVelAtt.hpp"
+#include "NodeData/WiFi/WiFiObs.hpp"
+#include "NodeData/WiFi/WiFiPositioningSolution.hpp"
 
 void NAV::NodeRegistry::RegisterNodeDataTypes()
 {
@@ -342,4 +354,7 @@ void NAV::NodeRegistry::RegisterNodeDataTypes()
     registerNodeDataType<Pos>();
     registerNodeDataType<PosVel>();
     registerNodeDataType<PosVelAtt>();
+    // WiFi
+    registerNodeDataType<WiFiObs>();
+    registerNodeDataType<WiFiPositioningSolution>();
 }