Repository: Navigine/Indoor-Positioning-And-Navigation-Algorithms Branch: master Commit: 67e11c4d398a Files: 149 Total size: 418.2 KB Directory structure: gitextract_n3y_rmqw/ ├── .gitattributes ├── .github/ │ ├── ISSUE_TEMPLATE/ │ │ ├── bug_report.md │ │ ├── custom.md │ │ └── feature_request.md │ └── workflows/ │ ├── macos.yml │ └── ubuntu.yml ├── .gitignore ├── CMakeLists.txt ├── CODE_OF_CONDUCT.md ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── include/ │ └── navigine/ │ └── navigation-core/ │ ├── CHANGELOG.md │ ├── README.md │ ├── barriers_geometry_builder.h │ ├── boost_geometry_adaptation.h │ ├── declare_identifier.h │ ├── geolevel.h │ ├── graph.h │ ├── graph_particle.h │ ├── level.h │ ├── level_collector.h │ ├── level_geometry.h │ ├── location_point.h │ ├── logger.h │ ├── motion_info.h │ ├── navigation_client.h │ ├── navigation_input.h │ ├── navigation_output.h │ ├── navigation_settings.h │ ├── navigation_state.h │ ├── point.h │ ├── radiomap.h │ ├── reference_point.h │ ├── transmitter.h │ ├── vector3d.h │ ├── version.h │ └── xy_particle.h ├── src/ │ ├── README │ ├── barriers_geometry_builder.cpp │ ├── device_properties.h │ ├── geometry.cpp │ ├── geometry.h │ ├── knn/ │ │ ├── knn_utils.cpp │ │ └── knn_utils.h │ ├── level.cpp │ ├── level_collector.cpp │ ├── level_estimator/ │ │ ├── barometer.cpp │ │ ├── barometer.h │ │ ├── level_estimator.cpp │ │ ├── level_estimator.h │ │ ├── level_estimator_radiomap.cpp │ │ ├── level_estimator_radiomap.h │ │ ├── level_estimator_transmitters.cpp │ │ ├── level_estimator_transmitters.h │ │ ├── level_history.cpp │ │ └── level_history.h │ ├── level_geometry.cpp │ ├── likelihood/ │ │ ├── likelihood.cpp │ │ ├── likelihood.h │ │ ├── likelihood_radiomap.cpp │ │ └── likelihood_radiomap.h │ ├── measurement_types.h │ ├── measurements/ │ │ ├── measurement_preprocessor.cpp │ │ └── measurement_preprocessor.h │ ├── navigation_client_impl.cpp │ ├── navigation_client_impl.h │ ├── navigation_error_codes.h │ ├── point.cpp │ ├── position.h │ ├── position_estimator/ │ │ ├── Readme.md │ │ ├── docs/ │ │ │ └── KNN.md │ │ ├── position_estimator.h │ │ ├── position_estimator_knn.cpp │ │ ├── position_estimator_knn.h │ │ ├── position_estimator_outdoor.cpp │ │ ├── position_estimator_outdoor.h │ │ ├── position_estimator_zone.cpp │ │ └── position_estimator_zone.h │ ├── position_postprocessor/ │ │ ├── navigation_time_smoother.cpp │ │ ├── navigation_time_smoother.h │ │ ├── polynomial_fit.cpp │ │ ├── polynomial_fit.h │ │ ├── position_postprocessor.cpp │ │ ├── position_postprocessor.h │ │ ├── position_smoother.h │ │ ├── position_smoother_ab.cpp │ │ ├── position_smoother_ab.h │ │ ├── position_smoother_lstsq.cpp │ │ └── position_smoother_lstsq.h │ ├── radiomap.cpp │ ├── sensor_fusion/ │ │ ├── complementary_filter.cpp │ │ ├── complementary_filter.h │ │ ├── pedometer.cpp │ │ ├── pedometer.h │ │ ├── quaternion.cpp │ │ ├── quaternion.h │ │ ├── sensor_fusion.cpp │ │ └── sensor_fusion.h │ ├── triangulation.cpp │ ├── triangulation.h │ ├── trilateration.cpp │ ├── trilateration.h │ └── vector3d.cpp ├── standalone_algorithms/ │ ├── README.md │ ├── System description.md │ ├── complementary_filter/ │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── examples/ │ │ │ └── main.cpp │ │ ├── helpers/ │ │ │ └── plot_angles.py │ │ ├── include/ │ │ │ ├── complementary_filter.h │ │ │ ├── quaternion.h │ │ │ └── vector3d.h │ │ ├── src/ │ │ │ ├── complementary_filter.cpp │ │ │ ├── quaternion.cpp │ │ │ └── vector3d.cpp │ │ └── tests/ │ │ └── complementary_filter_test.cpp │ ├── pedometer/ │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── examples/ │ │ │ └── main.cpp │ │ ├── include/ │ │ │ ├── pedometer.h │ │ │ └── vector3d.h │ │ ├── src/ │ │ │ ├── pedometer.cpp │ │ │ └── vector3d.cpp │ │ └── tests/ │ │ └── pedometer_test.cpp │ ├── position_estimation/ │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── examples/ │ │ │ └── main.cpp │ │ ├── helpers/ │ │ │ └── plot_positions.py │ │ ├── include/ │ │ │ ├── measurement_preprocessor.h │ │ │ ├── navigation_structures.h │ │ │ ├── nearest_transmitter_estimator.h │ │ │ ├── position_smoother.h │ │ │ └── transmitter.h │ │ ├── logs/ │ │ │ └── transmitters.txt │ │ └── src/ │ │ ├── measurement_preprocessor.cpp │ │ ├── nearest_transmitter_estimator.cpp │ │ └── position_smoother.cpp │ └── trilateteration/ │ ├── CMakeLists.txt │ ├── README.md │ └── src/ │ ├── beacon.cpp │ ├── beacon.h │ ├── test_trilateration.cpp │ ├── test_trilateration.h │ ├── trilateration.cpp │ └── trilateration.h ├── tests/ │ └── navigation_test.cpp └── tools/ └── verification/ ├── helpers.cpp └── helpers.h ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitattributes ================================================ # Auto detect text files and perform LF normalization * text=auto # Custom for Visual Studio *.cs diff=csharp *.sln merge=union *.csproj merge=union *.vbproj merge=union *.fsproj merge=union *.dbproj merge=union # Standard to msysgit *.doc diff=astextplain *.DOC diff=astextplain *.docx diff=astextplain *.DOCX diff=astextplain *.dot diff=astextplain *.DOT diff=astextplain *.pdf diff=astextplain *.PDF diff=astextplain *.rtf diff=astextplain *.RTF diff=astextplain ================================================ FILE: .github/ISSUE_TEMPLATE/bug_report.md ================================================ --- name: Bug report about: Create a report to help us improve title: '' labels: '' assignees: '' --- **Describe the bug** A clear and concise description of what the bug is. **To Reproduce** Steps to reproduce the behavior: 1. Go to '...' 2. Click on '....' 3. Scroll down to '....' 4. See error **Expected behavior** A clear and concise description of what you expected to happen. **Screenshots** If applicable, add screenshots to help explain your problem. **Desktop (please complete the following information):** - OS: [e.g. iOS] - Browser [e.g. chrome, safari] - Version [e.g. 22] **Smartphone (please complete the following information):** - Device: [e.g. iPhone6] - OS: [e.g. iOS8.1] - Browser [e.g. stock browser, safari] - Version [e.g. 22] **Additional context** Add any other context about the problem here. ================================================ FILE: .github/ISSUE_TEMPLATE/custom.md ================================================ --- name: Custom issue template about: Describe this issue template's purpose here. title: '' labels: '' assignees: '' --- ================================================ FILE: .github/ISSUE_TEMPLATE/feature_request.md ================================================ --- name: Feature request about: Suggest an idea for this project title: '' labels: '' assignees: '' --- **Is your feature request related to a problem? Please describe.** A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] **Describe the solution you'd like** A clear and concise description of what you want to happen. **Describe alternatives you've considered** A clear and concise description of any alternative solutions or features you've considered. **Additional context** Add any other context or screenshots about the feature request here. ================================================ FILE: .github/workflows/macos.yml ================================================ name: macOS on: push: branches: - dev - master pull_request: jobs: xcode: runs-on: macos-10.15 strategy: matrix: xcode: [12.4, 12.3, 12.2, 12.1.1, 12.1, 12] env: DEVELOPER_DIR: /Applications/Xcode_${{ matrix.xcode }}.app/Contents/Developer steps: - uses: actions/checkout@v2 # install dependencies - name: boost run: brew install boost - name: cmake run: cmake -S . -B build -D CMAKE_BUILD_TYPE=Debug -DJSON_BuildTests=On -DJSON_FastTests=ON - name: build run: cmake --build build --parallel 10 - name: test run: cd build ; ctest -j 10 --output-on-failure ================================================ FILE: .github/workflows/ubuntu.yml ================================================ name: Ubuntu on: push: branches: - dev - master pull_request: jobs: ci_test_clang: runs-on: ubuntu-latest steps: # install dependencies - name: boost run: sudo apt-get update && sudo apt-get install libboost-all-dev - uses: actions/checkout@v2 - name: install_ninja run: | sudo apt update sudo apt install ninja-build shell: bash - name: install_clang run: | wget https://apt.llvm.org/llvm.sh chmod +x llvm.sh sudo ./llvm.sh 11 sudo apt-get install clang-tools-11 shell: bash - name: cmake run: cmake -S . -B build -DJSON_CI=On - name: build run: cmake --build build ci_test_gcc: runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 # install dependencies - name: boost run: sudo apt-get update && sudo apt-get install libboost-all-dev - name: cmake run: cmake -S . -B build -DJSON_CI=On - name: build run: cmake --build build ================================================ FILE: .gitignore ================================================ # Windows image file caches Thumbs.db ehthumbs.db # Folder config file Desktop.ini # Recycle Bin used on file shares $RECYCLE.BIN/ # Windows Installer files *.cab *.msi *.msm *.msp # ========================= # Operating System Files # ========================= # OSX # ========================= .DS_Store .AppleDouble .LSOverride # Icon must end with two \r Icon # Thumbnails ._* # Files that might appear on external disk .Spotlight-V100 .Trashes # Directories potentially created on remote AFP share .AppleDB .AppleDesktop Network Trash Folder Temporary Items .apdisk *.sdf *.pdb *.sbr *.obj *.tlog *.ilk *.log *.idb *.bsc *.opensdf # Prerequisites *.d # Compiled Object files *.slo *.lo *.o *.obj # Precompiled Headers *.gch *.pch # Compiled Dynamic libraries *.so *.dylib *.dll # Fortran module files *.mod *.smod # Compiled Static libraries *.lai *.la *.a *.lib # Executables *.exe *.out *.app CMakeLists.txt.user CMakeCache.txt CMakeFiles CMakeScripts Testing Makefile cmake_install.cmake install_manifest.txt compile_commands.json CTestTestfile.cmake _deps [Bb][Uu][Ii][Ll][Dd]/ .vscode/ ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.7) project(Navigation-core) include(FetchContent) FetchContent_Declare(json GIT_REPOSITORY https://github.com/ArthurSonzogni/nlohmann_json_cmake_fetchcontent.git GIT_TAG v3.10.3) FetchContent_GetProperties(json) if(NOT json_POPULATED) FetchContent_Populate(json) add_subdirectory(${json_SOURCE_DIR} ${json_BINARY_DIR} EXCLUDE_FROM_ALL) endif() FetchContent_Declare( Eigen GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git GIT_TAG master GIT_SHALLOW TRUE GIT_PROGRESS TRUE) set(EIGEN_BUILD_DOC OFF) set(BUILD_TESTING OFF) set(EIGEN_BUILD_PKGCONFIG OFF) set( OFF) FetchContent_MakeAvailable(Eigen) set(BOOST_ENABLE_CMAKE ON) set(Boost_USE_STATIC_LIBS ON) find_package(Boost REQUIRED) IF (Boost_FOUND) INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR}) message(${Boost_INCLUDE_DIR}) include_directories(${Boost_SOURCE_DIR}) ENDIF () #tests:navigation_test set("target" "navigation_test") list(APPEND "${target}__cxx_srcs" "tests/navigation_test.cpp" "tools/verification/helpers.cpp") add_executable("${target}" ${${target}__cxx_srcs}) target_include_directories("${target}" PRIVATE "tests/include/" "tests/build/" "include/") set_property(TARGET "${target}" PROPERTY CXX_STANDARD 17) set_target_properties("${target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;") set_target_properties("${target}" PROPERTIES COMPILE_FLAGS "-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions ") target_link_libraries("${target}" PRIVATE "navigation-core" nlohmann_json::nlohmann_json Eigen3::Eigen) #navigation-core:copy_public_include_dir set("target" "navigation-core_copy_public_include_dir") set("${target}__other_srcs" "include") set("${target}__output" "tests/build/obj/include") add_custom_command(OUTPUT ${${target}__output} COMMAND ${CMAKE_COMMAND} -E copy_directory "include" "tests/build/obj/include" DEPENDS ${${target}__other_srcs} WORKING_DIRECTORY "tests/build/" COMMENT "Copy ${target}" VERBATIM) add_custom_target("${target}" SOURCES ${${target}__other_srcs} DEPENDS ${${target}__output}) #navigation-core:navigation-core set("target" "navigation-core") list(APPEND "${target}__cxx_srcs" "src/geometry.cpp" "src/triangulation.cpp" "src/knn/knn_utils.cpp" "src/likelihood/likelihood.cpp" "src/likelihood/likelihood_radiomap.cpp" "src/measurements/measurement_preprocessor.cpp" "src/position_estimator/position_estimator_knn.cpp" "src/position_estimator/position_estimator_zone.cpp" "src/position_estimator/position_estimator_outdoor.cpp" "src/position_postprocessor/navigation_time_smoother.cpp" "src/position_postprocessor/polynomial_fit.cpp" "src/position_postprocessor/position_postprocessor.cpp" "src/position_postprocessor/position_smoother_ab.cpp" "src/position_postprocessor/position_smoother_lstsq.cpp" "src/sensor_fusion/complementary_filter.cpp" "src/sensor_fusion/quaternion.cpp" "src/sensor_fusion/pedometer.cpp" "src/sensor_fusion/sensor_fusion.cpp" "src/level_estimator/barometer.cpp" "src/level_estimator/level_estimator.cpp" "src/level_estimator/level_estimator_radiomap.cpp" "src/level_estimator/level_estimator_transmitters.cpp" "src/level_estimator/level_history.cpp" "src/navigation_client_impl.cpp" "src/barriers_geometry_builder.cpp" "src/level.cpp" "src/point.cpp" "src/level_collector.cpp" "src/level_geometry.cpp" "src/radiomap.cpp" "src/vector3d.cpp" "tools/verification/helpers.cpp" "src/trilateration.cpp") add_library("${target}" STATIC ${${target}__cxx_srcs}) # add_dependencies("${target}" boost_geometry) target_include_directories("${target}" PRIVATE "include/" "tests/build/") set_property(TARGET "${target}" PROPERTY CXX_STANDARD 17) set_target_properties("${target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;") set_target_properties("${target}" PROPERTIES COMPILE_FLAGS "-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions ") target_link_libraries("${target}" PRIVATE nlohmann_json::nlohmann_json Eigen3::Eigen) ================================================ FILE: CODE_OF_CONDUCT.md ================================================ #

Navigine Open Source Code of Conduct

This code of conduct provides guidance on participation in Navigine-managed open source communities and denotes the process for reporting unacceptable behavior. We are committed to providing a welcoming and inspiring community for all. People violating this code of conduct may be banned from the community. ## Our Standards Examples of behavior that contributes to a positive environment for our community include: - Demonstrating empathy and kindness toward other people - Being respectful of differing opinions, viewpoints, and experiences - Giving and gracefully accepting constructive feedback - Accepting responsibility and apologizing to those affected by our mistakes, and learning from the experience - Focusing on what is best not just for us as individuals, but for the overall community Examples of unacceptable behavior include: - The use of sexualized language or imagery, and sexual attention or advances of any kind - Trolling, insulting or derogatory comments, and personal or political attacks - Public or private harassment - Publishing others’ private information, such as a physical or email address, without their explicit permission - Other conduct which could reasonably be considered inappropriate in a professional setting - Advocating for or encouraging any of the above behaviors ## Enforcement Responsibilities Community leaders are responsible for clarifying and enforcing our standards of acceptable behavior and will take appropriate and fair corrective action in response to any behavior that they deem inappropriate, threatening, offensive, or harmful. Community leaders have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, and will communicate reasons for moderation decisions when appropriate. ## Scope This Code of Conduct applies within all community spaces, and also applies when an individual is officially representing the community in public spaces. Examples of representing our community include using an official e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event. ## Enforcement Instances of abusive, harassing, or otherwise unacceptable behavior may be reported to the community leaders responsible for enforcement at info@navigine.com. All complaints will be reviewed and investigated promptly and fairly. All community leaders are obligated to respect the privacy and security of the reporter of any incident. ## Attribution This Code of Conduct is adapted from the Contributor Covenant, version 2.1, available at https://www.contributor-covenant.org/version/2/1/code_of_conduct.html. For answers to common questions about this code of conduct, see the FAQ at https://www.contributor-covenant.org/faq. Translations are available at https://www.contributor-covenant.org/translations. ================================================ FILE: CONTRIBUTING.md ================================================ #

Contributing to Navigine Routing Library

The easiest way to help out is to submit bug reports and feature requests on our [issues](https://github.com/Navigine/Indoor-Routing-Library/issues) page. When submitting a bug report, please include: - The device and operating system version that produced the bug - The version or commit of Routing Library that produced the bug - Steps required to recreate the issue - What happened - What you expected to happen Thanks! ## Development To develop for the library, it is usually easiest to build and test your changes using either the Mac OS X or Ubuntu desktop targets. ## Code Style In general, code changes should follow the style of the surrounding code. When in doubt, you can use the provided clang-format style file for automatic styling. Install clang-format (available through brew or apt-get): ``` brew install clang-format ``` or ``` sudo apt-get install clang-format ``` Run clang-format with specified style (use -i to modify the contents of the specified file): ``` clang-format -i -style=file [file] ``` ================================================ FILE: LICENSE ================================================ MIT License Copyright (c) 2021 Navigine Corporation Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: README.md ================================================ Indoor-navigation-algorithms [![Build](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/ubuntu.yml/badge.svg)](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/ubuntu.yml) [![Build](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/macos.yml/badge.svg)](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/macos.yml) ============================ ## Useful Links - Refer to the [Navigine official website](https://navigine.com/) for complete list of downloads, useful materials, information about the company, and so on. - [Get started](http://locations.navigine.com/login) with Navigine to get full access to Navigation services, SDKs, and applications. - Refer to the Navigine [User Manual](http://docs.navigine.com/) for complete product usage guidelines. - Find company contact information at the official website under [Contacts](https://navigine.com/contacts/) tab. - Get support, contribute feedback, and connect with developers—visit [the Navigine Community](https://community.navigine.com/t/indoor-positioning-and-navigation-algorithms/173). ## Contents This repository consists of the following components * [src](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/src) - the source code of Navigine positioning algorithms * [standalone_algorithms](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/standalone_algorithms) - set of algorithms used in navigation * [tests](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/tests) - test and utilities for the evaluation of source code quality * [tools](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/tools/verification) - utilities for data extraction from map and settings ## Creating Navigation Client Here are some examples to give you an idea how to use [Navigation Client](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/blob/master/include/navigine/navigation-core/navigation_client.h). First, create variables that will store your data. ```c++ // variables for storing location elements navigine::navigation_core::GeoLevel geoLevel; std::shared_ptr levelCollector; navigine::navigation_core::NavigationSettings navigationSettings; std::shared_ptr navClient; ``` Add transmitters (Beacon, Eddystone, WIFI, WIFI-RTT, etc.) as `GeoTransmitters` from your location. Then they will be converted to `XYTransmitters` for internal evaluations Parameters: - id - identifier of transmitter like (major,minor,uuid) for beacon, (namespaceId,instanceId) for eddystone, mac for WIFI; - point - latitude and longitude as GeoPoint; - pathlossModel - A, B and power of transmitter as `PathlossModel` struct variable; - type - type of transmitter, like Beacon, Eddystone, WIFI, WIFI-RTT, etc.. Inside for loop add all the transmitters from your location. Here is an example of adding one element ```c++ geoLevel.transmitters.emplace_back(transmitterId, navigine::navigation_core::GeoPoint(latitude, longitude), navigine::navigation_core::PathlossModel{A, B, power}, navigine::navigation_core::TransmitterType::BEACON); ``` Create geometry of the level using method `getGeometry` from [barriers_geometry_builder.h](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/blob/master/include/navigine/navigation-core/barriers_geometry_builder.h) file. Geometry could be created using the barriers and the size of the level Parameters: - barrierList - list of polygons, where each polygon describes the barrier of the level; - allowedArea - polygon, which is created using width and height of the map; Create the list of Polygons which will describe all the barriers ```c++ std::list barriersList; for (size_t i = 0; i < barriers.size(); ++i) { auto coords = barriers.at(i); navigine::navigation_core::Polygon barrier; for (const auto& coord : coords) boost::geometry::append(barrier, navigine::navigation_core::Point(coord.first, coord.second)); barriersList.push_back(barrier); } ``` Create the polygon of allowed area ```c++ navigine::navigation_core::Polygon levelArea; auto boundingBox = navigation_core::Box( navigation_core::Point(leftMin.latitude, leftMin.longitude), navigation_core::Point(rightMax.latitude, rightMax.longitude)); boost::geometry::convert(boundingBox, allowedArea); geoLevel.geometry = navigine::navigation_core::getGeometry(barriersList, levelArea); ``` Create `LevelCollector` using method `createLevelCollector` and add all available geo levels. ```c++ levelCollector = navigine::navigation_core::createLevelCollector(); levelCollector->addGeoLevel(geoLevel); ``` Create `NavigationSettings`, with two parameters - level settings and common settings. (You can find them in navigation.xml i.e.) Create `NavigationClient` using method `createNavigationClient` which will take as arguments level collector and navigation settings. ```c++ navClient = navigine::navigation_core::createNavigationClient(levelCollector, navigationSettings); ``` ## Navigation test The example of navigation is presented in a file [Navigation Test](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/blob/master/tests/navigation_test.cpp). The test application takes 3 parameters: - path to the file of the location in` geojson` format - path to the file of logs gathered in this location in `json` format - path to the settings file in `json` format in which parameters of the algorithm are specified ### Build In order to build the test application go to the root directory of the repository and execute the following commands ``` sh cmake -H. -Bbuild cmake --build build ``` To run the tests ``` sh cd build/ ./navigation_test location.geojson log.json settings.json ``` ## Evaluate the navigation Quality of navigation can be evaluated via calculation of different navigational errors. The most important among them are presented in the following table | Error | Description | | :--- | :---- | | `nav_err` | Average navigation error in meters | | `q_75_err` | 75% quantile of all the measurements have navigation error less than the value of `nav_75_err` | | `avr_fi` | Average angle error in degrees | | `level_err` | Percentage of wrongly determined levels | | `exec_time` | Execution time in seconds | ## Navigation Algorithms explanation Here are lectures on navigational algorithms used in this repository - [Navigine's demo of WIFI-RTT](https://www.youtube.com/watch?v=VAzMhld3bH0&t) - [Pedometer Algorithm Explained](https://www.youtube.com/watch?v=cpwrwPTqMac&t) - [Complementary Filter](https://www.youtube.com/watch?v=49mJcFc2xXw) - [Particle Filter](https://www.youtube.com/watch?v=JFZ-HARdZA8) ================================================ FILE: include/navigine/navigation-core/CHANGELOG.md ================================================ # Backlog of changes in navigation protocol ## Version 1.0 * Added first version of the navigation protocol ## Version 1.1 * Deleted accuracy * Deleted battery * Deleted height from LOCATION ================================================ FILE: include/navigine/navigation-core/README.md ================================================ Message format ============================ Each message consists of one line of json format and describes an individual measurement. The line types are shown below. ### Type 101 - ACCELEROMETER ```{“timestamp”: , “type”: 101, “values”: [value_x, value_y, value_z]}``` ### Type 102 - GYROSCOPE ```{“timestamp”: , “type”: 102, “values”: [value_x, value_y, value_z]}``` ### Type 103 - MAGNETOMETER ```{“timestamp”: , “type”: 103, “values”: [value_x, value_y, value_z]}``` ### Type 104 - BAROMETER ```{“timestamp”: , “type”: 104, “values”: [pressure, pressure, pressure]}``` ### Type 105 - ORIENTATION ```{“timestamp”: , “type”: 105, “values”: [value_x, value_y, value_z]}``` ### Type 106 - LOCATION ```{“timestamp”: , “type”: 106, “values”: [lat, lon, accuracy]}``` ### Type 201 - WIFI ```{“timestamp”: , “type”: 201, “bssid”: , “rssi”: }``` ### Type 202 - BLE ```{“timestamp”: , “type”: 202, “bssid”: , “rssi”: }``` ### Type 203 - BEACON ```{“timestamp”: , “type”: 203, “bssid”: , “rssi”: , “power”: }``` ### Type 204 - EDDYSTONE ```{“timestamp”: , “type”: 204, “bssid”: , “rssi”: , “power”: }``` ### Type 205 - WIFI RTT ```{“timestamp”: , “type”: 205, “bssid”: , “distance”: , “stddev”: }``` ### Type 300 - NMEA ```{“timestamp”: , “type”: 300, “sentence_number”: , “num_sats”: }``` ### Type 900 - Comment ```{“timestamp”: , “type”: 900, “comment”: }``` ================================================ FILE: include/navigine/navigation-core/barriers_geometry_builder.h ================================================ /** barriers_geometry_builder.h * * Copyright (c) 2019 Navigine. * */ #ifndef NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H #define NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H #include "level_geometry.h" namespace navigine { namespace navigation_core { LevelGeometry getGeometry(const std::list& allowedArea); } } // namespace geometry::navigine::navigation_core #endif // NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H ================================================ FILE: include/navigine/navigation-core/boost_geometry_adaptation.h ================================================ #pragma once #include #include #include #include #include #include "xy_particle.h" BOOST_GEOMETRY_REGISTER_POINT_2D(navigine::navigation_core::XYPoint, double, boost::geometry::cs::cartesian, x, y); BOOST_GEOMETRY_REGISTER_POINT_2D(navigine::navigation_core::XYParticle, double, boost::geometry::cs::cartesian, x, y); namespace navigine { namespace navigation_core { using Point = boost::geometry::model::d2::point_xy; using Box = boost::geometry::model::box; using Ring = boost::geometry::model::ring; using Polygon = boost::geometry::model::polygon; using Multipolygon = boost::geometry::model::multi_polygon; using Linestring = boost::geometry::model::linestring; using Multilinestring = boost::geometry::model::multi_linestring; using BoxIdx = std::pair; using RTree = boost::geometry::index::rtree>; inline std::pair getWidthHeightPair(const Box& box) { double width = box.max_corner().x() - box.min_corner().x(); double height = box.max_corner().y() - box.min_corner().y(); return std::make_pair(width, height); } } } //namespace navigine::navigation_core ================================================ FILE: include/navigine/navigation-core/declare_identifier.h ================================================ #pragma once #include #define DECLARE_IDENTIFIER(IdentifierType) \ struct IdentifierType { \ std::string value; \ inline IdentifierType() = default; \ inline explicit IdentifierType(const std::string& value): value(value) {} \ inline bool isValid() const { return !value.empty(); } \ inline bool operator< (const IdentifierType& _id)const { return value < _id.value; } \ inline bool operator!= (const IdentifierType& _id)const { return value != _id.value; } \ inline bool operator== (const IdentifierType& _id)const { return value == _id.value; } \ }; \ \ struct Hasher##IdentifierType \ { \ std::size_t operator()(const IdentifierType& k) const \ { \ return std::hash()(k.value); \ } \ }; ================================================ FILE: include/navigine/navigation-core/geolevel.h ================================================ /** geolevel.h * * Copyright (c) 2019 Navigine. * */ #ifndef NAVIGINE_GEOLEVEL_H #define NAVIGINE_GEOLEVEL_H #include #include #include #include #include "level.h" #include "level_geometry.h" #include "boost_geometry_adaptation.h" #include "graph.h" #include "reference_point.h" #include "transmitter.h" namespace navigine { namespace navigation_core { struct GeoLevel { LevelId id; Geo3DTransmitters transmitters; GeoReferencePoints referencePoints; LevelGeometry geometry; Graph graph; double altitude; }; typedef std::vector > GeoLevels; } } // namespace navigine::navigation_core #endif // NAVIGINE_GEOLEVEL_H ================================================ FILE: include/navigine/navigation-core/graph.h ================================================ /** graph.h * * Copyright (c) 2019 Navigine. * */ #ifndef NAVIGINE_GRAPH_H #define NAVIGINE_GRAPH_H #include #include #include namespace navigine { namespace navigation_core { template class Graph { public: struct Vertex { int id; Point point; }; public: typedef typename std::vector::const_iterator VertexIterator; typedef std::set >::const_iterator EdgeIterator; public: Graph() {} bool hasVertex(int id) const { return id >= 0 && id < static_cast(mVertices.size()); } const Vertex& vertex(int id) const { return mVertices.at(id); } const Vertex getVertex(int id) const { return mVertices.at(id); } const VertexIterator vertexBegin() const { return mVertices.begin(); } const VertexIterator vertexEnd() const { return mVertices.end(); } const EdgeIterator edgesBegin() const { return mEdges.begin(); } const EdgeIterator edgesEnd() const { return mEdges.end(); } Vertex addVertex(const Point& p) { Vertex v = {static_cast(mVertices.size()), p}; mVertices.push_back(v); return v; } void addEdge(int v1, int v2) { if (!hasVertex(v1) || !hasVertex(v2)) { throw std::logic_error("vertex does not exist!"); } std::pair p = v1 < v2 ? std::make_pair(v1, v2) : std::make_pair(v2, v1); mEdges.insert(std::move(p)); } private: std::vector mVertices; std::set> mEdges; // contains indices of elements in vertices vector }; } } // namespace navigine::navigation_core #endif // NAVIGINE_GRAPH_H ================================================ FILE: include/navigine/navigation-core/graph_particle.h ================================================ /** graph_particle.h * * Copyright (c) 2017 Navigine. * */ #ifndef NAVIGINE_GRAPH_PARTICLE_H #define NAVIGINE_GRAPH_PARTICLE_H #include namespace navigine { namespace navigation_core { struct GraphParticle : XYParticle { GraphParticle() : edgeId(0) , fromVertex(0) , toVertex(0) , percent(0) { } GraphParticle(int _edge, int _fromVertex, int _toVertex, double _percent, double _x, double _y, double _angle) : XYParticle(_x, _y, _angle) , edgeId(_edge) , fromVertex(_fromVertex) , toVertex(_toVertex) , percent(_percent) { } int edgeId; int fromVertex; int toVertex; double percent; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_GRAPH_PARTICLE_H ================================================ FILE: include/navigine/navigation-core/level.h ================================================ /** level.h * * Copyright (c) 2019 Navigine. * */ #ifndef NAVIGINE_LEVEL_H #define NAVIGINE_LEVEL_H #include #include #include #include #include "boost_geometry_adaptation.h" #include "level_geometry.h" #include "graph.h" #include "radiomap.h" #include "reference_point.h" #include "transmitter.h" #include "navigation_input.h" namespace navigine { namespace navigation_core { DECLARE_IDENTIFIER(LevelId) class Level { public: Level(const LevelId& id, const GeoPoint& bindingPoint) : mId (id) , mBindingPoint (bindingPoint) {} void addTransmitters(const XYZTransmitters& transmitters); void setReferencePoints(const XYReferencePoints& referencePoints); void setGraph(const Graph& graph); void setGeometry(const LevelGeometry &levelGeometry); const LevelId& id() const; const GeoPoint& bindingPoint() const; bool containsTransmitter(const TransmitterId& txId) const; const Transmitter& transmitter(const TransmitterId& txId) const; const Radiomap& radiomap() const; const Graph& graph() const; const LevelGeometry& geometry() const; private: const LevelId mId; const GeoPoint mBindingPoint; std::unordered_map, HasherTransmitterId> mTransmitters; Radiomap mRadiomap; LevelGeometry mGeometry; Graph mGraph; }; typedef std::vector >::const_iterator LevelIterator; typedef std::vector > Levels; RadioMeasurementsData getLevelRadioMeasurements( const Level& level, const RadioMeasurementsData& radioMsr); } } // namespace navigine::navigation_core #endif // NAVIGINE_LEVEL_H ================================================ FILE: include/navigine/navigation-core/level_collector.h ================================================ /** level_collector.h * * Copyright (c) 2019 Navigine. * */ #ifndef NAVIGINE_LEVEL_COLLECTOR_H #define NAVIGINE_LEVEL_COLLECTOR_H #include #include #include #include #include #include "boost_geometry_adaptation.h" #include "graph.h" #include #include "geolevel.h" #include "reference_point.h" #include "transmitter.h" namespace navigine { namespace navigation_core { class LevelCollector { public: LevelCollector() {} bool hasLevel(const LevelId& levelId) const; const Level& level(const LevelId& levelId) const; void addGeoLevel(const GeoLevel& geoLevel); const std::vector& levels() const; boost::optional findLevelByTransmitterId(const TransmitterId& transmitterId) const; private: std::vector mLevels; std::unordered_map mLevelsIndices; }; std::shared_ptr createLevelCollector(); } } // namespace navigine::navigation_core #endif // NAVIGINE_LEVEL_COLLECTOR_H ================================================ FILE: include/navigine/navigation-core/level_geometry.h ================================================ /** level.h * * Copyright (c) 2019 Navigine. * */ #ifndef NAVIGINE_LEVEL_GEOMETRY_H #define NAVIGINE_LEVEL_GEOMETRY_H #include "boost_geometry_adaptation.h" #include namespace navigine { namespace navigation_core { class LevelGeometry { public: LevelGeometry() {}; LevelGeometry(const Multipolygon& allowedArea, const Box& box); bool isPathTouchesBarriers(const Linestring& path) const; const Multipolygon& allowedArea() const; const Box& boundingBox() const; private: Multipolygon mAllowedArea; Box mBoundingBox; RTree mTree; std::vector mInnerBarriers; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_LEVEL_GEOMETRY_H ================================================ FILE: include/navigine/navigation-core/location_point.h ================================================ /** location_point.h * * Copyright (c) 2018 Navigine. * */ #ifndef NAVIGINE_LOCATION_POINT_H #define NAVIGINE_LOCATION_POINT_H #include #include namespace navigine { namespace navigation_core { struct LocationPoint { LevelId level; double x; double y; }; typedef std::vector PolyLine; } } // namespace navigine::navigation_core #endif ================================================ FILE: include/navigine/navigation-core/logger.h ================================================ /** logger.h * * Copyright (c) 2019 Navigine. * */ #ifndef NAVIGINE_LOGGER_H #define NAVIGINE_LOGGER_H #include #include #define LOG_MESSAGE(lvl, fmt, ...) \ do { \ navigine::navigation_core::_Logger.print(lvl, __FILE__, __LINE__, fmt, ##__VA_ARGS__); \ } while (false) namespace navigine { namespace navigation_core { class Logger { public: Logger() = default; // Function is used for specifying a log file. // If @file parameter is null, function sets a default log file. // Otherwise, function sets a separate log file for the corresponding module. void setLogFile(const std::string& logFile, const char* file = 0); // Function is used for specifying a debug level. // If @file parameter is null, function sets a default debug level. // Otherwise, function sets a separate debug level for the corresponding module. void setDebugLevel(int level, const char* file = 0); void printDebugLevels(FILE* fp = stdout); void printLogFiles(FILE* fp = stdout); void setOutStream(FILE* os); // Function prints a format message in printf-like way. // @level - message debug level. If @level is greater than the debug level // specified earlier, the message will be ignored. // @file - module name, from where Logger has been called. // @line - line number where Logger has been called. // @fmt - message format string void print(int level, const char* file, int line, const char* fmt, ...) const; private: FILE * mOutStream = stdout; std::map mDebugLevels = {}; std::map mLogFiles = {}; }; extern Logger _Logger; extern int GetLocalTime (int unixTime, int* year, int* month, int* day, int* hour, int* minute, int* second); extern long long GetCurrentTimeMillis(); } } // namespace navigine::navigation_core #endif ================================================ FILE: include/navigine/navigation-core/motion_info.h ================================================ /** motion_info.h * * Copyright (c) 2017 Navigine. * */ #ifndef NAVIGINE_MOTION_INFO_H #define NAVIGINE_MOTION_INFO_H #include namespace navigine { namespace navigation_core { struct MotionInfo { MotionInfo() : isStepDetected ( false ) , dtSec ( 0.0 ) , deltaAngle ( 0.0 ) , distance ( 0.0 ) , stepCounter ( 0 ) , heading ( 0.0 ) , azimuth ( 0.0 ) , lastMotionTs ( 0 ) , isAzimuthValid (false) , gyroHeading ( 0.0 ) {} bool isStepDetected; double dtSec; double deltaAngle; double distance; size_t stepCounter; double heading; double azimuth; long long lastMotionTs; bool isAzimuthValid; double gyroHeading; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_MOTION_INFO_H ================================================ FILE: include/navigine/navigation-core/navigation_client.h ================================================ #pragma once #include "navigation_input.h" #include "navigation_output.h" #include "navigation_state.h" #include "navigation_settings.h" #include "level_collector.h" namespace navigine { namespace navigation_core { class NavigationClient { public: // Return navigation state of this client virtual std::vector getStates() const = 0; // Main navigation function. Calculates current position based on incoming measurements virtual std::vector navigate(const std::vector& navInput) = 0; virtual ~NavigationClient() {} }; std::shared_ptr createNavigationClient( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); } } // namespace navigine::navigation_core ================================================ FILE: include/navigine/navigation-core/navigation_input.h ================================================ /** navigation_input.h * * Copyright (c) 2018 Navigine. * */ #ifndef NAVIGINE_NAVIGATION_INPUT_H #define NAVIGINE_NAVIGATION_INPUT_H #define _CRTDBG_MAP_ALLOC #include #include #include "boost/variant.hpp" #include "vector3d.h" #include "transmitter.h" namespace navigine { namespace navigation_core { template struct MeasurementDataWithTimeStamp { MeasurementData data; long long ts; }; struct RadioMeasurementData { enum class Type { WIFI, BEACON, BLUETOOTH, EDDYSTONE, WIFI_RTT }; Type type; TransmitterId id; double rssi = 0; double power = 0; double distance = 0; double stddev = 0; }; struct SensorMeasurementData { enum class Type { ACCELEROMETER, MAGNETOMETER, GYROSCOPE, BAROMETER, LOCATION, ORIENTATION }; Type type; Vector3d values; }; struct NmeaMeasurementData { int sentenceNumber; int satellitesNumber; }; using RadioMeasurementsData = std::vector; using MeasurementData = boost::variant; using Measurement = MeasurementDataWithTimeStamp; } } // namespace navigine::navigation_core #endif ================================================ FILE: include/navigine/navigation-core/navigation_output.h ================================================ #ifndef NAVIGINE_NAVIGATION_OUTPUT_H #define NAVIGINE_NAVIGATION_OUTPUT_H #include #include #include namespace navigine { namespace navigation_core { enum NavigationStatus : int { OK = 0, NAVIGATION_ERROR = -1, NO_SOLUTION = 4, NO_RPS = 7, NO_LEVEL = 30, PF_MUTATION_FAILED = 32, PF_SAMPLING_FAILED = 33, }; enum class Provider { GNSS, INDOOR, FUSED, NONE }; struct NavigationOutput { NavigationStatus status; LevelId posLevel; // Level identifier double posLatitude = 0.0; double posLongitude = 0.0; double posAltitude = 0.0; double posR; // Output radius (meters) double posOrientation; // Output orientation in radians (clockwise) Provider provider; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_NAVIGATION_OUTPUT_H ================================================ FILE: include/navigine/navigation-core/navigation_settings.h ================================================ #ifndef NAVIGINE_NAVIGATION_SETTINGS_H #define NAVIGINE_NAVIGATION_SETTINGS_H #include #include #include #include #include namespace navigine { namespace navigation_core { struct CommonSettings { enum class UseAlgorithm { PF_PDR, PF_NOPDR, ZONES, GRAPH, KNN }; //PF_PDR, PF_NOPDR, ZONES, GRAPH, KNN enum class MeasurementType { RSSI, DISTANCE_V1, DISTANCE_V2 }; // rssi, distance_21, distance_v2 enum class SignalsToUse { BLE, WIFI, BOTH }; //ble, wifi, both UseAlgorithm useAlgorithm = UseAlgorithm::PF_PDR; // "Use_algorithm" double lklCorrectionNoise = 10.0; // "Correction_noise" bool useEnuAzimuth = false; // "Use_enu_azimuth" bool useDifferentionMode = false; // "Differention_mode" bool useTxPower = false; // "Use_beacon_power" bool useRadiomap = false; // "Use_radiomap" bool useStops = false; // "Use_stops" bool useTracking = false; // "Use_tracking" double stopDetectionTime = 3.0; // "Stop_detection_time" double minDeviationM = 1.0; // "Min_deviation_m" double averageMovSpeed = 1.0; // "Average_speed" MeasurementType measurementType = MeasurementType::RSSI; // "Measurement_type" bool useBestRefPointLevel = false; // "Use_best_ref_point_level" bool useBarometer = false; // "Use_barometer" bool useUnknownTransmitters = false; // "Use_unknown_beacons" double sigCutOffRss = -100; // "Cutoff_rss" bool useClosestAps = false; // "Use_closest_aps" unsigned int numClosestAps = 1; // "N_closest_aps" double sigAveragingTime = 1.0; // "Sig_averaging_time" double sigWindowShift = 0.0; // "Sig_window_shift" SignalsToUse signalsToUse = SignalsToUse::BLE; // "Use_Signals" unsigned int numParticles = 1000; // "N_particles" unsigned int minMsrNumForMutation = 3; // "Min_msr_num_for_mutation" double resamplingThreshold = 600.0; // "Resampling_threshold_num", Level unsigned int minNumParticlesAlive = 100; // "Min_num_of_particles", Level double gyroNoiseDegree = 4.0; // "Gyro_noise_degree" double stepNoiseDeviationMeters = 0.4; // "Step_noise_deviation_meters" bool useProjectionToWhiteArea = false; // "Use_projection_to_white_area" unsigned int numParticlesAroundClosestAp = 10; // "N_particles_around_closest_AP" unsigned int numParticlesToMutate = 50; // "N_particles_to_mutate" unsigned int numSamplingAttempts = 60; // "Num_sampling_attempts" unsigned int numInitializationAttempts = 500; // "Num_initialization_attempts" bool useUniformSampling = false; // "Use_uniform_sampling" unsigned int kNeighbors = 8; // "K_neighbors" bool useTriangles = true; // "Use_triangles" bool useDiffMode = true; // "Use_diff_mode" unsigned int minNumOfMeasurementsForPositionCalculation = 4; // "Min_num_of_measurements_for_position_calculation" double smoothing = 0.5; // "Smoothing" double deadReckoningTime = 5.0; // "Dead_reckon_time" double stopUpdateTime = 4.0; // "Stop_update_time" double graphProjDist = 5.0; // "Graph_prog_distance" double useStopsDistanceThresholdM = 10.0; // "Stopped_distance_threshold_m" // if distance between stopped position and current position // is more than the threshold - update stopped position bool useSmoothing = true; // "Use_smoothing" bool useGraphProjection = false; // "Use_graph_projection" bool useAccuracyForSmoothing = false; // "Use_accuracy_for_smoothing" bool useSpeedConstraint = true; // "Use_speed_constraint" bool useBarriers = false; // "Use_barriers" bool useTimeSmoothing = false; // "Use_time_smoothing" bool useAbFilter = true; // "Use_ab_filter" bool useAltitude = false; // "Use_altitude" bool useGps = false; // "Use_gps" bool useInstantGpsPosition = false; // "Use_instant_gps_position" bool preferIndoorSolution = false; // "Prefer_indoor_solution" bool fuseGps = false; // "Fuse_gps" bool useGpsOutsideMap = false; // "Use_gps_outside_map" bool useGpsSigmaFilter = false; // "Use_gps_sigma_filter" unsigned int sigmaFilterWindow = 1; // "Sigma_filter_window" double sigmaFilterDelta = 0.0; // "Sigma_filter_delta" unsigned int minNumSats = 20; // "Min_num_sats" double priorDeviation = 50.0; // "Prior_deviation" double minGpsDeviation = 5.0; // "Gps_deviation" double maxGpsDeviation = std::numeric_limits::infinity(); // Maximum_to_accept_gps_measurement" double fuseGpsBorderM = 10; // "Fuse_gps_border" long long noSignalTimeThreshold = 35; // "No_signal_time_threshold" long long noActionTimeThreshold = 20; // "No_action_time_threshold" double positionIsTooOldSec = 20.0; // "Position_old_for_fusing_sec" double gpsValidTimeWindowSec = 1.0; // "Gps_valid_time_window_sec" bool useCalculatedAzimuth = false; // "Use_calculated_azimuth" double deviceAzimuthLifetimeSeconds = 10.0; // "Device_azimuth_lifetime_seconds" double stepMultiplier = 1.0; // "Step_multiplier" bool hasAccelerometer = true; // "Has_accelerometer" bool hasGyroscope = true; // "Has_gyroscope" double wiFiRttOffset = -1.5; // "WiFi_RTT_Offset" }; struct LevelSettings { double normalModeA = -82.0; // "GP_Normal_Mode", "A", Level double normalModeB = 3.0; // "GP_Normal_Mode", "B", Level }; struct NavigationSettings { CommonSettings commonSettings; std::map levelsSettings; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_NAVIGATION_SETTINGS_H ================================================ FILE: include/navigine/navigation-core/navigation_state.h ================================================ /** navigation_state.h * * Copyright (c) 2017 Navigine. * */ #ifndef NAVIGINE_NAVIGATION_STATE_H #define NAVIGINE_NAVIGATION_STATE_H #include #include "navigation_input.h" #include #include "xy_particle.h" namespace navigine { namespace navigation_core { class NavigationState { public: NavigationState() { } NavigationState(const std::vector& particles, const std::vector& weights, const RadioMeasurementsData& radioMsr, const std::vector& likelihoodPoints, const std::vector& likelihoodWeights, double heading = 0, double stepLen = 0, int stepCounter = 0) : mParticles ( particles ) , mParticleWeights ( weights ) , mMeasurements ( radioMsr ) , mLikelihoodMapPoints ( likelihoodPoints ) , mLikelihoodMapWeights ( likelihoodWeights ) , mHeading ( heading ) , mStepLen ( stepLen ) , mStepCounter ( stepCounter ) { } void setStepCounter (double stepCounter) { mStepCounter = stepCounter; } void setStepLen (double stepLen) { mStepLen = stepLen; } void setIndoorPosition (const XYPoint& pos) { mIndoorPosition = pos; } void setOutdoorPosition (const XYPoint& pos) { mOutdoorPosition = pos; } XYPoint getIndoorPosition ( void ) const { return mIndoorPosition; } XYPoint getOutdoorPosition ( void ) const { return mOutdoorPosition; } int getStepCounter ( void ) const { return mStepCounter; } double getHeading ( void ) const { return mHeading; } double getStepLen ( void ) const { return mStepLen; } std::vector getParticles ( void ) const { return mParticles; } std::vector getParticleWeights ( void ) const { return mParticleWeights; } RadioMeasurementsData getLevelRadioMeasurements ( void ) const { return mMeasurements; } std::vector getLikelihoodMapPoints ( void ) const { return mLikelihoodMapPoints; } std::vector getLikelihoodMapWeights ( void ) const { return mLikelihoodMapWeights; } private: std::vector mParticles; std::vector mParticleWeights; RadioMeasurementsData mMeasurements; std::vector mLikelihoodMapPoints; std::vector mLikelihoodMapWeights; double mHeading; double mStepLen; int mStepCounter; XYPoint mIndoorPosition; XYPoint mOutdoorPosition; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_NAVIGATION_STATE_H ================================================ FILE: include/navigine/navigation-core/point.h ================================================ /** point.h * * Copyright (c) 2017 Navigine. * */ #ifndef NAVIGINE_POINT_H #define NAVIGINE_POINT_H namespace navigine { namespace navigation_core { struct GeoPoint { GeoPoint() : latitude ( 0.0 ) , longitude( 0.0 ) { } GeoPoint(double _latitude, double _longitude) : latitude ( _latitude ) , longitude( _longitude ) { } double latitude; double longitude; }; struct GeoPoint3D { GeoPoint3D() : latitude ( 0.0 ) , longitude( 0.0 ) , altitude ( 0.0 ) { } GeoPoint3D(double _latitude, double _longitude, double _altitude) : latitude ( _latitude ) , longitude( _longitude ) , altitude ( _altitude ) { } double latitude; double longitude; double altitude; }; struct XYPoint { XYPoint() : x( 0.0 ) , y( 0.0 ) { } XYPoint(double _x, double _y) : x( _x ) , y( _y ) { } double x; double y; }; struct XYZPoint { XYZPoint() : x( 0.0 ) , y( 0.0 ) , z( 0.0 ) { } XYZPoint(double _x, double _y, double _z) : x( _x ) , y( _y ) , z( _z ) { } double x; double y; double z; }; XYPoint gpsToLocal(const GeoPoint& point, const GeoPoint& bindPoint); GeoPoint localToGps(const XYPoint& localPoint, const GeoPoint& bindPoint); } } // namespace navigine::navigation_core #endif // NAVIGINE_POINT ================================================ FILE: include/navigine/navigation-core/radiomap.h ================================================ /** radiomap.h * * Copyright (c) 2019 Navigine. * */ #ifndef NAVIGINE_RADIOMAP_H #define NAVIGINE_RADIOMAP_H #include #include "reference_point.h" #include "transmitter.h" namespace navigine { namespace navigation_core { class Radiomap { public: Radiomap(const XYReferencePoints& referencePoints = {}); const XYReferencePoints& referencePoints() const; ReferencePoint getReferencePoint(const ReferencePointId& refPointId) const; bool hasReferencePoint(const ReferencePointId& refPointId) const; private: XYReferencePoints mReferencePoints; std::map mReferencePointsIndex; // {id -> index} }; } } // namespace navigine::navigation_core #endif // NAVIGINE_RADIOMAP_H ================================================ FILE: include/navigine/navigation-core/reference_point.h ================================================ /** reference_point.h * * Copyright (c) 2017 Navigine. * */ #ifndef NAVIGINE_REFERENCE_POINT_H #define NAVIGINE_REFERENCE_POINT_H #include #include #include #include "transmitter.h" namespace navigine { namespace navigation_core { struct SignalStatistics { TransmitterType type; double mean; double variance; size_t nMeasurements; void addMeasurement(double meas); }; DECLARE_IDENTIFIER(ReferencePointId) template struct ReferencePoint { ReferencePoint( ReferencePointId _id, Point _point, const std::map& _fingerprints) : id(_id) , point(_point) , fingerprints(_fingerprints) {} ReferencePointId id; Point point; std::map fingerprints; }; typedef std::vector> XYReferencePoints; typedef std::vector> GeoReferencePoints; } } // namespace navigine::navigation_core #endif // NAVIGINE_REFERENCE_POINT_H ================================================ FILE: include/navigine/navigation-core/transmitter.h ================================================ #pragma once #include #include #include #include "declare_identifier.h" #include namespace navigine { namespace navigation_core { struct PathlossModel { double A; double B; double power; inline double operator()(double r) const { static constexpr double MIN_DISTANCE = 0.01; return A - B * std::log(std::max(r, MIN_DISTANCE)); } }; enum class TransmitterType { WIFI, BEACON, BLUETOOTH, UNKNOWN, EDDYSTONE, LOCATOR }; DECLARE_IDENTIFIER(TransmitterId) template struct Transmitter { Transmitter() = default; Transmitter(TransmitterId _id, Point _p, PathlossModel _model, TransmitterType _type) : id ( _id ) , point ( _p ) , pathlossModel ( _model ) , type ( _type ) { } TransmitterId id; Point point; PathlossModel pathlossModel; TransmitterType type; bool operator== (const Transmitter& tx)const { return id == tx.id; } bool operator< (const Transmitter& tx)const { return id < tx.id; } }; typedef std::vector> XYZTransmitters; typedef std::vector> Geo3DTransmitters; } } // namespace navigine::navigation_core ================================================ FILE: include/navigine/navigation-core/vector3d.h ================================================ /** vector3d.h * ** Copyv2 (c) 2018 Navigine. All v2s reserved. * */ #ifndef NAVIGINE_VECTOR3D_H #define NAVIGINE_VECTOR3D_H #include namespace navigine { namespace navigation_core { struct Vector3d { double x; double y; double z; Vector3d(): x(0.0), y(0.0), z(0.0) { } Vector3d(double _x, double _y, double _z); double magnitude() const; Vector3d normalized() const; Vector3d& normalize(); bool isNull() const; static Vector3d crossProduct(const Vector3d& v1, const Vector3d& v2); static double dotProduct (const Vector3d& v1, const Vector3d& v2); bool operator==(const Vector3d& v2); bool operator!=(const Vector3d& v2); Vector3d& operator+=(const Vector3d& v2); Vector3d& operator-=(const Vector3d& v2); Vector3d& operator*=(double multiplier); Vector3d& operator/=(double divisor); }; inline Vector3d operator*(double multiplier, const Vector3d& v) { return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier); } inline Vector3d operator*(const Vector3d& v, double multiplier) { return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier); } inline Vector3d operator+(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z); } inline Vector3d operator-(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); } inline Vector3d operator-(const Vector3d& v) { return Vector3d(-v.x, -v.y, -v.z); } inline Vector3d operator/(const Vector3d& v1, double divisor) { return Vector3d(v1.x / divisor, v1.y / divisor, v1.z / divisor); } } } // namespace navigine::navigation_core #endif // NAVIGINE_VECTOR3D_H ================================================ FILE: include/navigine/navigation-core/version.h ================================================ #ifndef NAVIGINE_VERSION_H #define NAVIGINE_VERSION_H #define NAVIGATION_LIBRARY_VERSION "2.0" // 09 July 2019 #endif ================================================ FILE: include/navigine/navigation-core/xy_particle.h ================================================ /** xy_particle.h * * Copyright (c) 2017 Navigine. * */ #ifndef NAVIGINE_PARTICLE_H #define NAVIGINE_PARTICLE_H #include namespace navigine { namespace navigation_core { struct XYParticle : XYPoint { XYParticle() : XYPoint ( 0.0, 0.0 ) , angle ( 0.0 ) { } XYParticle(double _x, double _y, double _angle) : XYPoint( _x, _y ) , angle ( _angle ) { } double angle; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_PARTICLE ================================================ FILE: src/README ================================================ the implementations of navigation algorithms developed by Navigine ================================================ FILE: src/barriers_geometry_builder.cpp ================================================ /** barriers_geometry_builder.cpp * * Copyright (c) 2019 Navigine. * */ #include #include namespace navigine { namespace navigation_core { //TODO test for empty barrier list LevelGeometry getGeometry(const std::list& allowedArea) { Multipolygon allowedFullArea; for (const auto& area: allowedArea) { Polygon correctedArea = area; boost::geometry::correct(correctedArea); allowedFullArea.push_back(correctedArea); } return {allowedFullArea, boost::geometry::return_envelope(allowedFullArea)}; } } } // namespace navigine::navigation_core ================================================ FILE: src/device_properties.h ================================================ #ifndef NAVIGINE_DEVICE_PROPERTIES_H #define NAVIGINE_DEVICE_PROPERTIES_H #include namespace navigine { namespace navigation_core { class DeviceProperties { public: DeviceProperties() { } DeviceProperties(const std::string& _id, const std::string& _model, bool _hasAcc = false, bool _hasGyro = false, bool _hasMagn = false, bool _hasBaro = false) : mId ( _id ) , mModel ( _model ) , mHasAccel ( _hasAcc ) , mHasGyro ( _hasGyro ) , mHasMagn ( _hasMagn ) , mHasBaro ( _hasBaro ) { } public: std::string getId ( void ) const { return mId; } std::string getModel ( void ) const { return mModel; } bool hasAccel ( void ) const { return mHasAccel; } bool hasGyro ( void ) const { return mHasGyro; } bool hasMagn ( void ) const { return mHasMagn; } bool hasBaro ( void ) const { return mHasBaro; } private: std::string mId = ""; // Client ID std::string mModel = ""; // Telephone model bool mHasAccel = false; bool mHasGyro = false; bool mHasMagn = false; bool mHasBaro = false; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_DEVICE_PROPERTIES_H ================================================ FILE: src/geometry.cpp ================================================ #include #include "geometry.h" namespace navigine { namespace navigation_core { // Get distance from A to B double GetDist(double ax, double ay, double bx, double by) { return std::sqrt((ax - bx) * (ax - bx) + (ay - by) * (ay - by)); } // Get distance from A to B double GetDist(const LocationPoint& A, const LocationPoint& B) { return A.level == B.level ? GetDist(A.x, A.y, B.x, B.y) : NAN; } // Get distance from segment to point double SegmentPointDist(const XYPoint& p1, const XYPoint& p2, const XYPoint& p) { double v1x = p.x - p1.x; double v1y = p.y - p1.y; double v2x = p2.x - p1.x; double v2y = p2.y - p1.y; double len = v2x * v2x + v2y * v2y; if (len < GEOMETRY_DOUBLE_EPSILON) return GetDist(p1.x, p1.y, p.x, p.y); double t = (v1x * v2x + v1y * v2y) / len; t = std::min(std::max(t, 0.0), 1.0); double sx = p1.x + t * (p2.x - p1.x); double sy = p1.y + t * (p2.y - p1.y); double dist = std::sqrt((sx - p.x) * (sx - p.x) + (sy - p.y) * (sy - p.y)); return dist; } // Get distance from C to line (AB) double GetDist(const LocationPoint& A, const LocationPoint& B, const LocationPoint& C) { if (A.level != B.level || A.level != C.level) return NAN; double d = GetDist(A, B); return d >= GEOMETRY_DOUBLE_EPSILON ? std::fabs((B.x - A.x) * (C.y - A.y) - (C.x - A.x) * (B.y - A.y)) / d : NAN; } // Calculate the oriented area of triangle (ABC). extern double GetArea(const LocationPoint& A, const LocationPoint& B, const LocationPoint& C) { if (A.level != B.level || A.level != C.level) return NAN; return GetArea(A.x, A.y, B.x, B.y, C.x, C.y); } // Calculate the oriented area of triangle (ABC). extern double GetArea(double ax, double ay, double bx, double by, double cx, double cy) { return Determinant(ax - bx, ay - by, ax - cx, ay - cy) / 2; } // Calculate determinant of the 2x2 matrix: // a11 a12 // a21 a22 double Determinant(double a11, double a12, double a21, double a22) { return a11 * a22 - a12 * a21; } // Calculate determinant of the 3x3 matrix: // a11 a12 a13 // a21 a22 a23 // a31 a32 a33 double Determinant(double a11, double a12, double a13, double a21, double a22, double a23, double a31, double a32, double a33) { return (a11 * a22 * a33 + a12 * a23 * a31 + a21 * a32 * a13) - (a21 * a12 * a33 + a11 * a23 * a32 + a13 * a22 * a31); } // Get projection from (ox,oy) to segment [(ax,ay), (bx,by)]. double GetProjection(double ax, double ay, double bx, double by, double ox, double oy, double* px, double* py) { // Find such point P, that vector OP is orthogonal to the vector AB // Decompose vector OP for basis (OA, OB): // OP = (1-k) * OA + k * OB // Return value: k double ab = ((bx - ax) * (bx - ax) + (by - ay) * (by - ay)); double ao = ((ox - ax) * (bx - ax) + (oy - ay) * (by - ay)); double k = ab > GEOMETRY_DOUBLE_EPSILON ? ao / ab : 0.0; if (px) { *px = ax + k * (bx - ax); } if (py) { *py = ay + k * (by - ay); } return k; } double GetIntersection(double px, double py, double qx, double qy, double ax, double ay, double bx, double by, double* ox, double* oy) { double pqx = qx - px; double pqy = qy - py; double abx = bx - ax; double aby = by - ay; double pax = ax - px; double pay = ay - py; double det = Determinant(pqx, abx, pqy, aby); if (std::fabs(det) < GEOMETRY_DOUBLE_EPSILON) return NAN; double k = Determinant(pax, abx, pay, aby) / det; double l = -Determinant(pqx, pax, pqy, pay) / det; if (k < 0 || k > 1.0 || l < 0 || l > 1.0) return NAN; if (ox) { *ox = px + k * pqx; } if (oy) { *oy = py + k * pqy; } return k; } // Check if segments [AB] and [CD] has intersection bool CheckIntersection(double ax, double ay, double bx, double by, double cx, double cy, double dx, double dy) { auto boundBoxCheck = [](double a, double b, double c, double d) -> bool { if (a > b) std::swap(a, b); if (c > d) std::swap(c, d); return std::max(a,c) < std::min(b,d); }; return boundBoxCheck(ax, bx, cx, dx) && boundBoxCheck(ay, by, cy, dy) && GetArea(ax, ay, bx, by, cx, cy) * GetArea(ax, ay, bx, by, dx, dy) < 0 && GetArea(cx, cy, dx, dy, ax, ay) * GetArea(cx, cy, dx, dy, bx, by) < 0; } bool PointOnLine(double x, double y, double x1, double y1, double x2, double y2) { const double Ax = x1 - x; const double Ay = y1 - y; const double Bx = x2 - x; const double By = y2 - y; return (std::fabs(Ax * By - Ay * Bx) < GEOMETRY_DOUBLE_EPSILON && Ax * Bx + Ay * By < GEOMETRY_DOUBLE_EPSILON); } bool XRayIntersectsLine(double x, double y, double x1, double y1, double x2, double y2) { static const double DELTA = std::sqrt(GEOMETRY_DOUBLE_EPSILON); if (std::fabs(y - y1) < GEOMETRY_DOUBLE_EPSILON) y1 += DELTA; if (std::fabs(y - y2) < GEOMETRY_DOUBLE_EPSILON) y2 += DELTA; if (y1 < y && y < y2) return Determinant(x2, y2, 1, x, y, 1, x1, y1, 1) + GEOMETRY_DOUBLE_EPSILON > 0; if (y2 < y && y < y1) return Determinant(x1, y1, 1, x, y, 1, x2, y2, 1) + GEOMETRY_DOUBLE_EPSILON > 0; return false; } } } // namespace navigine::navigation_core ================================================ FILE: src/geometry.h ================================================ /** geometry.h * * Copyright (c) 2018 Navigine. * */ #ifndef NAVIGINE_GEOMETRY_H #define NAVIGINE_GEOMETRY_H #include namespace navigine { namespace navigation_core { static const double GEOMETRY_DOUBLE_EPSILON = 1e-8; // Get distance from segment to point extern double SegmentPointDist(const XYPoint &p1, const XYPoint &p2, const XYPoint &p); // Get distance from A to B extern double GetDist(const LocationPoint& A, const LocationPoint& B); extern double GetDist(double ax, double ay, double bx, double by); // Get distance from C to line (AB) extern double GetDist(const LocationPoint& A, const LocationPoint& B, const LocationPoint& C); // Calculate the oriented area of triangle (ABC). extern double GetArea(const LocationPoint& A, const LocationPoint& B, const LocationPoint& C); // Calculate the oriented area of triangle (ABC). extern double GetArea(double ax, double ay, double bx, double by, double cx, double cy); // Calculate determinant of the 2x2 matrix: // a11 a12 // a21 a22 extern double Determinant(double a11, double a12, double a21, double a22); // Calculate determinant of the 3x3 matrix: // a11 a12 a13 // a21 a22 a23 // a31 a32 a33 extern double Determinant(double a11, double a12, double a13, double a21, double a22, double a23, double a31, double a32, double a33); // Get distance from O to segment [AB]. // Px - is the closest point on [AB]. extern double GetProjection(double ax, double ay, double bx, double by, double ox, double oy, double* px, double* py); // Get intersection of vector PQ and segment [AB]. extern double GetIntersection(double px, double py, double qx, double qy, double ax, double ay, double bx, double by, double* x, double* y); // Check if segments [AB] and [CD] has intersection extern bool CheckIntersection(double ax, double ay, double bx, double by, double cx, double cy, double dx, double dy); extern bool PointOnLine ( double x, double y, double x1, double y1, double x2, double y2 ); extern bool XRayIntersectsLine ( double x, double y, double x1, double y1, double x2, double y2 ); } } // namespace navigine::navigation_core #endif ================================================ FILE: src/knn/knn_utils.cpp ================================================ /** knn_utils.cpp * * Copyright (c) 2018 Navigine. * */ #include #include #include #include "knn_utils.h" namespace navigine { namespace navigation_core { namespace { static constexpr double MIN_WEIGHT_EPSILON = 1e-8; static constexpr bool USE_AP_WEIGHTS = true; static constexpr bool USE_TRIANGULATION = true; static constexpr int MIN_NUM_VISIBLE_BS_IN_RP = 3; // should be more than 0!!! static constexpr double RSSI_ON_METER = -10.0; static constexpr double MIN_RP_WEIGHT = 1e-6; // we skip RPs with 0 weights because they are most improbable, // but if all signals coincide to map, we should add small additive static constexpr double GAMMA = 3.0; static constexpr double G_WEIGHT_COEF = 1.8; // Coef to find weight of AP (See Accuracy of RSS-Based Centroid Localization // Algorithms in an Indoor Environment } //namespace std::map> triangulate(const std::shared_ptr& levelCollector) { std::map> levelTriangles; #ifdef DEBUG_OUTPUT_TRIANGULATION std::ofstream debugOs; std::string fileName = DEBUG_OUTPUT_TRIANGULATION; debugOs.open(fileName); debugOs << "level ax ay bx by cx cy " << std::endl; debugOs.close(); #endif for (const Level& level : levelCollector->levels()) { const XYReferencePoints& referencePoints = level.radiomap().referencePoints(); if (referencePoints.size() > 3) { std::vector vertices; for(const ReferencePoint& rp: referencePoints) { TriangleVertex v = TriangleVertex(rp.point.x, rp.point.y, rp.id); vertices.push_back(v); } auto widthHeightPair = getWidthHeightPair(level.geometry().boundingBox()); double maxedge = std::max(widthHeightPair.first, widthHeightPair.second); std::vector triangles = TriangulateVertices(vertices, maxedge); if (triangles.empty()) { // triangulation failed! return levelTriangles; } #ifdef DEBUG_OUTPUT_TRIANGULATION std::ofstream debugOs; std::string fileName = DEBUG_OUTPUT_TRIANGULATION; debugOs.open(fileName, std::ios_base::app); #endif std::vector refPointsTriangles; for (auto it = triangles.begin(); it != triangles.end(); it++) { #ifdef DEBUG_OUTPUT_TRIANGULATION Triangle& t = *it; debugOs << level.id << " " << t.getA().x << " " << t.getA().y << " " << t.getB().x << " " << t.getB().y << " " << t.getC().x << " " << t.getC().y << std::endl; #endif RefPointsTriangle rpTriangle = {it->getA().id, it->getB().id, it->getC().id}; refPointsTriangles.push_back(rpTriangle); } #ifdef DEBUG_OUTPUT_TRIANGULATION debugOs.close(); #endif levelTriangles.insert(std::make_pair(level.id(), refPointsTriangles)); } } return levelTriangles; } std::map>> getLevelsRadiomaps(const std::shared_ptr &levelCollector) { std::map>> levelsRadiomaps; for (const Level& level : levelCollector->levels()) { std::multimap> bssidRpToSigstatMap; const auto& levelId = level.id(); for (const ReferencePoint& rp : level.radiomap().referencePoints()) { const std::map& rpFingerprint = rp.fingerprints; for (const auto& bssidSigStatPair: rpFingerprint) { bssidRpToSigstatMap.insert({bssidSigStatPair.first, std::make_pair(rp.id, bssidSigStatPair.second)}); } } levelsRadiomaps[levelId] = bssidRpToSigstatMap; } return levelsRadiomaps; } std::map> getLevelReferenceTransmittersMap(const std::shared_ptr &levelCollector) { std::map> levelTransmitters; for (const auto& level : levelCollector->levels()) { const LevelId levelId = level.id(); for (const ReferencePoint& rp : level.radiomap().referencePoints()) { const std::map& rpFingerprint = rp.fingerprints; std::set rpTransmittersIds; std::transform(rpFingerprint.begin(), rpFingerprint.end(), std::inserter(rpTransmittersIds, rpTransmittersIds.end()), [](const auto& p){ return p.first; }); if (rpTransmittersIds.empty()) continue; if (levelTransmitters.find(levelId) == levelTransmitters.end()) levelTransmitters.insert({levelId, rpTransmittersIds}); else levelTransmitters[levelId].insert(rpTransmittersIds.begin(), rpTransmittersIds.end()); } } return levelTransmitters; } /** * @brief calculate weight of the RadioSignal * * The function weight the RadioSignal from AP based on this RSSI value. * The stronger the signal the bigger weight it will get. * The RSSI at some distance d is: RSSI = A + B * gamma * lg (d / d0) * RSSI1 - RSSI2 = B * gamma * lg (d1 / d2) and d1 can be found when d2 = 1 meter. * The rssi at 1 meter distance from the transmitter assumed to be defined in parameter. * Gamma is the pass loss exponent, or attenuation coefficient. * For G_WEIGHT_COEF see Accuracy of RSS-Based Centroid Localization Algorithms in an Indoor Environment * * @param msr RadioMeasurement either from the access point * * @return @warning unnormalized weight of the measurement. */ static double calculateTransmitterTrustworthiness(const RadioMeasurementData& msr) { double B = 10 * GAMMA; double txPower = RSSI_ON_METER; double gCoef = G_WEIGHT_COEF; txPower = msr.power < 0.00001 ? RSSI_ON_METER : msr.power; double distance = pow(10, (txPower - msr.rssi) / B); //double apWeight = exp((aps[i].rssi - txPower) / B); double apWeight = 1 / pow(distance, gCoef); return apWeight; } static std::map calcTransmittersWeights( const RadioMeasurementsData& msr) { // Calculate AP weights in the basis of signal strength // We should also take into account number of incoming measurements during weights calculation!!!!!!!!!!!! // We call this function after averageDuplicateMeasurements std::map trWeightMap; double trWeightSum = 0.0; for (const auto& m : msr) { const TransmitterId& trId = m.id; double trWeight = USE_AP_WEIGHTS ? calculateTransmitterTrustworthiness(m) : 1.0; trWeightMap[trId] = trWeight; trWeightSum += trWeight; } for (auto& tp : trWeightMap) { tp.second /= trWeightSum; } return trWeightMap; } /** * This function calculates weights of RPs on the basis of RSS measurements and * radio map of the building. * The radiomap is multimap of transmitter as keys * and pairs of reference points ids and corresponding signal statistics as values */ std::map calcRpWeightsKnn( const std::multimap>& radiomap, const RadioMeasurementsData& msr, bool useDiffMode) { struct SigStat { TransmitterId txId; double rssiMeasured; double rssiDatabase;}; std::map> rpsStatistics; for (const auto& curMsr : msr) { auto range = radiomap.equal_range(curMsr.id); for (auto it = range.first; it != range.second; it++) { const std::pair& refPointsSsPair = it->second; const ReferencePointId& rpId = refPointsSsPair.first; if (rpsStatistics.find(rpId) == rpsStatistics.end()) rpsStatistics[rpId] = std::vector(); SigStat sigStat = {curMsr.id, curMsr.rssi, refPointsSsPair.second.mean}; rpsStatistics[rpId].push_back(sigStat); } } std::map rpWeights; std::map rpNormalize; std::map trWeights = calcTransmittersWeights(msr); for (const auto& pair : rpsStatistics) { const auto& rpId = pair.first; const auto& vecSigStat = pair.second; if (vecSigStat.size() < MIN_NUM_VISIBLE_BS_IN_RP) continue; for (auto it1 = vecSigStat.begin(); it1 != vecSigStat.end(); it1 ++) { if (useDiffMode) { for (auto it2 = vecSigStat.begin(); it2 != it1; it2++) { double measDiff = it1->rssiMeasured - it2->rssiMeasured; double mapDiff = it1->rssiDatabase - it2->rssiDatabase; // We sum with rp Penalties, to make sum of apWeight = 1 after normalization) double apW = trWeights[it1->txId] * trWeights[it2->txId]; rpNormalize[rpId] += apW; double weight = std::max(fabs(mapDiff - measDiff) * apW, MIN_RP_WEIGHT); rpWeights[rpId] += weight; } } else { double apW = trWeights[it1->txId]; rpNormalize[rpId] += apW; double weight = std::max(fabs(it1->rssiDatabase + it1->rssiMeasured) * apW, MIN_RP_WEIGHT); rpWeights[rpId] += weight; } } } for (auto& pair : rpWeights) { const ReferencePointId& rpId = pair.first; double& rpWeight = pair.second; if (rpWeight > MIN_WEIGHT_EPSILON) { // normalize in order to make sum of transmitter weights == 1, // because we summed transmitter weights on all APs, not only visible if (USE_AP_WEIGHTS) { auto rpDivider = rpNormalize[rpId]; if (rpDivider > MIN_WEIGHT_EPSILON) rpWeight /= rpDivider; } // do sqrt because this norm is better (it was shown empirically) // this norm also takes into account // the number of visible APS, due to its asymptotic is 1 / sqrt(N) rpWeight = std::sqrt(rpWeight); // Divide by number of visible APs because it differs from point to point // rely more on RP where there are more visible tx from measurements vector // for KNN the more penalty the more probability auto visibleApNum = rpsStatistics[rpId].size(); if (visibleApNum > 0) rpWeight /= visibleApNum; // Revert weights in KNN (to transform weights to probabilities) rpWeight = 1.0 / rpWeight; } } return rpWeights; } static double getTriangleWeight(const RefPointsTriangle& triangle, const std::map& rpWeights) { double w1, w2, w3; std::tie(w1, w2, w3) = getValueForEachKey(rpWeights, 0.0, triangle.rp1, triangle.rp2, triangle.rp3); return w1 + w2 + w3; } XYPoint calcPositionInTriangle(const std::vector& triangles, const Radiomap& radiomap, const std::map& rpWeights) { auto maxWeightTriangle = std::max_element(triangles.begin(), triangles.end(), [&rpWeights] (const RefPointsTriangle& lhs, const RefPointsTriangle& rhs) { return getTriangleWeight(lhs, rpWeights) < getTriangleWeight(rhs, rpWeights); }); double w1, w2, w3; std::tie(w1, w2, w3) = getValueForEachKey(rpWeights, 0.0, maxWeightTriangle->rp1, maxWeightTriangle->rp2, maxWeightTriangle->rp3); double norm = w1 + w2 + w3; if (norm < std::numeric_limits::epsilon()) { //TODO send error code return XYPoint(0.0, 0.0); } if (!radiomap.hasReferencePoint(maxWeightTriangle->rp1) || !radiomap.hasReferencePoint(maxWeightTriangle->rp2) || !radiomap.hasReferencePoint(maxWeightTriangle->rp3)) { //TODO send error code return XYPoint(0.0, 0.0); } ReferencePoint rp1 = radiomap.getReferencePoint(maxWeightTriangle->rp1); ReferencePoint rp2 = radiomap.getReferencePoint(maxWeightTriangle->rp2); ReferencePoint rp3 = radiomap.getReferencePoint(maxWeightTriangle->rp3); double x = (w1 * rp1.point.x + w2 * rp2.point.x + w3 * rp3.point.x) / norm; double y = (w1 * rp1.point.y + w2 * rp2.point.y + w3 * rp3.point.y) / norm; return XYPoint(x, y); } XYPoint calcKHeaviestRefPointsAverage(const Radiomap& radiomap, const std::map& rpToWeight, size_t k) { k = std::min(k, rpToWeight.size()); std::vector> kHeaviestPoints; kHeaviestPoints.reserve(k); std::partial_sort_copy( rpToWeight.begin(), rpToWeight.end(), kHeaviestPoints.begin(), kHeaviestPoints.end(), [](std::pair const& l, std::pair const& r) { return l.second > r.second; }); double x = 0.0; double y = 0.0; double norm = 0.0; for (auto it = kHeaviestPoints.cbegin(); it != kHeaviestPoints.cend(); it++) { // valid, due to heaviest point build structure ReferencePoint rp = radiomap.getReferencePoint(it->first); double w = it->second; x += w * rp.point.x; y += w * rp.point.y; norm += w; } if (norm < std::numeric_limits::epsilon()) { //TODO check if norm can be zero and process error return XYPoint(0.0, 0.0); } x /= norm; y /= norm; return XYPoint(x, y); } RadioMeasurementsData getWhitelistedMeasurements( const std::set& wl, const RadioMeasurementsData msrs) { RadioMeasurementsData filtered; std::copy_if(msrs.begin(), msrs.end(), std::back_inserter(filtered), [&wl](const RadioMeasurementData& msr) {return wl.find(msr.id) != wl.end(); }); return filtered; } } } // namespace navigine::navigation_core ================================================ FILE: src/knn/knn_utils.h ================================================ /** knn_utils.h * * Copyright (c) 2018 Navigine. * */ #ifndef KNN_UTILS_H #define KNN_UTILS_H #include #include #include #include #include #include "../triangulation.h" namespace navigine { namespace navigation_core { struct RefPointsTriangle { ReferencePointId rp1; ReferencePointId rp2; ReferencePointId rp3; }; struct TriangleVerticesWeights { double w1; double w2; double w3; }; std::map> triangulate(const std::shared_ptr& levelCollector); std::map>> getLevelsRadiomaps(const std::shared_ptr& levelCollector); std::map> getLevelReferenceTransmittersMap( const std::shared_ptr& levelCollector); std::map calcRpWeightsKnn( const std::multimap >& radiomap, const RadioMeasurementsData& msr, bool useDiffMode = true); XYPoint calcPositionInTriangle(const std::vector& triangles, const Radiomap& rps, const std::map& rpToWeight); XYPoint calcKHeaviestRefPointsAverage(const Radiomap& radiomap, const std::map& rpToWeight, size_t k); RadioMeasurementsData getWhitelistedMeasurements( const std::set& wl, const RadioMeasurementsData msrs); template ValType func_helper(const std::map& map, KeyType key, ValType defVal) { if (map.find(key) == map.end()) return defVal; else return map.find(key)->second; } template auto getValueForEachKey(const std::map& map, ValType defVal, Keys... keys) -> decltype(std::make_tuple(func_helper(map, keys, defVal)...)) { return std::make_tuple(func_helper(map, keys, defVal)...); } } } // namespace navigine::navigation_core #endif // KNN_UTILS_H ================================================ FILE: src/level.cpp ================================================ /** level.cpp * * Copyright (c) 2019 Navigine. * */ #include namespace navigine { namespace navigation_core { const GeoPoint& Level::bindingPoint() const { return mBindingPoint; } const LevelId& Level::id() const { return mId; } const Radiomap &Level::radiomap() const { return mRadiomap; } const Graph& Level::graph() const { return mGraph; } const LevelGeometry& Level::geometry() const { return mGeometry; } bool Level::containsTransmitter(const TransmitterId& txId) const { return mTransmitters.find(txId) != mTransmitters.end(); } const Transmitter& Level::transmitter(const TransmitterId& txId) const { return mTransmitters.at(txId); } void Level::addTransmitters(const XYZTransmitters& transmitters) { for (const Transmitter& tx: transmitters) mTransmitters.insert({tx.id, tx}); } //TODO after second call should reference points be added to existed or replaced? void Level::setReferencePoints(const XYReferencePoints& referencePoints) { mRadiomap = Radiomap(referencePoints); } void Level::setGraph(const Graph& graph) { mGraph = graph; } void Level::setGeometry(const LevelGeometry& levelGeometry) { mGeometry = levelGeometry; } RadioMeasurementsData getLevelRadioMeasurements( const Level& level, const RadioMeasurementsData& radioMsr) { RadioMeasurementsData levelRadioMeasurements; for (const RadioMeasurementData& msr: radioMsr) { if (level.containsTransmitter(msr.id)) levelRadioMeasurements.push_back(msr); } return levelRadioMeasurements; } } } // namespace navigine::navigation_core ================================================ FILE: src/level_collector.cpp ================================================ /** level_collector.cpp * * Copyright (c) 2019 Navigine. * */ #include namespace navigine { namespace navigation_core { namespace { XYZTransmitters getTransmittersInLocalCoordinates( const GeoPoint& binding, const std::vector>& transmitters, const double& altitude) { XYZTransmitters localTransmitters; for (const Transmitter& t: transmitters) { XYPoint point2D = gpsToLocal(GeoPoint(t.point.latitude, t.point.longitude), binding); localTransmitters.emplace_back( TransmitterId(t.id.value), XYZPoint(point2D.x, point2D.y, altitude + t.point.altitude), t.pathlossModel, t.type); } return localTransmitters; } XYReferencePoints getRefPointsInLocalCoordinates( const GeoPoint& binding, const std::vector>& referencePoints) { XYReferencePoints localReferencePoints; for (const ReferencePoint& rp: referencePoints) { localReferencePoints.emplace_back( ReferencePointId(rp.id.value), gpsToLocal(rp.point, binding), rp.fingerprints); } return localReferencePoints; } Polygon getPolygonInLocalCoordinates( const GeoPoint& binding, const Polygon& geoPolygon) { Polygon result; for (const auto& p : geoPolygon.outer()) { XYPoint pxy = gpsToLocal(GeoPoint(p.x(), p.y()), binding); boost::geometry::append(result.outer(), Point(pxy.x, pxy.y)); } for (const auto& innerRing : geoPolygon.inners()) { result.inners().push_back({}); for (const auto& p : innerRing) { XYPoint pxy = gpsToLocal(GeoPoint(p.x(), p.y()), binding); boost::geometry::append(result.inners().back(), Point(pxy.x, pxy.y)); } } boost::geometry::correct(result); return result; } Box getBBoxInLocalCoordinates(const GeoPoint& binding, const Box& geoBox) { XYPoint pmin = gpsToLocal(GeoPoint(geoBox.min_corner().x(), geoBox.min_corner().y()), binding); XYPoint pmax = gpsToLocal(GeoPoint(geoBox.max_corner().x(), geoBox.max_corner().y()), binding); return Box(Point(pmin.x, pmin.y), Point(pmax.x, pmax.y)); } LevelGeometry getGeometryInLocalCoordinates( const GeoPoint& binding, const LevelGeometry& geolevelGeometry) { Multipolygon localArea; for (const auto& poly : geolevelGeometry.allowedArea()) { localArea.push_back(getPolygonInLocalCoordinates(binding, poly)); } boost::geometry::correct(localArea); Box localBox = getBBoxInLocalCoordinates(binding, geolevelGeometry.boundingBox()); boost::geometry::correct(localBox); return LevelGeometry{localArea, localBox}; } Graph getGraphInLocalCoordinates( const GeoPoint& binding, const Graph& geoGraph) { Graph xyGraph; std::map geoToXyVertexIds; for (Graph::VertexIterator it = geoGraph.vertexBegin(); it != geoGraph.vertexEnd(); it++) { Graph::Vertex geoVertex = *it; XYPoint p = gpsToLocal(geoVertex.point, binding); Graph::Vertex xyVertex = xyGraph.addVertex(p); geoToXyVertexIds[geoVertex.id] = xyVertex.id; } for (Graph::EdgeIterator it = geoGraph.edgesBegin(); it != geoGraph.edgesEnd(); it++) { Graph::Vertex geoV1 = geoGraph.getVertex(it->first); Graph::Vertex geoV2 = geoGraph.getVertex(it->second); int xyV1 = geoToXyVertexIds[geoV1.id]; int xyV2 = geoToXyVertexIds[geoV2.id]; xyGraph.addEdge(xyV1, xyV2); } return xyGraph; } } //namespace std::shared_ptr createLevelCollector() { return std::make_shared(); } boost::optional LevelCollector::findLevelByTransmitterId(const TransmitterId &transmitterId) const { for (const Level& level: mLevels) { if (level.containsTransmitter(transmitterId)) { return level.id(); } } return boost::none; } const std::vector& LevelCollector::levels() const { return mLevels; } bool LevelCollector::hasLevel(const LevelId& levelId) const { return mLevelsIndices.find(levelId) != mLevelsIndices.end(); } const Level& LevelCollector::level(const LevelId& levelId) const { return mLevels.at(mLevelsIndices.at(levelId)); } GeoPoint getBinding(const GeoLevel& geoLevel) { auto geoBox = geoLevel.geometry.boundingBox(); boost::geometry::correct(geoBox); double minLatitude = geoBox.min_corner().x(); double minLongitude = geoBox.min_corner().y(); return GeoPoint(minLatitude, minLongitude); } void LevelCollector::addGeoLevel(const GeoLevel& geoLevel) { LevelId levelId = LevelId(geoLevel.id); GeoPoint binding = getBinding(geoLevel); Level level(levelId, binding); level.addTransmitters(getTransmittersInLocalCoordinates(binding, geoLevel.transmitters, geoLevel.altitude)); level.setGeometry(getGeometryInLocalCoordinates(binding, geoLevel.geometry)); level.setReferencePoints(getRefPointsInLocalCoordinates(binding, geoLevel.referencePoints)); level.setGraph(getGraphInLocalCoordinates(binding, geoLevel.graph)); mLevels.push_back(std::move(level)); mLevelsIndices.insert({levelId, mLevels.size() - 1}); } } } // namespace navigine::navigation_core ================================================ FILE: src/level_estimator/barometer.cpp ================================================ #include #include #include #include #include "barometer.h" namespace navigine { namespace navigation_core { #ifdef DEBUG_OUTPUT_BAROMETER #include #include #endif static const double BAROMETER_R = 8.31; // [J / (mol * K)] static const double BAROMETER_G = 9.8; // [m / s^2] static const double BAROMETER_T = 288.0; // [K] static const double BAROMETER_M = 0.0289; // [kg / mol] static const double BAROMETER_PASCAL_TO_METERS = -BAROMETER_R * BAROMETER_T / (BAROMETER_G * BAROMETER_M); static const double BAROMETER_MBAR_TO_PASCAL = 100.0; static const double BAROMETER_MIN_FLOOR_HEIGHT_M = 2.0; // [m] static const size_t SIZE_OF_CHUNK = 100; static double calcMean(const std::deque& measurements) { if (measurements.empty()) { //TODO process error return 0; } return std::accumulate(measurements.begin(), measurements.end(), 0.0) / measurements.size(); } //TODO test if there is no barometer in the phone //TODO try to use ring buffer with fixed size //TODO take into account vertical speed in Pa/sec // - the floor is changed only if the vertical displacement is more than distance between floors (i.e. 3m) LevelId Barometer::checkLevelChange( const LevelId& levelId, const SensorMeasurement& sensorMsr) { if (!mIsInitialized) { mIsInitialized = true; return levelId; } updateBuffers(sensorMsr); updateLevel(levelId); return mLastKnownLevel; } void Barometer::updateBuffers(const SensorMeasurement& sensorMsr) { #ifdef DEBUG_OUTPUT_BAROMETER std::ofstream debugOutputFile; debugOutputFile.open(DEBUG_OUTPUT_BAROMETER, std::ios_base::app); if (sensorMsr.data.type == SENSOR_ENTRY_BAROMETER) debugOutputFile << m.ts << " " << m.values.x << std::endl; debugOutputFile.close(); #endif if (sensorMsr.data.type == SensorMeasurementData::Type::BAROMETER) { if (mStoredMeasurements.size() > SIZE_OF_CHUNK) mStoredMeasurements.pop_front(); if (mNewMeasurements.size() > SIZE_OF_CHUNK) { mStoredMeasurements.push_back(mNewMeasurements.front()); mNewMeasurements.pop_front(); } mNewMeasurements.push_back(sensorMsr.data.values.x); } } //TODO take into account pressure change speed - i.e. a person can move slowly upstairs void Barometer::updateLevel(LevelId levelId) { if ((mNewMeasurements.size() > SIZE_OF_CHUNK) && (mStoredMeasurements.size() >= SIZE_OF_CHUNK)) { if (mLastKnownLevel != levelId) { double curMean = calcMean(mNewMeasurements); double lastMean = calcMean(mStoredMeasurements); if (curMean <= std::numeric_limits::epsilon() || lastMean <= std::numeric_limits::epsilon()) { //TODO: process error: incorrect average pressure! return; } double p2Pa = BAROMETER_MBAR_TO_PASCAL * curMean; double p1Pa = BAROMETER_MBAR_TO_PASCAL * lastMean; double deltaHeightM = BAROMETER_PASCAL_TO_METERS * std::log(p2Pa / p1Pa); bool levelHasChanged = std::abs(deltaHeightM) > BAROMETER_MIN_FLOOR_HEIGHT_M; if (levelHasChanged) mLastKnownLevel = levelId; } } else mLastKnownLevel = levelId; } } } // namespace navigine::navigation_core ================================================ FILE: src/level_estimator/barometer.h ================================================ /** barometer.h * * Copyright (c) 2018 Navigine. * */ #ifndef BAROMETER_H #define BAROMETER_H #include #include #include #include "../measurement_types.h" namespace navigine { namespace navigation_core { class Barometer { public: Barometer() : mIsInitialized ( false ), mLastKnownLevel ( LevelId("")) {} LevelId checkLevelChange( const LevelId &levelId, const SensorMeasurement& sensorMsr); private: void updateBuffers(const SensorMeasurement& sensorMsr); void updateLevel(LevelId levelId); std::deque mNewMeasurements = {}; std::deque mStoredMeasurements = {}; bool mIsInitialized; LevelId mLastKnownLevel; }; } } // namespace navigine::navigation_core #endif // BAROMETER_H ================================================ FILE: src/level_estimator/level_estimator.cpp ================================================ /** level_estimator.cpp * ** Copyright (c) 2017 Navigine. * */ #include #include "level_estimator.h" namespace navigine { namespace navigation_core { LevelEstimator::LevelEstimator(const NavigationSettings& navProps) : mUseBarometer(navProps.commonSettings.useBarometer) { } /** * function detects id of current level according to highest average RSSI for * placed transmitters. In a case of any error returns -1 */ LevelId LevelEstimator::calculateLevel( const RadioMeasurementsData& radioMsr, const SensorMeasurement& sensorMsr) { LevelId detectedLevelId = detectCurrentLevel(radioMsr); LevelId levelIdWithHistory = mHistory.detectLevelUsingHistory(detectedLevelId); LevelId levelId = mUseBarometer ? mBarometer.checkLevelChange(levelIdWithHistory, sensorMsr) : levelIdWithHistory; return levelId; } } } // namespace navigine::navigation_core ================================================ FILE: src/level_estimator/level_estimator.h ================================================ /** level_estimator.h * ** Copyright (c) 2017 Navigine. * */ #ifndef LEVEL_ESTIMATOR_H #define LEVEL_ESTIMATOR_H #include #include #include #include "level_history.h" #include "barometer.h" #include "../measurement_types.h" namespace navigine { namespace navigation_core { class LevelEstimator { public: LevelEstimator(const NavigationSettings& navProps); virtual ~LevelEstimator() {} LevelId calculateLevel( const RadioMeasurementsData& radioMsr, const SensorMeasurement& sensorMsr); protected: virtual LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) = 0; private: LevelHistory mHistory; Barometer mBarometer; const bool mUseBarometer; }; } } // namespace navigine::navigation_core #endif // LEVEL_ESTIMATOR_H ================================================ FILE: src/level_estimator/level_estimator_radiomap.cpp ================================================ /** level_estimator_radiomap.cpp * ** Copyright (c) 2017 Navigine. * */ #include #include #include "level_estimator_radiomap.h" #include "../knn/knn_utils.h" namespace navigine { namespace navigation_core { LevelEstimatorRadiomap::LevelEstimatorRadiomap( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) : LevelEstimator(navProps) , mLevelIndex(levelCollector) , mUseBestRp(navProps.commonSettings.useBestRefPointLevel) { mLevelsRadiomaps = getLevelsRadiomaps(levelCollector); } LevelId LevelEstimatorRadiomap::detectCurrentLevel(const RadioMeasurementsData& radioMsr) { LevelId bestLevelId = LevelId(""); double bestLevelWeight = 0.0; for (const Level& level : mLevelIndex->levels()) { const std::multimap& radiomap = mLevelsRadiomaps.at(level.id()); double levelWeight = mUseBestRp ? calcWeightBestRp(radioMsr, radiomap) : calcWeight(radioMsr, radiomap); if (levelWeight > bestLevelWeight) { bestLevelWeight = levelWeight; bestLevelId = level.id(); } } return bestLevelId; } /** level weight is calculated based of difference between measured and reference signals */ double LevelEstimatorRadiomap::calcWeight( const RadioMeasurementsData& radioMsr, const std::multimap &fingerprints) { int nUniqueTransmitters = 0; double levelWeight = 0.0; for (const RadioMeasurementData& meas : radioMsr) { //TODO try to use lower weight for wifi signal since it is not reliable for level identification double signalTypeWeight = 1.0;//(meas.type == SIGNAL_ENTRY_WIFI) ? 0.01 : 1.0; double measWeight = 0.0; int numOfOccurrences = 0; auto range = fingerprints.equal_range(meas.id); for (auto it = range.first; it != range.second; ++it) { const fingerprint& refPointsSsPair = it->second; const SignalStatistics& sigStat = refPointsSsPair.second; // It is possible that positive rssi values are used in measure.xml // and negative rssi values are used in measurements - so both abs are necessary here: double difference = std::fabs(std::fabs(meas.rssi) - std::fabs(sigStat.mean)); measWeight += signalTypeWeight * 1.0 / (difference + 0.1); numOfOccurrences ++; } if (numOfOccurrences > 0) levelWeight += measWeight / numOfOccurrences; if (std::distance(range.first, range.second) > 0) nUniqueTransmitters++; } levelWeight *= static_cast(nUniqueTransmitters); return levelWeight; } /** level weight is given equal to weight of best reference point */ double LevelEstimatorRadiomap::calcWeightBestRp( const RadioMeasurementsData& radioMsr, const std::multimap& radiomap) { std::map rpToWeight = calcRpWeightsKnn(radiomap, radioMsr, true); double bestWeight = 0.0; if (!rpToWeight.empty()) { ReferencePointId bestRp = rpToWeight.begin()->first; for (auto const& rpWeightPair : rpToWeight) { if (rpWeightPair.second > bestWeight) { bestRp = rpWeightPair.first; bestWeight = rpWeightPair.second; } } } return bestWeight; } } } // namespace navigine::navigation_core ================================================ FILE: src/level_estimator/level_estimator_radiomap.h ================================================ /** level_estimator_radiomap.h * ** Copyright (c) 2017 Navigine. * */ #ifndef LEVEL_ESTIMATOR_RADIOMAP_H #define LEVEL_ESTIMATOR_RADIOMAP_H #include "level_estimator.h" namespace navigine { namespace navigation_core { class LevelEstimatorRadiomap : public LevelEstimator { public: LevelEstimatorRadiomap( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); protected: LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) override; private: using fingerprint = std::pair; double calcWeight( const RadioMeasurementsData& radioMsr, const std::multimap &fingerprints); double calcWeightBestRp( const RadioMeasurementsData& radioMsr, const std::multimap &radiomap); std::shared_ptr mLevelIndex; std::map> mLevelsRadiomaps; const bool mUseBestRp; }; } } // namespace navigine::navigation_core #endif // LEVEL_ESTIMATOR_RADIOMAP_H ================================================ FILE: src/level_estimator/level_estimator_transmitters.cpp ================================================ /** level_estimator_transmitters.cpp * ** Copyright (c) 2019 Navigine. * */ #include "level_estimator_transmitters.h" namespace navigine { namespace navigation_core { LevelEstimatorTransmitters::LevelEstimatorTransmitters( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) : LevelEstimator(navProps) , mLevelIndex(levelCollector) , mUseTxPower(navProps.commonSettings.useTxPower) { } static double getAverageRssi(const RadioMeasurementsData& radioMsr) { double averageRssi = 0.0; for(int i = 0; i < (int)radioMsr.size(); ++i) averageRssi += radioMsr[i].rssi; averageRssi /= std::max((int)radioMsr.size(), 1); return averageRssi; } static double getAverageA(const Level& level, const RadioMeasurementsData& radioMsr) { double averageA = 0.0; for(int i = 0; i < (int)radioMsr.size(); ++i) { const Transmitter transmitter = level.transmitter(radioMsr[i].id); const PathlossModel pathLossModel = transmitter.pathlossModel; averageA += pathLossModel.A; } averageA /= std::max((int)radioMsr.size(), 1); return averageA; } LevelId LevelEstimatorTransmitters::detectCurrentLevel(const RadioMeasurementsData& radioMsr) { LevelId bestLevelId(""); double bestAverCost = -1000.0; for (const Level& level : mLevelIndex->levels()) { RadioMeasurementsData levelMsr = getLevelRadioMeasurements(level, radioMsr); if (!levelMsr.empty()) { double averageRssi = getAverageRssi(levelMsr); double averageA; if (mUseTxPower) averageA = getAverageA(level, levelMsr); else averageA = 0; double averageCost = averageRssi + averageA; // Using weighted average. Weight depends on number of visible Transmitters double weight = 1.0 + 0.1 * levelMsr.size() / radioMsr.size(); averageCost /= weight; if (averageCost > bestAverCost) { bestAverCost = averageCost; bestLevelId = level.id(); } } } return bestLevelId; } } } // namespace navigine::navigation_core ================================================ FILE: src/level_estimator/level_estimator_transmitters.h ================================================ /** level_estimator_transmitters.h * ** Copyright (c) 2019 Navigine. * */ #ifndef LEVEL_ESTIMATOR_TRANSMITTERS_H #define LEVEL_ESTIMATOR_TRANSMITTERS_H #include "level_estimator.h" namespace navigine { namespace navigation_core { class LevelEstimatorTransmitters : public LevelEstimator { public: LevelEstimatorTransmitters( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); protected: LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) override; private: std::shared_ptr mLevelIndex; const bool mUseTxPower; }; } } // namespace navigine::navigation_core #endif // LEVEL_ESTIMATOR_TRANSMITTERS_H ================================================ FILE: src/level_estimator/level_history.cpp ================================================ /** level_history.cpp * * Copyright (c) 2017 Navigine. * */ #include "level_history.h" namespace navigine { namespace navigation_core { static const int MAX_LEVEL_ID_OCCUR = 4; LevelHistory::LevelHistory() : mMaxLevelIdOccurrence(MAX_LEVEL_ID_OCCUR) , mIsInitialized(false) , mCurrentLevelId(LevelId("")) { } LevelId LevelHistory::detectLevelUsingHistory(const LevelId& detectedLevelId) { //TODO change to boost optional!!! if (detectedLevelId.value == "") return mCurrentLevelId; if (!mIsInitialized) { mIsInitialized = true; mCurrentLevelId = detectedLevelId; } if (++mFloorCnt[detectedLevelId] > MAX_LEVEL_ID_OCCUR) { mFloorCnt.clear(); mFloorCnt[detectedLevelId] = MAX_LEVEL_ID_OCCUR; mCurrentLevelId = detectedLevelId; } return mCurrentLevelId; } } } // namespace navigine::navigation_core ================================================ FILE: src/level_estimator/level_history.h ================================================ /** level_history.h * * Copyright (c) 2017 Navigine. * */ #ifndef LEVEL_HISTORY_H #define LEVEL_HISTORY_H #include #include #include namespace navigine { namespace navigation_core { class LevelHistory { public: LevelHistory(); LevelId detectLevelUsingHistory(const LevelId &detectedLevelId); private: const int mMaxLevelIdOccurrence; LevelId mCurrentLevelId; bool mIsInitialized; std::map mFloorCnt; }; } } // namespace navigine::navigation_core #endif // LEVEL_HISTORY_H ================================================ FILE: src/level_geometry.cpp ================================================ #include namespace navigine { namespace navigation_core { namespace { } LevelGeometry::LevelGeometry(const Multipolygon& allowedArea, const Box& box) : mAllowedArea(allowedArea) , mBoundingBox(box) { for (const auto& polygon : mAllowedArea) for (const auto& inner : polygon.inners()) mInnerBarriers.push_back(inner); std::vector boxIndices; for (std::size_t i = 0; i < mInnerBarriers.size(); i++) { boost::geometry::correct(mInnerBarriers[i]); boxIndices.push_back({boost::geometry::return_envelope(mInnerBarriers[i]), i}); } mTree = RTree(boxIndices); } bool LevelGeometry::isPathTouchesBarriers(const Linestring& path) const { bool isInsideAllowedArea = false; for (const auto& allowedZone : mAllowedArea) if (boost::geometry::covered_by(path, allowedZone.outer())) isInsideAllowedArea = true; if (!isInsideAllowedArea) return true; std::vector intersectedGeometryBoxes; mTree.query(boost::geometry::index::intersects(path), std::back_inserter(intersectedGeometryBoxes)); for (const auto& boxIdxPair : intersectedGeometryBoxes) { if (boost::geometry::intersects(path, mInnerBarriers[boxIdxPair.second])) return true; } return false; } const Multipolygon& LevelGeometry::allowedArea() const { return mAllowedArea; } const Box& LevelGeometry::boundingBox() const { return mBoundingBox; } } } // namespace navigine::navigation_core ================================================ FILE: src/likelihood/likelihood.cpp ================================================ #include #include #include #include #include #include "likelihood.h" namespace navigine { namespace navigation_core { bool isParticleInIntersectionArea( const Level& level, const RadioMeasurementsData& radioMsr, double x, double y) { for (const auto& msr : radioMsr) { const Transmitter tx = level.transmitter(msr.id); const double circleEps = -1e-7; const double dx = tx.point.x - x; const double dy = tx.point.y - y; const double modelValue = std::sqrt(dx * dx + dy * dy); const double delta = msr.distance - modelValue; if (delta < circleEps) return false; } return true; }; } } ================================================ FILE: src/likelihood/likelihood.h ================================================ #ifndef LIKELIHOOD_H #define LIKELIHOOD_H #include #include #include #include #include "../position.h" namespace navigine { namespace navigation_core { const constexpr double MIN_WEIGHT_SUM_DOUBLE_EPSILON = 1e-8; enum class MeasurementLikelihoodModel {DISTANCE_V1, DISTANCE_V2, RSSI, RADIOMAP}; class Likelihood { public: virtual ~Likelihood() {} virtual std::vector calculateWeights( const Level& levelCollector, const RadioMeasurementsData& msr, const std::vector& particles) = 0; /** * optimization for mutation and sampling - first we perform all calculations necessary for likelihood * and then use this likelihood to get particle weights in method getWeight() */ virtual void doPreliminaryCalculations( const Level& level, const RadioMeasurementsData& radioMsr) = 0; /** * return position weight base on information calculated during Likelihood::compute() method */ virtual double getWeight( const Level& level, const RadioMeasurementsData& radioMsr, double x, double y) const = 0; virtual bool hasSufficientMeasurementsForUpdate( const Level& level, const RadioMeasurementsData& radioMsr) const = 0; virtual bool hasSufficientMeasurementsForMutation( const Level& level, const RadioMeasurementsData& radioMsr) const = 0; /** * Heuristic to improve particle filter robustness. * The method returns points for sampling particles around them. * Depending on implementation it can be transmitter positions with best signals, * or reference points with best weighs */ virtual std::vector calculateSamplingCenters( const Level& level, const RadioMeasurementsData& radioMsr) = 0; virtual MeasurementLikelihoodModel getLikelihoodModel(void) = 0; virtual RadioMeasurementsData getOnlyCorrectlyIntersectedMeasurements( const Level& level, const RadioMeasurementsData& radioMsr) const = 0; }; bool isParticleInIntersectionArea( const Level& level, const RadioMeasurementsData& radioMsr, double x, double y); std::shared_ptr createLikelihoodLogModel(const std::shared_ptr& levelCollector, const NavigationSettings& navProps, MeasurementLikelihoodModel likelihoodModel); } } // namespace navigine::navigation_core #endif // LIKELIHOOD_H ================================================ FILE: src/likelihood/likelihood_radiomap.cpp ================================================ #include "likelihood_radiomap.h" #include "../knn/knn_utils.h" namespace navigine { namespace navigation_core { #ifdef NAVIGATION_VISUALIZATION XYPoint LikelihoodRadiomap::debugTrianglePoint = XYPoint(0, 0); std::map LikelihoodRadiomap::debugRefPointWeights = std::map(); #endif LikelihoodRadiomap::LikelihoodRadiomap( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) : mK(navProps.commonSettings.kNeighbors) , mMinMsrNumForMutation(navProps.commonSettings.minMsrNumForMutation) , mMinMsrNumForPositionCalculation(navProps.commonSettings.minNumOfMeasurementsForPositionCalculation) , mUseTriangles(navProps.commonSettings.useTriangles) , mUseDiffMode(navProps.commonSettings.useDiffMode) , mLikelihoodPoint(boost::none) { if (mUseTriangles) mLevelTriangles = triangulate(levelCollector); mLevelReferenceTransmittersMap = getLevelReferenceTransmittersMap(levelCollector); mLevelsRadiomaps = getLevelsRadiomaps(levelCollector); } bool LikelihoodRadiomap::hasSufficientMeasurementsForUpdate(const Level& level, const RadioMeasurementsData& radioMsr) const { const std::set& txIds = mLevelReferenceTransmittersMap.at(level.id()); auto validMsr = getWhitelistedMeasurements(txIds, radioMsr); return validMsr.size() >= mMinMsrNumForPositionCalculation; } bool LikelihoodRadiomap::hasSufficientMeasurementsForMutation( const Level& level, const RadioMeasurementsData& radioMsr) const { const std::set& txIds = mLevelReferenceTransmittersMap.at(level.id()); auto validMsr = getWhitelistedMeasurements(txIds, radioMsr); return validMsr.size() >= mMinMsrNumForMutation; } double calcParticleWeight( XYParticle particle, XYPoint trianglePosition) { double sqrDist = (particle.x - trianglePosition.x) * (particle.x - trianglePosition.x) + (particle.y - trianglePosition.y) * (particle.y - trianglePosition.y); return 1.0 / (0.1 + sqrt(sqrDist)); } std::vector LikelihoodRadiomap::calculateWeights(const Level& level, const RadioMeasurementsData& radioMsr, const std::vector& particles) { const std::set& txIds = mLevelReferenceTransmittersMap.at(level.id()); auto validMsr = getWhitelistedMeasurements(txIds, radioMsr); std::vector weightsMult(particles.size(), 1.0 / particles.size()); if (validMsr.size() < mMinMsrNumForPositionCalculation) { //TODO check if error is possible and process error return weightsMult; } const std::multimap>& radiomap = mLevelsRadiomaps.at(level.id()); std::map rpToWeight = calcRpWeightsKnn(radiomap, validMsr, mUseDiffMode); boost::optional p = calculateRadiomapPoint(level.id(), level.radiomap(), rpToWeight); #ifdef NAVIGATION_VISUALIZATION std::map weights; for (const auto &rpPair: rpToWeight) weights.insert(rpPair); debugRefPointWeights = weights; if (p.is_initialized()) debugTrianglePoint = p.get(); #endif double sum = 0.0; for (size_t i = 0; i < particles.size(); i++) { double w = p.is_initialized() ? calcParticleWeight(particles[i], p.get()) : 0.0; weightsMult[i] = w; sum += w; } if (sum > MIN_WEIGHT_SUM_DOUBLE_EPSILON) { for (size_t i = 0; i < particles.size(); i++) { weightsMult[i] /= sum; } } return weightsMult; } boost::optional LikelihoodRadiomap::calculateRadiomapPoint( const LevelId& levelId, const Radiomap& radiomap, const std::map& rpToWeight) { if (!mUseTriangles) { return calcKHeaviestRefPointsAverage(radiomap, rpToWeight, mK); } else { auto trianglesIt = mLevelTriangles.find(levelId); if (trianglesIt == mLevelTriangles.end()) { return boost::none; } const std::vector& triangles = trianglesIt->second; if (triangles.empty()) { return boost::none; } return calcPositionInTriangle(triangles, radiomap, rpToWeight); } } std::vector LikelihoodRadiomap::calculateSamplingCenters(const Level& level, const RadioMeasurementsData& radioMsr) { this->doPreliminaryCalculations(level, radioMsr); std::vector samplingCenters; if (mLikelihoodPoint.is_initialized()) samplingCenters.push_back(mLikelihoodPoint.get()); return samplingCenters; } void LikelihoodRadiomap::doPreliminaryCalculations( const Level& level, const RadioMeasurementsData& radioMsr) { const std::set& txIds = mLevelReferenceTransmittersMap.at(level.id()); auto validMsr = getWhitelistedMeasurements(txIds, radioMsr); if (validMsr.size() >= mMinMsrNumForPositionCalculation) { const Radiomap& refPointsMap = level.radiomap(); const std::multimap>& signalsMap = mLevelsRadiomaps.at(level.id()); std::map rpToWeight = calcRpWeightsKnn(signalsMap, validMsr, mUseDiffMode); mLikelihoodPoint = calculateRadiomapPoint(level.id(), refPointsMap, rpToWeight); } } double LikelihoodRadiomap::getWeight(const Level&, const RadioMeasurementsData&, double x, double y) const { return mLikelihoodPoint.is_initialized() ? calcParticleWeight(XYParticle{x, y, 0}, mLikelihoodPoint.get()) : 0.0; } RadioMeasurementsData LikelihoodRadiomap::getOnlyCorrectlyIntersectedMeasurements( const Level& level, const RadioMeasurementsData& radioMsr) const { return radioMsr; } } } // namespace navigine::navigation_core ================================================ FILE: src/likelihood/likelihood_radiomap.h ================================================ #ifndef LIKELIHOOD_TRIANGULATION_H #define LIKELIHOOD_TRIANGULATION_H #include #include #include "likelihood.h" #include "../knn/knn_utils.h" namespace navigine { namespace navigation_core { class LikelihoodRadiomap : public Likelihood { public: LikelihoodRadiomap( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); std::vector calculateWeights( const Level& level, const RadioMeasurementsData& radioMsr, const std::vector& particles ) override; void doPreliminaryCalculations(const Level& level, const RadioMeasurementsData& radioMsr ) override; double getWeight( const Level& level, const RadioMeasurementsData& radioMsr, double x, double y ) const override; std::vector calculateSamplingCenters( const Level& level, const RadioMeasurementsData& radioMsr ) override; bool hasSufficientMeasurementsForUpdate( const Level& level, const RadioMeasurementsData& radioMsr ) const override; bool hasSufficientMeasurementsForMutation(const Level& level, const RadioMeasurementsData& radioMsr ) const override; MeasurementLikelihoodModel getLikelihoodModel(void) override{return mLikelihoodModel;} RadioMeasurementsData getOnlyCorrectlyIntersectedMeasurements( const Level& level, const RadioMeasurementsData& radioMsr ) const override; #ifdef NAVIGATION_VISUALIZATION static XYPoint debugTrianglePoint; static std::map debugRefPointWeights; #endif private: boost::optional calculateRadiomapPoint( const LevelId& levelId, const Radiomap& radiomap, const std::map& rpToWeight); const MeasurementLikelihoodModel mLikelihoodModel = MeasurementLikelihoodModel::RADIOMAP; const int mK; const size_t mMinMsrNumForMutation; const size_t mMinMsrNumForPositionCalculation; const bool mUseTriangles; const bool mUseDiffMode; //TODO take into account probability of such point! boost::optional mLikelihoodPoint; // point for particle mutation and particle sampling std::map> mLevelTriangles; std::map> mLevelReferenceTransmittersMap; std::map>> mLevelsRadiomaps; }; } } // namespace navigine::navigation_core #endif // LIKELIHOOD_TRIANGULATION_H ================================================ FILE: src/measurement_types.h ================================================ #pragma once #include namespace navigine { namespace navigation_core { using SensorMeasurement = MeasurementDataWithTimeStamp; // TODO: m.doroshenko rename me using NmeaMeasurement = MeasurementDataWithTimeStamp; // TODO: m.doroshenko rename me using RadioMeasurement = MeasurementDataWithTimeStamp; // TODO: m.doroshenko rename me } } // namespace navigine::navigation_core ================================================ FILE: src/measurements/measurement_preprocessor.cpp ================================================ /** measurement_preprocessor.cpp * * Copyright (c) 2017 Navigine. * */ #include #include #include "measurement_preprocessor.h" #if defined (DEBUG_OUTPUT_MSR_PREPROCESSOR) #include #include namespace navigine { namespace navigation_core { static const std::string PREPROCESSED_MSR_DEBUG_OUTPUT_FILE = DEBUG_OUTPUT_MSR_PREPROCESSOR; void clearDebugOutput(); void printInputSignals(NavigationMessage* navMsg, RadioMeasurementsData& entries); void printExtractedMeasurements(RadioMeasurementsData& radioMeasurements); } } // namespace navigine::navigation_core #endif namespace navigine { namespace navigation_core { namespace { const constexpr bool DEFAULT_USE_WIFI = false; const constexpr bool DEFAULT_USE_BLE = true; bool checkUseWifi(const NavigationSettings& navProps) { const auto signalsToUse = navProps.commonSettings.signalsToUse; if (signalsToUse == CommonSettings::SignalsToUse::WIFI || signalsToUse == CommonSettings::SignalsToUse::BOTH) { return true; } else if (signalsToUse == CommonSettings::SignalsToUse::BLE) { return false; } return DEFAULT_USE_WIFI; } bool checkUseBle(const NavigationSettings& navProps) { const auto signalsToUse = navProps.commonSettings.signalsToUse; if (signalsToUse == CommonSettings::SignalsToUse::BLE || signalsToUse == CommonSettings::SignalsToUse::BOTH) { return true; } else if (signalsToUse == CommonSettings::SignalsToUse::WIFI) { return false; } return DEFAULT_USE_BLE; } RadioMeasurementBuffer createRadioMeasurementBuffer( const NavigationSettings& navProps) { long long sigAverageTime = (long long)(1000 * navProps.commonSettings.sigAveragingTime); long long sigWindowShift = (long long)(1000 * navProps.commonSettings.sigWindowShift); long long stopDetectTime = (long long)(1000 * navProps.commonSettings.stopDetectionTime); bool useStops = navProps.commonSettings.useStops; // TO DO: why RadioMeasurementBuffer is not initialized here if (sigWindowShift == 0 || sigWindowShift > sigAverageTime) sigWindowShift = sigAverageTime; long long keepRadioTimeMs = sigAverageTime - sigWindowShift; double rssiBias = 0; return RadioMeasurementBuffer(keepRadioTimeMs, sigWindowShift, stopDetectTime, rssiBias, useStops); } } MeasurementsPreprocessor::MeasurementsPreprocessor( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) : mLevelIndex(levelCollector) , mWiFiRttOffset(navProps.commonSettings.wiFiRttOffset) , mCutOffRssi(navProps.commonSettings.sigCutOffRss) , mUseClosestAps(navProps.commonSettings.useClosestAps) , mNumClosestAps(navProps.commonSettings.numClosestAps) , mUseUnknownTxs(navProps.commonSettings.useUnknownTransmitters) , mMeasurementType(navProps.commonSettings.measurementType) , mUseWifi(checkUseWifi(navProps)) , mUseBle(checkUseBle(navProps)) , mIsTracking(navProps.commonSettings.useTracking) , mRadiosBuffer(createRadioMeasurementBuffer(navProps)) , mCurrentTs(0) { } void MeasurementsPreprocessor::updateMeasurements(const Measurement& navMsg) { mCurrentTs = navMsg.ts; boost::apply_visitor(*this, navMsg.data); } void MeasurementsPreprocessor::operator()(const RadioMeasurementData& measurement) { RadioMeasurementData signal = measurement; boost::optional validSignalData = getValidSignalEntry(signal); if (validSignalData.is_initialized()) { RadioMeasurement validSignal; validSignal.data = validSignalData.get(); validSignal.ts = mCurrentTs; mRadiosBuffer.addMeasurement(validSignal); mLastSignalTs = mCurrentTs; } } void MeasurementsPreprocessor::operator()(const SensorMeasurementData& measurement) { mSensor.data = measurement; mSensor.ts = mCurrentTs; } void MeasurementsPreprocessor::operator()(const NmeaMeasurementData& measurement) { mNmea.data = measurement; mNmea.ts = mCurrentTs; } RadioMeasurementsData MeasurementsPreprocessor::extractRadioMeasurements(long long lastStepTime) { RadioMeasurementsData radioMeasurements = mRadiosBuffer.extractMeasurements(lastStepTime); if (mUseClosestAps) { std::sort(radioMeasurements.rbegin(), radioMeasurements.rend(), [](const RadioMeasurementData& lhs, const RadioMeasurementData& rhs) { return lhs.rssi < rhs.rssi; }); if (radioMeasurements.size() > (unsigned int) mNumClosestAps) radioMeasurements.resize(mNumClosestAps); } for (auto& msr : radioMeasurements) { if (msr.type != RadioMeasurementData::Type::WIFI_RTT) { boost::optional levelId = mLevelIndex->findLevelByTransmitterId(msr.id); if (levelId.is_initialized()) { const Level& level = mLevelIndex->level(levelId.get()); Transmitter transmitter = level.transmitter(msr.id); const PathlossModel pathLossModel = transmitter.pathlossModel; msr.distance = std::exp((pathLossModel.A - msr.rssi) / pathLossModel.B); } continue; } msr.distance -= mWiFiRttOffset; } return radioMeasurements; } long long MeasurementsPreprocessor::getCurrentTs() const { return mCurrentTs; } long long MeasurementsPreprocessor::getLastSignalTs() const { return mLastSignalTs; } boost::optional MeasurementsPreprocessor::getValidSignalEntry( const RadioMeasurementData& entry) const { bool isTransmitterValid = mUseUnknownTxs || (mLevelIndex->findLevelByTransmitterId(entry.id).is_initialized() && isTransmitterSupported(entry.id)); if (mMeasurementType != CommonSettings::MeasurementType::RSSI && entry.type != RadioMeasurementData::Type::WIFI_RTT) isTransmitterValid = false; if (mMeasurementType == CommonSettings::MeasurementType::RSSI && !isRssiValid(entry.rssi)) { isTransmitterValid = false; } if (isTransmitterValid && isSignalTypeSupported(entry.type)) return entry; else return boost::none; } bool MeasurementsPreprocessor::isSignalTypeSupported(RadioMeasurementData::Type signalType) const { return (signalType == RadioMeasurementData::Type::WIFI && mUseWifi) || (signalType == RadioMeasurementData::Type::BEACON && mUseBle) || (signalType == RadioMeasurementData::Type::BLUETOOTH && mUseBle) || (signalType == RadioMeasurementData::Type::EDDYSTONE && mUseBle) || (signalType == RadioMeasurementData::Type::WIFI_RTT && mUseWifi); } bool MeasurementsPreprocessor::isRssiValid(double rssi) const { return rssi < 0.0 && rssi > mCutOffRssi; } bool MeasurementsPreprocessor::isTransmitterSupported(const TransmitterId& transmitterId) const { boost::optional levelId = mLevelIndex->findLevelByTransmitterId(transmitterId); if (!levelId.is_initialized()) return false; if (!mLevelIndex->hasLevel(levelId.get())) return false; const Level& level = mLevelIndex->level(levelId.get()); if (!level.containsTransmitter(transmitterId)) return false; const Transmitter& transmitter = level.transmitter(transmitterId); return mIsTracking ? transmitter.type == TransmitterType::LOCATOR : transmitter.type != TransmitterType::LOCATOR; } RadioMeasurementBuffer::RadioMeasurementBuffer( long long radioKeepPeriodMs, long long sigWindowShiftMs, long long stopDetectionTime, double rssiBias, bool useStops) : mRadioKeepPeriodMs(radioKeepPeriodMs) , mSignalWindowShiftMs(sigWindowShiftMs) , mStopDetectionTimeMs(stopDetectionTime) , mRssiBias(rssiBias) , mUseStops(useStops) , mLastExtractionTs(0) , mCurrentTs(0) , mLastStepTs(0) { } void RadioMeasurementBuffer::addMeasurement(const RadioMeasurement& msr) { if (mCurrentTs == 0) { mCurrentTs = msr.ts; mLastExtractionTs = msr.ts; } RadioMeasurement radioMsr = msr; radioMsr.data.rssi = msr.data.rssi + mRssiBias; mMeasurements.push_back(radioMsr); mCurrentTs = msr.ts; } RadioMeasurementsData RadioMeasurementBuffer::extractMeasurements(long long lastStepTs) { RadioMeasurementsData measurements; if (lastStepTs > 0) mLastStepTs = lastStepTs; if (mCurrentTs - mLastExtractionTs < mSignalWindowShiftMs) { // if not enough time passed return measurements; } mLastExtractionTs = mCurrentTs; // Averaging measurements from same Transmitters std::sort(mMeasurements.begin(), mMeasurements.end(), [](const RadioMeasurement& lhs, const RadioMeasurement& rhs) { return lhs.data.id < rhs.data.id; }); auto sameIdFirst = mMeasurements.cbegin(); while (sameIdFirst != mMeasurements.cend()) { auto sameIdLast = std::upper_bound(sameIdFirst, mMeasurements.cend(), *sameIdFirst, [](const RadioMeasurement& lhs, const RadioMeasurement& rhs) { return lhs.data.id < rhs.data.id; }); double nSignals = std::distance(sameIdFirst, sameIdLast); double sumRssi = std::accumulate(sameIdFirst, sameIdLast, 0.0, [](double sum, const RadioMeasurement& m) { return sum + m.data.rssi; }); double averRssi = sumRssi / nSignals; double sumDistances = std::accumulate(sameIdFirst, sameIdLast, 0.0, [](double sum, const RadioMeasurement& m) { return sum + m.data.distance; }); double averDistance = sumDistances / nSignals; RadioMeasurement radioMsr = *sameIdFirst; radioMsr.data.rssi = averRssi; radioMsr.data.distance = averDistance; measurements.push_back(radioMsr.data); sameIdFirst = sameIdLast; } // In case we are standing still we don't erase old measurements if (mUseStops & (mLastStepTs > 0) && (mCurrentTs - mLastStepTs > mStopDetectionTimeMs)) return measurements; // Erasing old measurements if (mRadioKeepPeriodMs == 0) { mMeasurements.clear(); } else { long long keepFromTs = mCurrentTs - mRadioKeepPeriodMs; std::sort(mMeasurements.begin(), mMeasurements.end(), [](const RadioMeasurement& lhs, const RadioMeasurement& rhs) { return lhs.ts < rhs.ts; }); auto eraseUpperBound = std::find_if(mMeasurements.begin(), mMeasurements.end(), [keepFromTs](const RadioMeasurement& rm) { return rm.ts >= keepFromTs; }); mMeasurements.erase(mMeasurements.begin(), eraseUpperBound); } return measurements; } SensorMeasurement MeasurementsPreprocessor::getValidSensor() const { return mSensor; } NmeaMeasurement MeasurementsPreprocessor::getCurrentNmea() const { return mNmea; } #if defined (DEBUG_OUTPUT_MSR_PREPROCESSOR) void clearDebugOutput() { cout << "print debug output to " << PREPROCESSED_MSR_DEBUG_OUTPUT_FILE << endl; std::ofstream debugOutputFile; debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE); debugOutputFile << "type id ts rssi" << endl; debugOutputFile.close(); } void printInputSignals(NavigationMessage* navMsg, RadioMeasurementsData& entries) { ofstream debugOutputFile; debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE, ofstream::out | ofstream::app); for (RadioMeasurement& entry: entries) { long long wsTsSec = navMsg->tmUnixTime; long long wsTsMs = wsTsSec * 1000 - entry.offset; debugOutputFile << "INPUT "<< entry.bssid << " " << wsTsMs << " " << entry.data.rssi << endl; } debugOutputFile.close(); } void printExtractedMeasurements(RadioMeasurementsData& radioMeasurements) { ofstream debugOutputFile; debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE, ofstream::out | ofstream::app); for (RadioMeasurement msr: radioMeasurements) { debugOutputFile << "OUTPUT "<< msr.data.id << " " << msr.ts << " " << msr.data.rssi << endl; } debugOutputFile.close(); } #endif } } // namespace navigine::navigation_core ================================================ FILE: src/measurements/measurement_preprocessor.h ================================================ #ifndef MEASUREMENT_PREPROCESSOR_H #define MEASUREMENT_PREPROCESSOR_H #include #include #include #include #include #include #include #include #include "../measurement_types.h" namespace navigine { namespace navigation_core { class RadioMeasurementBuffer { public: RadioMeasurementBuffer(const RadioMeasurementBuffer&) = default; RadioMeasurementBuffer(long long radioKeepPeriodMs, long long sigWindowShiftMs, long long stopDetectionTime, double rssiBias, bool useStops); RadioMeasurementsData extractMeasurements(long long lastStepTime); void addMeasurement(const RadioMeasurement& msr); private: const long long mRadioKeepPeriodMs; const long long mSignalWindowShiftMs; const long long mStopDetectionTimeMs; const double mRssiBias; const bool mUseStops; long long mLastExtractionTs; long long mCurrentTs; long long mLastStepTs; std::vector mMeasurements = {}; }; class MeasurementsPreprocessor: public boost::static_visitor<> { public: MeasurementsPreprocessor( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); void updateMeasurements(const Measurement& navMsg); long long getCurrentTs() const; long long getLastSignalTs() const; RadioMeasurementsData extractRadioMeasurements(long long lastStepTime); SensorMeasurement getValidSensor() const; NmeaMeasurement getCurrentNmea() const; void operator()(const RadioMeasurementData& measurement); void operator()(const SensorMeasurementData& measurement); void operator()(const NmeaMeasurementData& measurement); private: boost::optional getValidSignalEntry(const RadioMeasurementData& entry) const; bool isSignalTypeSupported(RadioMeasurementData::Type signalType) const; bool isRssiValid(double rssi) const; std::pair transmitterLevelId(const TransmitterId& transmitterId) const; bool isTransmitterSupported(const TransmitterId& transmitterId) const; private: const std::shared_ptr mLevelIndex; const double mWiFiRttOffset; const double mCutOffRssi; const bool mUseClosestAps; const int mNumClosestAps; const bool mUseUnknownTxs; const CommonSettings::MeasurementType mMeasurementType; const bool mUseWifi; const bool mUseBle; const bool mIsTracking; long long mLastSignalTs; RadioMeasurementBuffer mRadiosBuffer; long long mCurrentTs; SensorMeasurement mSensor; NmeaMeasurement mNmea; }; } } // namespace navigine::navigation_core #endif // MEASUREMENT_PREPROCESSOR_H ================================================ FILE: src/navigation_client_impl.cpp ================================================ #include #include #include "navigation_client_impl.h" #include "position_estimator/position_estimator_outdoor.h" #include "position_estimator/position_estimator_zone.h" #include "position_estimator/position_estimator_knn.h" #include "level_estimator/level_estimator_radiomap.h" #include "level_estimator/level_estimator_transmitters.h" #include "trilateration.h" namespace navigine { namespace navigation_core { NavigationClientImpl::NavigationClientImpl( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) : mUseEnuAzimuth(navProps.commonSettings.useEnuAzimuth) , mNoSignalTimeThreshold((long long)(1000 * navProps.commonSettings.noSignalTimeThreshold)) , mNoActionTimeThreshold((long long)(1000 * navProps.commonSettings.noActionTimeThreshold)) , mLevelIndex(levelCollector) , mLevelEstimator(createLevelEstimator(levelCollector, navProps)) , mPositionEstimatorIndoor(createPostitionEstimator(levelCollector, navProps)) , mMsrPreprocessor(std::make_unique(levelCollector, navProps)) , mPositionEstimatorOutdoor(std::make_unique(levelCollector, navProps)) , mPositionPostprocessor(std::make_unique(navProps)) , mSensorFusion(std::make_unique(navProps)) , mFlagIndoorPos(false) , mPrevFusedPosHeading(boost::none) , mPrevGyroHeading(boost::none) , mUseAltitude(navProps.commonSettings.useAltitude) { } std::vector NavigationClientImpl::navigate(const std::vector& navBatchInput) { std::vector navBatchOutput; if (navBatchInput.size() == 0) return navBatchOutput; mNavigationStates.clear(); long long prevTs = navBatchInput[0].ts; for (const auto &navInput: navBatchInput) { if (prevTs > navInput.ts) { std::cerr << "prevTs: " << prevTs << ", t.ts: " << navInput.ts << std::endl; continue; } else { prevTs = navInput.ts; } NavigationStatus retStatus = NavigationStatus::OK; mMsrPreprocessor->updateMeasurements(navInput); const long long curTs = mMsrPreprocessor->getCurrentTs(); const long long lastSignalTs = mMsrPreprocessor->getLastSignalTs(); const SensorMeasurement sensorMsr = mMsrPreprocessor->getValidSensor(); const NmeaMeasurement nmeaMsr = mMsrPreprocessor->getCurrentNmea(); MotionInfo motionInfo = mSensorFusion->calculateDisplacement(sensorMsr, curTs); const auto radioMsr = mMsrPreprocessor->extractRadioMeasurements(motionInfo.lastMotionTs); LevelId levelId = mLevelEstimator->calculateLevel(radioMsr, sensorMsr); const Position outdoorPos = mPositionEstimatorOutdoor->calculatePosition(curTs, sensorMsr, nmeaMsr, mFlagIndoorPos); mFlagIndoorPos = false; // TODO refactoring needed if (curTs - lastSignalTs >= mNoSignalTimeThreshold) { mPositionEstimatorIndoor->reInit(); levelId = LevelId(); } if (curTs - lastSignalTs >= mNoActionTimeThreshold && curTs - motionInfo.lastMotionTs >= mNoActionTimeThreshold && motionInfo.lastMotionTs > 0) { mPositionEstimatorIndoor->reInit(); levelId = LevelId(); } if (mLevelIndex->hasLevel(levelId)) { const Level& level = mLevelIndex->level(levelId); const Position indoorPos = mPositionEstimatorIndoor->calculatePosition(level, curTs, radioMsr, motionInfo, retStatus); if (!indoorPos.isEmpty) mFlagIndoorPos = true; const Position fusedPos = mPositionPostprocessor->fusePositions(curTs, indoorPos, outdoorPos, retStatus); if (!fusedPos.isEmpty && mPrevFusedPosHeading.is_initialized() && (fusedPos.heading != mPrevFusedPosHeading.get())) { mPrevGyroHeading = motionInfo.gyroHeading; motionInfo.isAzimuthValid = false; } else if (mPrevGyroHeading.is_initialized()) { double deltaGyroHeading = motionInfo.gyroHeading - mPrevGyroHeading.get(); motionInfo.azimuth = - (mPrevFusedPosHeading.get() - deltaGyroHeading - M_PI_2); // NED motionInfo.isAzimuthValid = true; } if (!fusedPos.isEmpty) mPrevFusedPosHeading = fusedPos.heading; mPosition = mPositionPostprocessor->getProcessedPosition(fusedPos, curTs, motionInfo, level); if (!motionInfo.isAzimuthValid) { motionInfo.azimuth = - (fusedPos.heading - M_PI_2); // convert from ENU to NED } if (mUseEnuAzimuth) motionInfo.azimuth = -motionInfo.azimuth + M_PI_2; // convert from NED to ENU } else if (!outdoorPos.isEmpty) { mPosition = outdoorPos; } else { retStatus = NavigationStatus::NO_LEVEL; } // Filling navigationSate for Debug auto navigationState = mPositionEstimatorIndoor->getParticleFilterState(); navigationState.setStepCounter(motionInfo.stepCounter); navigationState.setStepLen(motionInfo.distance); navigationState.setIndoorPosition(XYPoint(mPositionPostprocessor->getIndoorPosition().x, mPositionPostprocessor->getIndoorPosition().y)); navigationState.setOutdoorPosition(XYPoint(mPositionPostprocessor->getOutdoorPosition().x, mPositionPostprocessor->getOutdoorPosition().y)); mNavigationStates.push_back(navigationState); // Filling navOutput NavigationOutput navOutput; navOutput.status = retStatus; // TODO: optimize NavClient state variables (mPosition, mRetStatus, etc.) // https://redmine.navigine.com/issues/2554 if (mPosition.isEmpty) { if (retStatus == NavigationStatus::OK) { navOutput.status = NavigationStatus::NAVIGATION_ERROR; //TODO ret rid of empty position status } navBatchOutput.emplace_back(navOutput); continue; } else { navOutput.status = NavigationStatus::OK; } navOutput.posLevel = mPosition.levelId; if (navOutput.posLevel.isValid()) { const Level& level = mLevelIndex->level(navOutput.posLevel); const GeoPoint gpsPosition = localToGps(XYPoint(mPosition.x, mPosition.y), level.bindingPoint()); navOutput.posLatitude = gpsPosition.latitude; navOutput.posLongitude = gpsPosition.longitude; if (mUseAltitude) { Trilateration altitudeWorker; boost::optional altitude = altitudeWorker.calculateAltitude(level, radioMsr); if (altitude.is_initialized()) navOutput.posAltitude = altitude.get(); } } navOutput.posR = mPosition.deviationM; navOutput.provider = mPosition.provider; navOutput.posOrientation = to_minus_Pi_Pi(motionInfo.azimuth); navBatchOutput.emplace_back(navOutput); } return navBatchOutput; } std::unique_ptr NavigationClientImpl::createLevelEstimator( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) { const bool useRadiomap = navProps.commonSettings.useRadiomap; // the following three lines are used for navigation.xml backcompatibility const CommonSettings::UseAlgorithm algName = navProps.commonSettings.useAlgorithm; const bool useKnn = (algName == CommonSettings::UseAlgorithm::KNN); if (useRadiomap || useKnn) return std::make_unique(levelCollector, navProps); return std::make_unique(levelCollector, navProps); } std::unique_ptr NavigationClientImpl::createPostitionEstimator( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) { const CommonSettings::UseAlgorithm algName = navProps.commonSettings.useAlgorithm; bool usePDR; if (algName == CommonSettings::UseAlgorithm::KNN) return std::make_unique(levelCollector, navProps); return std::make_unique(levelCollector, navProps); } std::vector NavigationClientImpl::getStates() const { return mNavigationStates; } std::shared_ptr createNavigationClient( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) { return std::make_shared(levelCollector, navProps); } } } // namespace navigine::navigation_core ================================================ FILE: src/navigation_client_impl.h ================================================ #pragma once #include #include #include #include #include #include #include "measurements/measurement_preprocessor.h" #include "position_estimator/position_estimator.h" #include "position_estimator/position_estimator_outdoor.h" #include "position_postprocessor/position_postprocessor.h" #include "sensor_fusion/sensor_fusion.h" #include "level_estimator/level_estimator.h" namespace navigine { namespace navigation_core { class NavigationClientImpl : public NavigationClient { public: NavigationClientImpl( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); // Return navigation state of this client std::vector getStates() const override; // Main navigation function. Calculates current position based on incoming measurements std::vector navigate(const std::vector& navBatchInput) override; private: NavigationClientImpl(const NavigationClientImpl&) = delete; void operator=(const NavigationClientImpl&) = delete; std::unique_ptr createLevelEstimator( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); std::unique_ptr createPostitionEstimator( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); private: const bool mUseEnuAzimuth; const long long mNoSignalTimeThreshold; const long long mNoActionTimeThreshold; std::shared_ptr mLevelIndex; std::unique_ptr mLevelEstimator; std::unique_ptr mPositionEstimatorIndoor; std::unique_ptr mMsrPreprocessor; std::unique_ptr mPositionEstimatorOutdoor; std::unique_ptr mPositionPostprocessor; std::unique_ptr mSensorFusion; std::vector mNavigationStates = {}; Position mPosition = {}; bool mFlagIndoorPos; boost::optional mPrevFusedPosHeading; boost::optional mPrevGyroHeading; const bool mUseAltitude; }; } } // namespace navigine::navigation_core ================================================ FILE: src/navigation_error_codes.h ================================================ /** navigation_error_codes.h * * Copyright (c) 2017 Navigine. * */ #ifndef NAVIGINE_NAVIGATION_ERROR_CODES_H #define NAVIGINE_NAVIGATION_ERROR_CODES_H namespace navigine { namespace navigation_core { static const int ERROR_NO_SOLUTION = 4; static const int MISSING_STRING_CONFIG = 5; static const int MISSING_LEVELS = 6; static const int NO_PARAMETERS_IN_JSON_FILE = 21; static const int ERROR_OPEN_FILE = 25; static const int INVALID_LOC_ID_IN_JSON_FILE = 26; static const int INVALID_PARAMETER_IN_JSON_FILE = 27; } } // namespace navigine::navigation_core #endif // NAVIGINE_NAVIGATION_ERROR_CODES_H ================================================ FILE: src/point.cpp ================================================ /** point.cpp * * Copyright (c) 2019 Navigine. * */ #include #include namespace navigine { namespace navigation_core { constexpr double EQUATOR_CIRCUMFERENCE_METERS = 40075160.0; constexpr double POLE_CIRCUMFERENCE_METERS = 40008000.0; // Normalize longitude to (-180, 180] double normalizeLongitude(double lon) { if (lon > 180) { lon -= 360; } else if (lon <= -180) { lon += 360; } return lon; } double degToRad(double deg) { return deg * M_PI / 180.0; } XYPoint gpsToLocal(const GeoPoint& point, const GeoPoint& bindPoint) { const double deltaLat = point.latitude - bindPoint.latitude; const double deltaLon = normalizeLongitude(point.longitude - bindPoint.longitude); const double latitudeCircumference = EQUATOR_CIRCUMFERENCE_METERS * std::cos(degToRad(bindPoint.latitude)); XYPoint localPoint; localPoint.x = deltaLon * (latitudeCircumference / 360.0); localPoint.y = deltaLat * (POLE_CIRCUMFERENCE_METERS / 360.0); return localPoint; } GeoPoint localToGps(const XYPoint& localPoint, const GeoPoint& bindPoint) { const double latitudeCircumference = EQUATOR_CIRCUMFERENCE_METERS * std::cos(degToRad(bindPoint.latitude)); const double lat = (localPoint.y * (360.0 / POLE_CIRCUMFERENCE_METERS)) + bindPoint.latitude; const double lon = normalizeLongitude(localPoint.x * 360.0 / latitudeCircumference + bindPoint.longitude); return GeoPoint(lat, lon); } } } // namespace navigine::navigation_core ================================================ FILE: src/position.h ================================================ /** position.h * * Copyright (c) 2018 Navigine. * */ #ifndef NAVIGINE_POSITION_H #define NAVIGINE_POSITION_H #include #include #include #include namespace navigine { namespace navigation_core { struct Position : XYPoint { Position() : XYPoint(0.0, 0.0) , levelId(LevelId("")) , deviationM(0) , ts(0) , isEmpty(true) , provider(Provider::NONE) , heading(0.0) { } Position(double _x, double _y, LevelId _levelId, double _deviationM, long long _ts, bool _isEmpty, const Provider& _provider, double _heading) : XYPoint(_x, _y) , levelId(_levelId) , deviationM(_deviationM) , ts(_ts) , isEmpty(_isEmpty) , provider(_provider) , heading(_heading) { } LevelId levelId; double deviationM; long long ts; bool isEmpty = true; //TODO use boost optional for empty? Provider provider = Provider::NONE; double heading; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_POSITION_H ================================================ FILE: src/position_estimator/Readme.md ================================================ ================================================ FILE: src/position_estimator/docs/KNN.md ================================================ # K-Nearest Neighbours Positioning Algorithm Note:: The algorithm requires special radiomap data measurement that can beperformed using procedure called Radiomap Measurement. For detailed information of data collection for algorithm usage refer to [https://docs.navigine.com/Measuring_Radiomap](https://docs.navigine.com/Measuring_Radiomap) Radiomap Measurement:: measuring the target location's radiomap on foot via an Android device with the [Navigine Application](https://github.com/Navigine/Indoor-Navigation-Android-Mobile-SDK-2.0) installed. *Requirements*: * infrastructure deployed * maps (locations) implemented * [radiomap data](https://docs.navigine.com/Measuring_Radiomap) collected *Our notation*: * RSSI * KNN * transmismitter * beacon * BLE * radio map ## Measurements & context Algorithm is based on an RSS mapping technique. For the set of locations in an observation area with an apriory known positions we record RSSI measurements. All observations from multiple positions we call a radio map. From radio map we can estimate user position by measuring similarity between observations and observations measured from a known prior location. The algorithms implies averaging on a radio map data, measuring and processing incoming RSSI signals from transmiters to receiver, and a signal propagation model, used for localization. ### What means **access point** is KNN positioning context We measure incoming RSSI in several points of building for a long enough time. After several minutes of collecting the signal, we perform averaging of signals. After that we have a precise measurements for a single point. Decent radio-map consists of several access points mapped to a building map. The "access point" is the landmark, point with known coordinates and a history of observations from each beacon in visible range. ### KNN logfile and map contents ![radio-map example](img/3-10-2021-21-50-31-PM.png) On figure above we see GeoJSON visualization containing some access points (circles). The structure of data used for this algorithm is as follows: ```json { "type": "Feature", "properties": { "name": null, "type": "reference_point", "level": "5179", "id": "1", "uuid": "eff14dbc-b33a-11e8-945d-7c7a91af7e27", "entries": [ { "value": [ { "rssi": 83, "count": 6 }, { ... }, ], "bssid": "20:18:00:00:04:32", "type": "WIFI" }, { ... } ] }, "geometry": { "type": "Point", "coordinates": [ -1.6307104042, 47.225793733499998 ] } } ``` For each entity of "type": "reference_point" we have many observations for each detected beacon in area. Approximately we can say: The example log will consist of 20-100 access point locations, 10000 RSSI observations from 20-50 detected beacons for a single location. We have set of reference points, for each of them RSSI data from closest beacons in area are measured. This is the starting point of an algorithm. ## KNN algorithm setup The settings we use for program configuration: ```json "Use_algorithm": "KNN", "Use_triangles": true, "Min_num_of_measurements_for_position_calculation": 6, "Use_diff_mode": true, "K": 3, ``` We have two mode options for this algorithm called tringle and differential modes respectively. ## The algorithm ### KNN algorithm math and general concept KNN approach is based on a radio-channel propagation model:

RSS(d) = RSS(d_0)-10\mu \log \frac{d}{d_0} + w

The goal of an RSS-based localization algorithm is to provide estimate \hat p = (\hat x, \hat y) of position p given the vector [RSS_1, RSS_2, . . . , RSS_n]. There exist several algorithms that can be used to determine the position of a target through RSS measurements: Some of them are geometric methods, such as lateration or a minimum—maximum method (min—max), whereas some others are based on statistical approaches, such as maximum likelihood. The algorithms for localization can be classified as either statistical of geometrical. When the target node communicates with all the anchors, i.e., all anchors are visible, the centroid results the center of the anchors coordinates. If we assume that all anchors we record were in the line of sight, or equally we asseme that there is no signal reflections in the model. If we have to consider noise from reflections and reflections model, the algorithm will be much more complicated. For simplicity, we work with a classical log-linear signal propagation model. Anchors - beacons transmitting the signal.

\hat p = \frac{1}{m}\cdot \sum_{i=1}^m {a_i}

If we have m different anchor points in our collected data - cardinality of visible subset. (under assumption that all the visible anchors are equally close to target node). ### Implementation #### Initialization step * collect measurements * construct building radio map Firstly we organise the recorded data, write set of detected anchors to their respective sublocations. We map radiomap data to each detected beacon(*reverse mapping* of radiomap data). Thus later during localization process, we may get closest reference points from an observed set of beacons at a zero computational cost. #### RP weights estimation step * get measurements * get reference points for the respective location * filter outliers * stop until enough measurements collected * calculate weights of RPs based on RSS measurements and building radio map * calculate position based on known RPs weights and positions (see below) We select the level / sublocation for localization. For sublocation we select: map, known reference points and a set of detected beacons. From beacon to radio data map we obtain positions of most similar reference points. We assume that reference points with the most similar signal are also the closest one, because of our signal prpagation model. #### Position calculation The pose estimation from weighted set of closest reference points can be performed in different ways: geometrically, statistically, etc. Based on our settings, we have two options: ##### Weighted centroid approach The position is the weighted mean of closest reference points with respect to their likelihood / signal similarity.

\hat x = \sum_{i=1}^{n}{x_i \cdot w_i};\\ \hat y = \sum_{i=1}^{n}{y_i \cdot w_i}

Where n is the cardinality of closest reference points set. ##### Triangulation * Triangulate our reference points set. * Select triange with maximum cumulative weight: w_i = w_1 + w_2 + w_3 From 3 closest reference points we select the triangle; Based on our signal model we calculate the position.

\hat x = \sum_{i=1}^{3}{x_i \cdot w_i};\\ \hat y = \sum_{i=1}^{3}{y_i \cdot w_i}

* Pros:: direct calclation with low number of reference points required. Robust solution. * Cons:: we don't consider information from all other reference points, our solution can be geometrically wrong and we have no options to tune the solution. ##### Discussion The effective number of nearest neighbours needed for lacation estimation can be different. For some cases we want to choose from 3 optimal closest points, for some cases we prefer to have averaging. All logic comes from observations, noise, location properties and optimization procedure. If during the measurement procedure, average visible number of beacons for each access points was high, we can possibly improve our accuracy. And if there's multiple signal reflections, we may want to localize only on 3 most strong signals and rejecting all noisy information. ## Preview & examples Demo video for KNN in triangulation mode: https://user-images.githubusercontent.com/68880242/139697439-7cffb9a0-e8cf-4f52-8ba3-f40350acbd14.mov ================================================ FILE: src/position_estimator/position_estimator.h ================================================ #ifndef POSITION_ESTIMATOR_H #define POSITION_ESTIMATOR_H #include #include #include #include #include #include #include "../position.h" namespace navigine { namespace navigation_core { class PositionEstimator { public: virtual ~PositionEstimator() = default; PositionEstimator(const std::shared_ptr& levelCollector) : mLevelIndex(levelCollector) , mPosition(Position()) , mParticleFilterState(NavigationState()) { } virtual Position calculatePosition( const Level& level, long long ts, const RadioMeasurementsData& radioMsr, const MotionInfo& motionInfo, NavigationStatus& retStatus) = 0; virtual void reInit() {}; NavigationState getParticleFilterState() const { return mParticleFilterState; } protected: std::shared_ptr mLevelIndex; Position mPosition; NavigationState mParticleFilterState; }; } } // namespace navigine::navigation_core #endif // POSITION_ESTIMATOR_H ================================================ FILE: src/position_estimator/position_estimator_knn.cpp ================================================ /** position_estimator_knn.cpp * * Copyright (c) 2018 Navigine. * */ #include #include #include #include #include "position_estimator_knn.h" namespace navigine { namespace navigation_core { #ifdef NAVIGATION_VISUALIZATION std::map PositionEstimatorKnn::debugRefPointWeights = std::map(); #endif PositionEstimatorKnn::PositionEstimatorKnn( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) : PositionEstimator(levelCollector) , mUseTriangles(navProps.commonSettings.useTriangles) , mUseDiffMode(navProps.commonSettings.useDiffMode) , mK(navProps.commonSettings.kNeighbors) , mMinMsrNumForPositionCalculation(navProps.commonSettings.minNumOfMeasurementsForPositionCalculation) , mLevelTriangles(triangulate(levelCollector)) , mLevelReferenceTransmittersMap(getLevelReferenceTransmittersMap(levelCollector)) , mLevelsRadiomaps(getLevelsRadiomaps(levelCollector)) { } Position PositionEstimatorKnn::calculatePosition( const Level& level, long long ts, const RadioMeasurementsData& radioMsr, const MotionInfo&, NavigationStatus &retStatus) { if (radioMsr.empty()) { retStatus = NavigationStatus::NO_SOLUTION; return mPosition; } retStatus = NavigationStatus::OK; const std::set& beaconsIds = mLevelReferenceTransmittersMap.at(level.id()); auto validMsr = getWhitelistedMeasurements(beaconsIds, radioMsr); if (validMsr.size() < mMinMsrNumForPositionCalculation) { retStatus = NavigationStatus::NO_SOLUTION; return mPosition; } const Radiomap& refPointsMap = level.radiomap(); const std::multimap>& signalMap = mLevelsRadiomaps.at(level.id()); std::map rpToWeight = calcRpWeightsKnn(signalMap, validMsr, mUseDiffMode); #ifdef NAVIGATION_VISUALIZATION std::map weights; for (std::map::iterator it = rpToWeight.begin(); it != rpToWeight.end(); it++) weights.insert(*it); debugRefPointWeights = weights; #endif XYPoint p = XYPoint(0.0, 0.0); if (mUseTriangles) { if (mLevelTriangles.find(level.id()) == mLevelTriangles.end()) { retStatus = NavigationStatus::NAVIGATION_ERROR; return mPosition; } const std::vector& triangles = mLevelTriangles.find(level.id())->second; if (triangles.empty()) { retStatus = NavigationStatus::NO_RPS; return mPosition; } p = calcPositionInTriangle(triangles, refPointsMap, rpToWeight); } else { p = calcKHeaviestRefPointsAverage(refPointsMap, rpToWeight, mK); } bool isEmpty = false; double precision = 1.0; //TO FIX: calculate position accuracy mPosition = Position(p.x, p.y, level.id(), precision, ts, isEmpty, Provider::INDOOR, 0.0); return mPosition; } } } // namespace navigine::navigation_core ================================================ FILE: src/position_estimator/position_estimator_knn.h ================================================ /** position_estimator_knn.h * * Copyright (c) 2018 Navigine. * */ #ifndef POSITION_ESTIMATOR_KNN_H #define POSITION_ESTIMATOR_KNN_H #include #include "position_estimator.h" #include "../knn/knn_utils.h" namespace navigine { namespace navigation_core { class PositionEstimatorKnn: public PositionEstimator { public: PositionEstimatorKnn( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); Position calculatePosition( const Level& level, long long ts, const RadioMeasurementsData& radioMsr, const MotionInfo& motionInfo, NavigationStatus& retStatus) override; #ifdef NAVIGATION_VISUALIZATION static std::map debugRefPointWeights; #endif private: const bool mUseTriangles; const bool mUseDiffMode; const size_t mK; const size_t mMinMsrNumForPositionCalculation; const std::map> mLevelTriangles; const std::map> mLevelReferenceTransmittersMap; const std::map>> mLevelsRadiomaps; }; } } // namespace navigine::navigation_core #endif // POSITION_ESTIMATOR_KNN_H ================================================ FILE: src/position_estimator/position_estimator_outdoor.cpp ================================================ /** position_estimator_outdoor.cpp * * Copyright (c) 2018 Navigine. * */ #include #include "position_estimator_outdoor.h" namespace navigine { namespace navigation_core { bool SigmaFilter::add(const XYPoint& point) { bool pntValid = false; if (mGNSSbuffer.size() == mSigmaWindow) { double sumX = 0; double sumY = 0; for (auto const& pnt : mGNSSbuffer) { sumX += pnt.x; sumY += pnt.y; } double meanX = sumX / mSigmaWindow; double meanY = sumY / mSigmaWindow; double accumX = 0.0; double accumY = 0.0; std::for_each(std::begin(mGNSSbuffer), std::end(mGNSSbuffer), [&](auto const d) { accumX += (d.x - meanX) * (d.x - meanX); accumY += (d.y - meanY) * (d.y - meanY); }); auto deviationX = 3.0 * std::sqrt(accumX / mSigmaWindow) + mSigmaDelta; auto deviationY = 3.0 * std::sqrt(accumY / mSigmaWindow) + mSigmaDelta; if (std::abs(meanX - point.x) <= deviationX && std::abs(meanY - point.y) <= deviationY) pntValid = true; mGNSSbuffer.pop_front(); } mGNSSbuffer.push_back(point); return pntValid; } void SigmaFilter::clear() { mGNSSbuffer.clear(); } PositionEstimatorOutdoor::PositionEstimatorOutdoor( const std::shared_ptr& levelCollector, const NavigationSettings& navProps) : mLevelIndex (levelCollector) , mSigmaFilter(navProps.commonSettings.sigmaFilterWindow, navProps.commonSettings.sigmaFilterDelta) , mGpsValidTimeWindowSec(navProps.commonSettings.gpsValidTimeWindowSec) , mUseGpsOutsideMap(navProps.commonSettings.useGpsOutsideMap) , mUseSigmaFilter(navProps.commonSettings.useGpsSigmaFilter) , mMinNumSats(navProps.commonSettings.minNumSats) , mMinGpsDeviation(navProps.commonSettings.minGpsDeviation) , mMaxGpsDeviation(navProps.commonSettings.maxGpsDeviation) { } GpsPosition PositionEstimatorOutdoor::extractGpsPosition(long long ts, const SensorMeasurement& sensorMsr) { if (sensorMsr.data.type == SensorMeasurementData::Type::LOCATION) { double lat = sensorMsr.data.values.x; double lng = sensorMsr.data.values.y; double dev = std::max(sensorMsr.data.values.z, mMinGpsDeviation); GpsPosition pos = GpsPosition(lat, lng, dev, ts); return pos; } else { return GpsPosition(); } } int PositionEstimatorOutdoor::extractNumberOfSatellites(const NmeaMeasurement& nmeaEntry) { int totalNumSats = 0; if ((nmeaEntry.data.sentenceNumber == 1) && (nmeaEntry.data.satellitesNumber != 0)) totalNumSats += nmeaEntry.data.satellitesNumber; return totalNumSats; } Position PositionEstimatorOutdoor::calculatePosition( long long ts, const SensorMeasurement& sensorEntry, const NmeaMeasurement& nmeaEntry, const bool flagIndoorPos) { GpsPosition gpsPos = extractGpsPosition(ts, sensorEntry); int totalNumSats = extractNumberOfSatellites(nmeaEntry); if (flagIndoorPos || (gpsPos.deviation > mMaxGpsDeviation)) { mSigmaFilter.clear(); //If there is indoor solution then clear sigma filter GNSS buffer. } if (gpsPos.isEmpty) { double timeSinceLastGpsPosition = (ts - mLastGpsPosition.ts) / 1000.0; if (timeSinceLastGpsPosition > mGpsValidTimeWindowSec) return Position(); //TODO error code: gps is empty! else return mLastGpsPosition; } //TODO find map closest to previous position (at least check if inside)!!!! for (const Level& level: mLevelIndex->levels()) { XYPoint point = gpsToLocal(GeoPoint(gpsPos.latitude, gpsPos.longitude), level.bindingPoint()); if ((!std::isnan(point.x) && !std::isnan(point.y) && boost::geometry::covered_by(Point(point.x, point.y), level.geometry().boundingBox())) || mUseGpsOutsideMap) { Position outPos; outPos.ts = gpsPos.ts; outPos.x = point.x; outPos.y = point.y; outPos.levelId = level.id(); outPos.isEmpty = false; outPos.deviationM = gpsPos.deviation; outPos.provider = Provider::GNSS; bool isValid = true; if (outPos.ts != mLastGpsPosition.ts) { if (mUseSigmaFilter && !mSigmaFilter.add(point)) isValid = false; } if (!isValid || (totalNumSats != 0 && totalNumSats < mMinNumSats)) return Position(); mLastGpsPosition = outPos; return outPos; } } return Position(); //TODO error code: "can not find map for gps position!" } } } // namespace navigine::navigation_core ================================================ FILE: src/position_estimator/position_estimator_outdoor.h ================================================ /** position_estimator_outdoor.h * * Copyright (c) 2018 Navigine. * */ #ifndef NAVIGINE_POSITION_ESTIMATOR_OUTDOOR_H #define NAVIGINE_POSITION_ESTIMATOR_OUTDOOR_H #include #include #include #include #include #include "../position.h" #include "../measurement_types.h" namespace navigine { namespace navigation_core { struct GpsPosition { GpsPosition(double _latitude, double _longitude, double _deviation, long long _ts) : latitude (_latitude) , longitude (_longitude) , deviation (_deviation) , ts (_ts) { isEmpty = false; } GpsPosition() { isEmpty = true; } double latitude = NAN; double longitude = NAN; double altitude = NAN; double deviation = NAN; long long ts = 0; bool isEmpty = true; }; class SigmaFilter { public: SigmaFilter( const unsigned int& sigmaWindow, const double& sigmaDelta) : mSigmaWindow(sigmaWindow) , mSigmaDelta(sigmaDelta) {} bool add(const XYPoint& point); void clear(); private: const unsigned int mSigmaWindow; const double mSigmaDelta; std::deque mGNSSbuffer; }; class PositionEstimatorOutdoor { public: PositionEstimatorOutdoor( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); Position calculatePosition( long long ts, const SensorMeasurement& sensorMsr, const NmeaMeasurement& nmeaMsr, const bool flagIndoorPos); private: GpsPosition extractGpsPosition(long long ts, const SensorMeasurement& sensorEntry); int extractNumberOfSatellites(const NmeaMeasurement& nmeaEntry); Position mLastGpsPosition; std::shared_ptr mLevelIndex; SigmaFilter mSigmaFilter; const double mGpsValidTimeWindowSec; const bool mUseGpsOutsideMap; const bool mUseSigmaFilter; const int mMinNumSats; const double mMinGpsDeviation; const double mMaxGpsDeviation; }; } } // namespace navigine::navigation_core #endif // POSITION_ESTIMATOR_OUTDOOR_H ================================================ FILE: src/position_estimator/position_estimator_zone.cpp ================================================ /** position_estimator_zone.cpp * * Copyright (c) 2017 Navigine. * */ #include #include "position_estimator_zone.h" namespace navigine { namespace navigation_core { PositionEstimatorZone::PositionEstimatorZone( const std::shared_ptr &levelCollector, const NavigationSettings& navProps) : PositionEstimator(levelCollector) { for (const Level& level: levelCollector->levels()) { mLogModelParameterA[level.id()] = navProps.levelsSettings.at(level.id()).normalModeA; mLogModelParameterB[level.id()] = navProps.levelsSettings.at(level.id()).normalModeB; } } // TODO take into account transmitter power! Position PositionEstimatorZone::calculatePosition( const Level& level, long long ts, const RadioMeasurementsData& radioMsr, const MotionInfo&, NavigationStatus &retStatus) { if (radioMsr.empty()) { retStatus = NavigationStatus::NO_SOLUTION; return mPosition; } retStatus = NavigationStatus::OK; RadioMeasurementsData radioMeasurements = getLevelRadioMeasurements(level, radioMsr); if (radioMeasurements.empty()) { retStatus = NavigationStatus::NO_SOLUTION; return Position(); } auto nearestTx = std::max_element(radioMeasurements.begin(), radioMeasurements.end(), [](RadioMeasurementData msr1, RadioMeasurementData msr2) {return msr1.rssi < msr2.rssi; }); TransmitterId nearestTxId = nearestTx->id; XYZPoint p = XYZPoint(0.0, 0.0, 0.0); if (level.containsTransmitter(nearestTxId)) { p = level.transmitter(nearestTxId).point; } //TODO rename prop from A to "GP_Normal_Mode_A" double A = mLogModelParameterA[level.id()]; double B = mLogModelParameterB[level.id()]; double nearestTxRssi = nearestTx->rssi; double precision = sqrt(exp((A - nearestTxRssi) / B)) + 1.0; return Position( p.x, p.y, level.id(), precision, ts, false, Provider::INDOOR, 0.0); } } } // namespace navigine::navigation_core ================================================ FILE: src/position_estimator/position_estimator_zone.h ================================================ /** position_estimator_zone.h * * Copyright (c) 2017 Navigine. * */ #ifndef POSITION_ESTIMATOR_ZONE_H #define POSITION_ESTIMATOR_ZONE_H #include "position_estimator.h" namespace navigine { namespace navigation_core { class PositionEstimatorZone: public PositionEstimator { public: PositionEstimatorZone( const std::shared_ptr& levelCollector, const NavigationSettings& navProps); Position calculatePosition( const Level& level, long long ts, const RadioMeasurementsData& radioMsr, const MotionInfo& motionInfo, NavigationStatus& retStatus) override; private: std::map mLogModelParameterA; std::map mLogModelParameterB; }; } } // namespace navigine::navigation_core #endif // POSITION_ESTIMATOR_ZONE_H ================================================ FILE: src/position_postprocessor/navigation_time_smoother.cpp ================================================ /** navigation_time_smoother.cpp * * Copyright (c) 2018 Navigine. * */ #include "navigation_time_smoother.h" namespace navigine { namespace navigation_core { NavigationTimeSmoother::NavigationTimeSmoother() : mPrevPosition() // init empty position , mNextPosition() // init empty position { } /** * Returns position smoothed in time (due to this navigation is updated every time wsMessage is received). * Keeps last two positions and calculates intermidiate position between them based on how much time passed since * last known position was calculated */ Position NavigationTimeSmoother::getTimeSmoothedPosition(const Position& lastKnownPosition, long long currentTs) { if (currentTs < lastKnownPosition.ts) return lastKnownPosition; if (mPrevPosition.levelId != lastKnownPosition.levelId || mNextPosition.levelId != lastKnownPosition.levelId) { mPrevPosition = lastKnownPosition; mNextPosition = lastKnownPosition; return lastKnownPosition; } if (mPrevPosition.isEmpty || mNextPosition.isEmpty || lastKnownPosition.isEmpty) { mPrevPosition = mNextPosition; mNextPosition = lastKnownPosition; return lastKnownPosition; } if (lastKnownPosition.ts > mNextPosition.ts) { mPrevPosition = mNextPosition; mNextPosition = lastKnownPosition; } if (mNextPosition.ts == mPrevPosition.ts) return mNextPosition; double actualTsDiff = double(currentTs - mNextPosition.ts); double keptTsDiff = double(mNextPosition.ts - mPrevPosition.ts); if (actualTsDiff > keptTsDiff) // we don't use extrapolation return lastKnownPosition; double timeSmoothedX = (mNextPosition.x * actualTsDiff + mPrevPosition.x * (keptTsDiff - actualTsDiff)) / keptTsDiff; double timeSmoothedY = (mNextPosition.y * actualTsDiff + mPrevPosition.y * (keptTsDiff - actualTsDiff)) / keptTsDiff; Position timeSmoothedPos = mNextPosition; timeSmoothedPos.x = timeSmoothedX; timeSmoothedPos.y = timeSmoothedY; timeSmoothedPos.ts = currentTs; return timeSmoothedPos; } } } // namespace navigine ================================================ FILE: src/position_postprocessor/navigation_time_smoother.h ================================================ /** navigation_time_smoother.h * * Copyright (c) 2018 Navigine. * */ #ifndef NAVIGINE_NAVIGATION_TIME_SMOOTHER_H #define NAVIGINE_NAVIGATION_TIME_SMOOTHER_H #include "../position.h" namespace navigine { namespace navigation_core { class NavigationTimeSmoother { public: NavigationTimeSmoother(); Position getTimeSmoothedPosition(const Position& lastKnownPosition, long long currentTs); private: Position mPrevPosition; Position mNextPosition; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_NAVIGATION_TIME_SMOOTHER_H ================================================ FILE: src/position_postprocessor/polynomial_fit.cpp ================================================ /** polynomial_fit.h * * Copyright (c) 2018 Navigine. * */ #include "polynomial_fit.h" namespace navigine { namespace navigation_core { PolynomialFit::PolynomialFit(int power, double timeInterval, double timeShift, double newWeight) : mPower(power) , mTimeInterval(timeInterval) , mTimeShift(timeShift) , mCoeff(mPower + 1, 0.) , mNewSolutionWeight(newWeight) , mLastOutput(0) { } void PolynomialFit::addSequencePoint(double x, double y) { if (mY.empty()) mCoeff.assign(mPower + 1, 0.); if (mY.empty() || mY.back() != y) { mY.push_back(y); mX.push_back(x); removeOldPoints(); if ((int)mY.size() > mPower) getLeastSquaresCoeff(); } } void PolynomialFit::removeOldPoints() { while ((mX.back() - mX.front()) > mTimeInterval) { mX.pop_front(); mY.pop_front(); } } void PolynomialFit::clear() { mX.clear(); mY.clear(); } double PolynomialFit::predict(double x) { double out = 0; double slnCoef = mNewSolutionWeight; int size = (int)mY.size(); if (size == 0) { return -1.0; } else if (size == 1 || size < mPower + 1) { out = getLagrangeSolution(x); } else { out = getLeastSquaresSolution(x); out = out * slnCoef + mLastOutput * (1.0 - slnCoef); } mLastOutput = out; return out; } double PolynomialFit::getLagrangeSolution(double curTime) { size_t pwr = mY.size(); double out = 0.0; double mult = 1.0; double time = curTime - mTimeShift; for (size_t i = 0; i < pwr; i++) { mult = 1.0; for (size_t j = 0; j < pwr; j++) { if (i != j) mult *= (time - mX[j]) / (mX[i] - mX[j]); } out += mY[i] * mult; } return out; } double PolynomialFit::getLeastSquaresSolution (double curTime) { double out = 0.0; double time = curTime - mTimeShift; for (int i = 0; i <= mPower; i++) { out += mCoeff[i] * pow(time, i); } return out; } void PolynomialFit::getLeastSquaresCoeff() { std::vector A ( (mPower + 1)*(mPower + 1), 0.); std::vector b ( mPower + 1, 0.); std::vector sub(2*mPower + 1, 0.); double sum; // Initialize sub for (int i = 0; i < 2 * mPower + 1; i++) { sum = 0; for (size_t j = 0; j < mY.size(); j++) { sum += pow(mX[j], i); } sub[i] = sum; } // Initialize matrix 'A' for (int i = 0; i < mPower + 1; i++) for (int j = 0; j < mPower + 1; j++) A[i + j*(mPower + 1)] = sub[i + j]; // Initialize vector 'b' for (int i = 0; i < mPower + 1; i++) { sum = 0; for (size_t j = 0; j < mY.size(); j++) { sum += pow(mX[j], i) * mY[j]; } b[i] = sum; } solveLinearSystem(A, b, mCoeff, mPower + 1); return; } void solveLinearSystem(const std::vector & A, const std::vector & b, std::vector & x, const int n) { // Rotations method int i, j, k; double cosphi, sinphi, norm, temp; std::vector E(n*(n + 1)); // E initialization. E is a [n x (n+1)] matrix formed as // E = [A,b] to simplify the calculations for (int i = 0; i < n; i++) { E[i*(n + 1) + n] = b[i]; for (int j = 0; j < n; j++) E[i*(n + 1) + j] = A[i*n + j]; } // Forward Gaussian for (k = 0; k < n; ++k) for (j = k + 1; j < n; ++j) { cosphi = E[k * (n + 1) + k]; sinphi = E[j * (n + 1) + k]; norm = sqrt(cosphi * cosphi + sinphi * sinphi); cosphi /= norm; sinphi /= norm; for (i = k; i < n + 1; ++i) { temp = cosphi * E[j * (n + 1) + i] - sinphi * E[k * (n + 1) + i]; E[k * (n + 1) + i] = sinphi * E[j * (n + 1) + i] + cosphi * E[k * (n + 1) + i]; E[j * (n + 1) + i] = temp; } } // Back-Gaussing for (k = n - 1; k >= 0; --k) { x[k] = E[k*(n + 1) + n]; for (i = k + 1; i < n; ++i) x[k] -= E[k*(n + 1) + i] * x[i]; x[k] /= E[k*(n + 1) + k]; } return; } } } // namespace navigine::navigation_core ================================================ FILE: src/position_postprocessor/polynomial_fit.h ================================================ /** polynomial_fit.h * * Copyright (c) 2018 Navigine. * */ #ifndef POLYNOMIAL_FIT_H #define POLYNOMIAL_FIT_H #include #include #include #include #include namespace navigine { namespace navigation_core { void solveLinearSystem(const std::vector & A, const std::vector & b, std::vector & x, const int n); class PolynomialFit { public: PolynomialFit(int power = 1, double timeInterval = 2.0, double timeShift = 0.7, double newWeight = 0.2); void addSequencePoint(double x, double y); double predict(double x); void clear(); private: const int mPower; const double mTimeInterval; const double mTimeShift; const double mNewSolutionWeight; double mLastOutput; std::deque mY; std::deque mX; std::vector mCoeff; void removeOldPoints(); void getLeastSquaresCoeff(); double getLagrangeSolution(const double x); double getLeastSquaresSolution(const double x); }; } } // namespace navigine::navigation_core #endif ================================================ FILE: src/position_postprocessor/position_postprocessor.cpp ================================================ #include #include "../geometry.h" #include "position_postprocessor.h" #include "position_smoother_ab.h" #include "position_smoother_lstsq.h" namespace navigine { namespace navigation_core { PositionPostprocessor::PositionPostprocessor(const NavigationSettings& navProps) : mUseSmoothing(navProps.commonSettings.useSmoothing) , mUseStops(navProps.commonSettings.useStops) , mUseInstantGpsPosition(navProps.commonSettings.useInstantGpsPosition) , mUseGps(navProps.commonSettings.useGps) , mFuseGps(navProps.commonSettings.fuseGps) , mUseGpsOutsideMap(navProps.commonSettings.useGpsOutsideMap) , mPreferIndoorSolution(navProps.commonSettings.preferIndoorSolution) , mUseTimeSmoothing(navProps.commonSettings.useTimeSmoothing) , mUseGraphProjection(navProps.commonSettings.useGraphProjection) , mDeadReckonTimeMs((long long)(1000 * navProps.commonSettings.deadReckoningTime)) , mStopUpdateTimeMs((long long)(1000 * navProps.commonSettings.stopUpdateTime)) , mStopDetectionTimeMs((long long)(1000 * navProps.commonSettings.stopDetectionTime)) , mGraphProjectionDistance(navProps.commonSettings.graphProjDist) , mPriorDev(navProps.commonSettings.priorDeviation) , mFuseGpsBorderM(navProps.commonSettings.fuseGpsBorderM) , mMotionSpeed(navProps.commonSettings.averageMovSpeed) , mPosIsTooOldForFusingSec(navProps.commonSettings.positionIsTooOldSec) , mStopsDistThresholdM(navProps.commonSettings.useStopsDistanceThresholdM) , mIndoorDev(navProps.commonSettings.priorDeviation) , mOutdoorDev(navProps.commonSettings.priorDeviation) , mNavigationTimeSmoother() , mIndoorPos() , mOutdoorPos() , mLastOutPosition() { mLastExtractionTs = 0; bool useAbFilter = navProps.commonSettings.useAbFilter; if (useAbFilter) mPositionSmoother = new PositionSmootherAB(navProps); else mPositionSmoother = new PositionSmootherLstsq(navProps); } PositionPostprocessor::~PositionPostprocessor() { if (mPositionSmoother) delete mPositionSmoother; } void PositionPostprocessor::fillIndoorOutdoorPositionStates( const Position& indoorPos, const Position& outdoorPos) { if (!indoorPos.isEmpty) { double priorIndoorDev = mIndoorPos.isEmpty ? mPriorDev : std::min(mPriorDev, mIndoorDev + mMotionSpeed * (indoorPos.ts - mIndoorPos.ts) / 1000.0); mIndoorPos = indoorPos; mIndoorDev = (priorIndoorDev + indoorPos.deviationM) / 2.0; } if (!outdoorPos.isEmpty) { double priorOutdoorDev = mOutdoorPos.isEmpty ? mPriorDev : std::min(mPriorDev, mOutdoorDev + mMotionSpeed * (outdoorPos.ts - mOutdoorPos.ts) / 1000.0); mOutdoorDev = (priorOutdoorDev + outdoorPos.deviationM) / 2.0; mOutdoorPos = outdoorPos; } } Position PositionPostprocessor::fusePositions( long long curTs, const Position& indoorPos, const Position& outdoorPos, const NavigationStatus& navStatus) { fillIndoorOutdoorPositionStates(indoorPos, outdoorPos); // return instant gps position for debugging: if (mUseInstantGpsPosition && !outdoorPos.isEmpty) return outdoorPos; if (mPreferIndoorSolution && !indoorPos.isEmpty and navStatus==0) return indoorPos; if (!mUseGps || mOutdoorPos.isEmpty) return mIndoorPos; if (mIndoorPos.isEmpty) return mOutdoorPos; double indoorPositionDelay = curTs >= mIndoorPos.ts ? (curTs - mIndoorPos.ts) / 1000.0 : 0.0; double outdoorPositionDelay = curTs >= mOutdoorPos.ts ? (curTs - mOutdoorPos.ts) / 1000.0 : 0.0; if ((indoorPositionDelay) > mPosIsTooOldForFusingSec) return mOutdoorPos; if ((outdoorPositionDelay) > mPosIsTooOldForFusingSec) return mIndoorPos; double indoorDev = mMotionSpeed * indoorPositionDelay + mIndoorDev; double outdoorDev = mMotionSpeed * outdoorPositionDelay + mOutdoorDev; double positionsDist = GetDist(mIndoorPos.x, mIndoorPos.y, mOutdoorPos.x, mOutdoorPos.y); double accuracyDist = mIndoorPos.deviationM + mOutdoorPos.deviationM + mFuseGpsBorderM; if (mFuseGps && (accuracyDist > positionsDist)) { Position fusedPos = mIndoorPos; fusedPos.ts = std::max(mIndoorPos.ts, mOutdoorPos.ts); fusedPos.x = (mIndoorPos.x * outdoorDev + mOutdoorPos.x * indoorDev) / (outdoorDev + indoorDev); fusedPos.y = (mIndoorPos.y * outdoorDev + mOutdoorPos.y * indoorDev) / (outdoorDev + indoorDev); fusedPos.deviationM = std::min(indoorDev, outdoorDev); fusedPos.provider = Provider::FUSED; return fusedPos; } else return indoorDev < outdoorDev ? mIndoorPos : mOutdoorPos; } LocationPoint getProjection(const Graph& graph, const LocationPoint& P) { LocationPoint P0 = P; double d0 = NAN; std::pair e0; for (auto edgePair = graph.edgesBegin(); edgePair != graph.edgesEnd(); ++edgePair) { const Graph::Vertex& u = graph.getVertex((*edgePair).first); const Graph::Vertex& v = graph.getVertex((*edgePair).second); LocationPoint P1 = P; double k = GetProjection(u.point.x, u.point.y, v.point.x, v.point.y, P.x, P.y, &P1.x, &P1.y); if (k < 0) P1 = LocationPoint{P.level, u.point.x, u.point.y}; else if (k > 1) P1 = LocationPoint{P.level, v.point.x, v.point.y}; double d = GetDist(P, P1); if (std::isnan(d0) || d < d0) { P0 = P1; d0 = d; e0 = *edgePair; } } return P0; } bool PositionPostprocessor::isInsideMap(const Position& pos, const Level& level) { if (mUseGpsOutsideMap) return true; return boost::geometry::covered_by(static_cast(pos), level.geometry().boundingBox()); } Position PositionPostprocessor::getProcessedPosition(const Position& fusedPosition, long long ts, const MotionInfo& motionInfo, const Level& level) { if (motionInfo.lastMotionTs > 0) mLastStepTs = motionInfo.lastMotionTs; if (fusedPosition.isEmpty) return Position(); //TODO error code "fused position is empty!" if (!isInsideMap(fusedPosition, level)) return Position(); // Position is outside of map! if (ts - fusedPosition.ts >= mDeadReckonTimeMs) return Position(); //TODO error code "last received position is too old!" Position result = fusedPosition; if (mUseSmoothing) { if (mLastOutPosition.levelId != result.levelId) mPositionSmoother->reset(result); result = mPositionSmoother->smoothPosition(result, level); } if (mUseTimeSmoothing) result = mNavigationTimeSmoother.getTimeSmoothedPosition(result, ts); //TODO check if pedometer is available before using steps! //TODO be very carefull when use stops and gps simultaneously bool lastStepWasTooLongAgo = (mLastStepTs > 0) && (ts - mLastStepTs >= mStopDetectionTimeMs); double dist = GetDist(mLastOutPosition.x, mLastOutPosition.y, result.x, result.y); bool newPositionIsNotTooFar = dist < mStopsDistThresholdM; mLastOutPosition.ts = ts; if (mUseStops && lastStepWasTooLongAgo) { if ((ts - mLastExtractionTs > mStopUpdateTimeMs) && newPositionIsNotTooFar) { mLastExtractionTs = ts; mLastOutPosition = result; mLastOutPosition.ts = ts; } else { mLastOutPosition.ts = ts; } } else { mLastOutPosition = result; mLastOutPosition.ts = ts; mLastOutPosition.deviationM = mIndoorPos.deviationM; } // Projecting coordinates to graph if (mUseGraphProjection) { const LocationPoint P1 = {mLastOutPosition.levelId, mLastOutPosition.x, mLastOutPosition.y}; const LocationPoint P2 = getProjection(level.graph(), P1); const double distance = GetDist(P1, P2); if (!std::isnan(distance) && distance < mGraphProjectionDistance) { mLastOutPosition.x = P2.x; mLastOutPosition.y = P2.y; } } // Fitting coordinates inside box Point minCorner = level.geometry().boundingBox().min_corner(); Point maxCorner = level.geometry().boundingBox().max_corner(); mLastOutPosition.x = std::min(std::max(mLastOutPosition.x, minCorner.x()), maxCorner.x()); mLastOutPosition.y = std::min(std::max(mLastOutPosition.y, minCorner.y()), maxCorner.y()); // Rounding coordinates to 0.01 mLastOutPosition.x = std::floor(mLastOutPosition.x * 100) / 100; mLastOutPosition.y = std::floor(mLastOutPosition.y * 100) / 100; return mLastOutPosition; } } } // namespace navigine::navigation_core ================================================ FILE: src/position_postprocessor/position_postprocessor.h ================================================ #ifndef POSITION_POSTPROCESSOR_H #define POSITION_POSTPROCESSOR_H #include #include #include #include "../position.h" #include "navigation_time_smoother.h" #include "position_smoother.h" namespace navigine { namespace navigation_core { class PositionPostprocessor { public: ~PositionPostprocessor(); PositionPostprocessor(const NavigationSettings& navProps); Position getProcessedPosition(const Position& fusedPos, long long ts, const MotionInfo& motionInfo, const Level& level); Position fusePositions(long long curTs, const Position& indoorPos, const Position& outdoorPos, const NavigationStatus& navStatus); void fillIndoorOutdoorPositionStates(const Position& indoorPos, const Position& outdoorPos); const Position getIndoorPosition() { return mIndoorPos; } const Position getOutdoorPosition() { return mOutdoorPos; } private: bool isInsideMap(const Position& pos, const Level& level); const bool mUseSmoothing; const bool mUseStops; const bool mUseInstantGpsPosition; const bool mUseGps; const bool mFuseGps; const bool mUseGpsOutsideMap; const bool mPreferIndoorSolution; const bool mUseTimeSmoothing; const bool mUseGraphProjection; const long long mDeadReckonTimeMs; const long long mStopUpdateTimeMs; const long long mStopDetectionTimeMs; const double mGraphProjectionDistance; const double mPriorDev; const double mFuseGpsBorderM; const double mMotionSpeed; const double mPosIsTooOldForFusingSec; const double mStopsDistThresholdM; double mIndoorDev; double mOutdoorDev; NavigationTimeSmoother mNavigationTimeSmoother; PositionSmoother* mPositionSmoother; Position mIndoorPos; Position mOutdoorPos; Position mLastOutPosition; long long mLastExtractionTs = -1; long long mLastStepTs = -1; }; } } // namespace navigine::navigation_core #endif // POSITION_POSTPROCESSOR_H ================================================ FILE: src/position_postprocessor/position_smoother.h ================================================ #ifndef NAVIGINE_POSITION_SMOOTHER_H #define NAVIGINE_POSITION_SMOOTHER_H #include #include #include "../position.h" namespace navigine { namespace navigation_core { class PositionSmoother { public: virtual ~PositionSmoother() {} virtual Position smoothPosition(Position pos, const Level& level) = 0; virtual void reset(Position pos) = 0; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_POSITION_SMOOTHER_H ================================================ FILE: src/position_postprocessor/position_smoother_ab.cpp ================================================ #include #include "position_smoother_ab.h" namespace navigine { namespace navigation_core { static const double MAX_ALPHA = 0.9; static const double MIN_ALPHA = 0.1; static const double TIME_THRESHOLD_SEC = 10; static const double MAX_OBJECT_VELOCITY_MPS = 4; PositionSmootherAB::PositionSmootherAB(const NavigationSettings& navProps) : mUseBarriers(navProps.commonSettings.useBarriers) , mUseSpeedConstraint(navProps.commonSettings.useSpeedConstraint) , mUseAccuracyForSmoothing(navProps.commonSettings.useAccuracyForSmoothing) , mWasCalled(false) , mSpeedX(0.0) , mSpeedY(0.0) { double smoothing = navProps.commonSettings.smoothing; smoothing = std::min(1.0, std::max(0.0, smoothing)); mAlpha = MIN_ALPHA * smoothing + MAX_ALPHA * (1.0 - smoothing); } Position PositionSmootherAB::smoothPosition(Position pos, const Level& level) { // assert(pos.ts > mPosition.ts); if (pos.ts > mPosition.ts) { double t = (pos.ts - mPosition.ts) / 1000.0; double a = mAlpha; if (mUseAccuracyForSmoothing) { double newAlpha = mPosition.deviationM / (pos.deviationM + mPosition.deviationM + 0.5); a = std::min(std::max(MIN_ALPHA, newAlpha), MAX_ALPHA); } /* temporary commented - it boost the filter to obtain the minimal speed */ // if (std::sqrt(mSpeedX * mSpeedX + mSpeedY * mSpeedY) < 0.5) // a = MAX_ALPHA / 2.0; double b = a * a / (2.0 - a); double xp = mPosition.x + mSpeedX * t; double vxp = mSpeedX + (b / t) * (pos.x - xp); double xs = xp + a * (pos.x - xp); double yp = mPosition.y + mSpeedY * t; double vyp = mSpeedY + (b / t) * (pos.y - yp); double ys = yp + a * (pos.y - yp); double velocity = std::sqrt(vxp * vxp + vyp * vyp); bool timeIsTooLong = t > TIME_THRESHOLD_SEC; bool velocityIsTooFast = (mUseSpeedConstraint && velocity > MAX_OBJECT_VELOCITY_MPS); bool isInsideBarrier = (mUseBarriers && !boost::geometry::covered_by(Point(xs, ys), level.geometry().allowedArea())); if (!mWasCalled || timeIsTooLong || velocityIsTooFast || isInsideBarrier) { mPosition = pos; mSpeedX = 0.0; mSpeedY = 0.0; mWasCalled = true; return pos; } mSpeedX = vxp; mSpeedY = vyp; if (std::isfinite(xs) && std::isfinite(ys)) { pos.x = xs; pos.y = ys; } mPosition = pos; if (mUseAccuracyForSmoothing) { //TODO more real accuracy update double predictedAccuracy = mPosition.deviationM + t * std::sqrt(mSpeedX * mSpeedX + mSpeedY * mSpeedY); mPosition.deviationM = (1.0 - a) * predictedAccuracy + a * pos.deviationM; } } return mPosition; } void PositionSmootherAB::reset(Position pos) { mPosition = pos; mSpeedX = 0.0; mSpeedY = 0.0; } } } // namespace navigine::navigation_core ================================================ FILE: src/position_postprocessor/position_smoother_ab.h ================================================ #ifndef NAVIGINE_POSITION_SMOOTHER_AB_H #define NAVIGINE_POSITION_SMOOTHER_AB_H #include "position_smoother.h" namespace navigine { namespace navigation_core { class PositionSmootherAB: public PositionSmoother { public: PositionSmootherAB(const NavigationSettings& navProps); Position smoothPosition(Position pos, const Level& level) override; void reset(Position pos) override; private: const bool mUseBarriers; const bool mUseSpeedConstraint; const bool mUseAccuracyForSmoothing; bool mWasCalled = false; double mSpeedX = 0.0; double mSpeedY = 0.0; double mAlpha = 0.0; Position mPosition; long long mTsMs; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_POSITION_SMOOTHER_AB_H ================================================ FILE: src/position_postprocessor/position_smoother_lstsq.cpp ================================================ #include #include "position_smoother_lstsq.h" namespace navigine { namespace navigation_core { PositionSmootherLstsq::PositionSmootherLstsq(const NavigationSettings& navProps) : mWasCalled(false) , mUseBarriers(navProps.commonSettings.useBarriers) , mTimeInterval(navProps.commonSettings.sigAveragingTime) , mWindowShift(navProps.commonSettings.sigWindowShift) , mSmoothing(std::min(1.0, std::max(0.0, navProps.commonSettings.smoothing))) , mPolyFitX(1, mTimeInterval, mWindowShift, mSmoothing) , mPolyFitY(1, mTimeInterval, mWindowShift, mSmoothing) { } Position PositionSmootherLstsq::smoothPosition(Position pos, const Level& level) { if (!mWasCalled) { mStartTime = pos.ts; mWasCalled = true; return pos; } double t = (pos.ts - mStartTime) / 1000.0; mPolyFitX.addSequencePoint(t, pos.x); mPolyFitY.addSequencePoint(t, pos.y); double xs = mPolyFitX.predict(t); double ys = mPolyFitY.predict(t); if (mUseBarriers && !boost::geometry::covered_by(Point(xs, ys), level.geometry().allowedArea())) { mPolyFitX.clear(); mPolyFitY.clear(); mStartTime = pos.ts; return pos; } if (std::isfinite(xs) && std::isfinite(ys)) { pos.x = xs; pos.y = ys; } return pos; } void PositionSmootherLstsq::reset(Position pos) { mPolyFitX.clear(); mPolyFitY.clear(); mStartTime = pos.ts; } } } // namespace navigine::navigation_core ================================================ FILE: src/position_postprocessor/position_smoother_lstsq.h ================================================ #ifndef NAVIGINE_POSITION_SMOOTHER_LSTSQ_H #define NAVIGINE_POSITION_SMOOTHER_LSTSQ_H #include "position_smoother.h" #include "polynomial_fit.h" namespace navigine { namespace navigation_core { class PositionSmootherLstsq: public PositionSmoother { public: PositionSmootherLstsq(const NavigationSettings& navProps); Position smoothPosition(Position pos, const Level& level) override; void reset(Position pos) override; private: const double mTimeInterval; const double mWindowShift; const double mSmoothing; bool mWasCalled; bool mUseBarriers; PolynomialFit mPolyFitX; PolynomialFit mPolyFitY; long long mStartTime; }; } } // namespace navigine::navigation_core #endif // NAVIGINE_POSITION_SMOOTHER_LSTSQ_H ================================================ FILE: src/radiomap.cpp ================================================ /** radiomap.cpp * * Copyright (c) 2019 Navigine. * */ #include namespace navigine { namespace navigation_core { Radiomap::Radiomap(const XYReferencePoints& referencePoints) { mReferencePoints = referencePoints; for (std::size_t i = 0; i < referencePoints.size(); i++) mReferencePointsIndex.insert({referencePoints[i].id, i}); } const XYReferencePoints& Radiomap::referencePoints() const { return mReferencePoints; } bool Radiomap::hasReferencePoint(const ReferencePointId& refPointId) const { return mReferencePointsIndex.find(refPointId) != mReferencePointsIndex.end(); } ReferencePoint Radiomap::getReferencePoint(const ReferencePointId& refPointId) const { return mReferencePoints.at(mReferencePointsIndex.at(refPointId)); } } } // namespace navigine::navigation_core ================================================ FILE: src/sensor_fusion/complementary_filter.cpp ================================================ /** complementary_filter.cpp * * Copyright (c) 2018 Navigine. * */ #include #include #include "complementary_filter.h" namespace navigine { namespace navigation_core { static const double CF_DOUBLE_EPSILON = 1e-8; static const double MIN_SAMPLING_PERIOD_SEC = 0.02; static const double LPF_TIME_CONSTANT_SEC = 1.0; double to_minus_Pi_Pi(double x) { while ( x >= M_PI ) x -= 2 * M_PI; while ( x < -M_PI ) x += 2 * M_PI; return x; } Quaternion updateQuaternion(const Quaternion& q, const Vector3d& v) { Quaternion qDot = q * Quaternion(v) * 0.5; Quaternion qOut = q + qDot; return qOut.normalized(); } SensorMeasurement lowPassFilter(const SensorMeasurement& prevMsr, const SensorMeasurement& currMsr) { double dt = (currMsr.ts - prevMsr.ts) / 1000.0; double alpha = LPF_TIME_CONSTANT_SEC / (LPF_TIME_CONSTANT_SEC + std::max(dt, MIN_SAMPLING_PERIOD_SEC)); SensorMeasurement res = currMsr; res.data.values = alpha * prevMsr.data.values + (1 - alpha) * currMsr.data.values; return res; } /** This function returns calculated orientation (in radians) and current timestamp */ /* the orientation is right-handed, therefore counter-clockwise rotation has positive sign */ Orientation ComplementaryFilter::getFusedOrientation() const { double roll, pitch, yaw; std::tie(roll, pitch, yaw) = mQ.toEuler(); return Orientation{roll, pitch, yaw, mCurrentTs}; } /** This function returns calculated fused azimuth (in radians), clockwise has positive sign */ double ComplementaryFilter::getFusedAzimuth() const { double roll, pitch, yaw; std::tie(roll, pitch, yaw) = mQ.toEuler(); return -yaw; } /** This function returns calculated magnetic azimuth (in radians), clockwise has positive sign */ double ComplementaryFilter::getMagneticAzimuth() const { return mMagneticAzimuth; } /** This function returns azimuth calculated on device (in radians), clockwise has positive sign */ double ComplementaryFilter::getDeviceAzimuth() const { return mDeviceAzimuth; } /** This function returns calculated gyro heading (in radians), clockwise has positive sign */ double ComplementaryFilter::getGyroHeading() const { return mGyroHeading; } /** measurements time must be in ascending order! */ void ComplementaryFilter::update(const SensorMeasurement& msr) { if(msr.data.type == SensorMeasurementData::Type::ACCELEROMETER) { mAccelLowPassed = lowPassFilter(mAccelLowPassed, msr); updateUsingAccelerometer(msr); } else if (msr.data.type == SensorMeasurementData::Type::MAGNETOMETER) { mMagnLowPassed = lowPassFilter(mMagnLowPassed, msr); if (mLastGyroTs == -1) updateUsingMagnetometer(msr); mMagneticAzimuth = calculateMagneticAzimuth(msr); } else if (msr.data.type == SensorMeasurementData::Type::GYROSCOPE) { if (mLastGyroTs == -1) mLastGyroTs = msr.ts; updateUsingGyroscope(msr); mGyroHeading = getFusedAzimuth(); } else if (msr.data.type == SensorMeasurementData::Type::ORIENTATION) { mDeviceAzimuth = msr.data.values.x; mLastDeviceAzimuthTs = msr.ts; } mCurrentTs = msr.ts; return; } bool ComplementaryFilter::isDeviceAzimuthOutdated() const { double dt = static_cast(mCurrentTs - mLastDeviceAzimuthTs) / 1000.0; if (dt > mDeviceAzimuthLifetimeSeconds || mLastDeviceAzimuthTs == -1) return true; else return false; } void ComplementaryFilter::updateUsingGyroscope(const SensorMeasurement& gyroMeas) { double dt = static_cast(gyroMeas.ts - mLastGyroTs) / 1000.0; mLastGyroTs = gyroMeas.ts; Vector3d gyro = gyroMeas.data.values; Vector3d omega = gyro * dt + mIntergalError * dt; mQ = updateQuaternion(mQ, omega); } void ComplementaryFilter::updateUsingAccelerometer(const SensorMeasurement& accelMeas) { Vector3d acc = accelMeas.data.values.normalized(); Vector3d gravSensorFrame = (mQ.conj() * Quaternion(0.0, 0.0, 0.0, 1.0) * mQ).toVector3d(); Vector3d error = Vector3d::crossProduct(acc, gravSensorFrame); Vector3d rotation = mKaccelerometer * error; mQ = updateQuaternion(mQ, rotation); mIntergalError += (mKintergalGain > CF_DOUBLE_EPSILON) ? (mKintergalGain * error) : Vector3d(0.0, 0.0, 0.0); } void ComplementaryFilter::updateUsingMagnetometer(const SensorMeasurement& magnMeas) { Vector3d magn = magnMeas.data.values.normalized(); if (magn.magnitude() < CF_DOUBLE_EPSILON) return; Vector3d magnGlobalFrame = (mQ * Quaternion(magn) * mQ.conj()).toVector3d(); magnGlobalFrame.y = std::sqrt(magnGlobalFrame.y * magnGlobalFrame.y + magnGlobalFrame.x * magnGlobalFrame.x); magnGlobalFrame.x = 0.0; Vector3d magnSensorFrame = (mQ.conj() * Quaternion(magnGlobalFrame) * mQ).toVector3d(); Vector3d error = Vector3d::crossProduct(magn, magnSensorFrame); Vector3d rotation = mKmagnetometer * error; mQ = updateQuaternion(mQ, rotation); mIntergalError += (mKintergalGain > CF_DOUBLE_EPSILON) ? (mKintergalGain * error) : Vector3d(0.0, 0.0, 0.0); } double ComplementaryFilter::calculateMagneticAzimuth(const SensorMeasurement& magn) { double roll, pitch, yaw; std::tie(roll, pitch, yaw) = mQ.toEuler(); double halfYaw = yaw / 2.0; Quaternion quatDifference = Quaternion(sin(halfYaw), 0, 0, cos(halfYaw)).normalized(); Quaternion quatZeroYaw = quatDifference * mQ; Vector3d magnetometer = magn.data.values.normalized(); Vector3d magnGlobalFrame = (quatZeroYaw * Quaternion(magnetometer) * quatZeroYaw.conj()).toVector3d(); return to_minus_Pi_Pi(M_PI / 2.0 + atan2(magnGlobalFrame.y, magnGlobalFrame.x)); } } } // namespace navigine::navigation_core ================================================ FILE: src/sensor_fusion/complementary_filter.h ================================================ /** complementary_filter * * Copyright (c) 2018 Navigine. * */ #ifndef COMPLEMENTARY_FILTER_H #define COMPLEMENTARY_FILTER_H #include #include #include #include "quaternion.h" #include "../measurement_types.h" namespace navigine { namespace navigation_core { double to_minus_Pi_Pi(double x); struct Orientation { double roll = 0.0; double pitch = 0.0; double yaw = 0.0; long long ts = 0; Orientation(double roll_, double pitch_, double yaw_, long long ts_) : roll{roll_} , pitch{pitch_} , yaw{yaw_} , ts{ts_} {}; }; class ComplementaryFilter { public: ComplementaryFilter (const NavigationSettings& navProps, const double Ka = 0.1, const double Km = 0.05, const double Ki = 0.0) : mKaccelerometer (Ka) , mKmagnetometer (Km) , mKintergalGain (Ki) , mDeviceAzimuthLifetimeSeconds(navProps.commonSettings.deviceAzimuthLifetimeSeconds) {}; double getMagneticAzimuth() const; double getFusedAzimuth() const; double getDeviceAzimuth() const; double getGyroHeading() const; Orientation getFusedOrientation() const; void update(const SensorMeasurement& msr); bool isDeviceAzimuthOutdated() const; private: void updateUsingGyroscope (const SensorMeasurement& gyro); void updateUsingAccelerometer (const SensorMeasurement& accel); void updateUsingMagnetometer (const SensorMeasurement& magn); double calculateMagneticAzimuth (const SensorMeasurement& magn); long long mCurrentTs = -1; long long mLastGyroTs = -1; long long mLastDeviceAzimuthTs = -1; const double mKaccelerometer; const double mKmagnetometer; const double mKintergalGain; const double mDeviceAzimuthLifetimeSeconds; Vector3d mIntergalError = {}; Quaternion mQ = {}; double mMagneticAzimuth = 0.0; double mDeviceAzimuth = 0.0; double mGyroHeading = 0.0; SensorMeasurement mMagnLowPassed; // Magnetometer measurements passed through low-pass filter SensorMeasurement mAccelLowPassed; // Accelerometer measurements passed through low-pass filter }; } } // namespace navigine::navigation_core #endif // COMPLEMENTARY_FILTER_H ================================================ FILE: src/sensor_fusion/pedometer.cpp ================================================ /** pedometer.cpp * * Copyright (c) 2018 Navigine. * */ #include #include #include #include #include "pedometer.h" //#define DEBUG_OUTPUT_PEDOMETER namespace navigine { namespace navigation_core { #if defined (DEBUG_OUTPUT_PEDOMETER) #include #include static const std::string PREPROCESSED_PEDOMETER_MSR_DEBUG_OUTPUT_FILE = DEBUG_OUTPUT_PEDOMETER; void clearPedometerDebugOutput(); void printDebugPedometer(const std::string &type, long long ts, double value); #endif static const long long AVERAGING_TIME_INTERVAL_MS = 2500; //2000 // Time interval on which we calculate averaged filtered magnitude in order to align it relative to zero value static const long long FILTER_TIME_INTERVAL_MS = 200; // Filtered acc measurements are calculated as average over this time interval static const long long UPDATE_TIME_INTERVAL_MS = 700; // 650 // The time interval(in millisec) on which we update accelMax, Min, threshold static const long long MIN_TIME_BETWEEN_STEPS_MS = 300; // We cut off possible steps if time between them is less then this thresh static const double STEP_LENGTH_CONST = 0.52; //0.48 // Constant using in algorithm which is responsible for step length calculation static const double MINIMAL_THRESHOLD_VALUE = 0.05 * 9.80665; // Empirical threshold values from article p.888 static const int MINIMAL_NUMBER_OF_STEPS = 5; // First N steps in which we accumulate data about average step time static const int MAXIMUM_NUMBER_OF_STEPS = 50; // Maximum number of steps to make an assumption about average step time static const int MAX_STEP_TIME = 2000; // The maximum possible time for step duration void Pedometer::update(const SensorMeasurement& msr) { mMagnitudeSize = (std::max)(mFilteredAccMagnitudes.size(), size_t(1)); if (msr.data.type == SensorMeasurementData::Type::ACCELEROMETER) { mAccelMeasurements.push_back(msr); Magnitude magn = calculateFilteredAccMagnitudes(); if (magn.ts!=0) mFilteredAccMagnitudes.push_back(magn); while (mAccelMeasurements.back().ts - mAccelMeasurements.front().ts > 2 * AVERAGING_TIME_INTERVAL_MS && mAccelMeasurements.size() > 0) { mAccelMeasurements.pop_front(); } #if defined (DEBUG_OUTPUT_PEDOMETER) if (mAccelMeasurements.size() == 1) { //Remove output file at the start of the test clearPedometerDebugOutput(); } if (magn.ts != 0) printDebugPedometer("MAGNITUDE", magn.ts, magn.value); printDebugPedometer("INPUT", msr.ts, msr.getMagnitude()); #endif } } /** * This method calculates and form array with filtered acceleration magnitude * sliding window average is used (low pass). This function also updates average * filtered magnitude */ Magnitude Pedometer::calculateFilteredAccMagnitudes() const { long long lastMeasTs = mAccelMeasurements.back().ts; std::deque::const_iterator lBorderAverIt = mAccelMeasurements.begin(); while (lastMeasTs - lBorderAverIt->ts >= AVERAGING_TIME_INTERVAL_MS) lBorderAverIt++; double averMagnitude = std::accumulate(lBorderAverIt, mAccelMeasurements.end(), 0.0, [](double sum, const SensorMeasurement msr) {return sum+msr.data.values.magnitude();}); double nMeasAverage = std::distance(lBorderAverIt, mAccelMeasurements.end()); std::deque::const_iterator lBorderFilterIt = mAccelMeasurements.begin(); while (lastMeasTs - lBorderFilterIt->ts >= FILTER_TIME_INTERVAL_MS) lBorderFilterIt++; double filterMagnitude = std::accumulate(lBorderFilterIt, mAccelMeasurements.end(), 0.0, [](double sum, const SensorMeasurement msr) {return sum+msr.data.values.magnitude();}); double nMeasFiltered = std::distance(lBorderFilterIt, mAccelMeasurements.end()); // There is no measurements at acceleration array!\n"); if (nMeasAverage == 0 || nMeasFiltered == 0) return Magnitude(0,0); filterMagnitude /= nMeasFiltered; averMagnitude += filterMagnitude; // For correct processing at the very beginning averMagnitude /= nMeasAverage; filterMagnitude -= averMagnitude; return Magnitude(lastMeasTs, filterMagnitude); } std::deque Pedometer::calculateSteps() { std::deque steps = {}; if (mFilteredAccMagnitudes.size() <= 3) //There is no enough accelerometer measurements to detect steps return steps; long long averageStepTime = 0; auto nSteps = std::max(std::distance(mTimes.begin(), mTimes.end()), std::ptrdiff_t(1)); if (nSteps >= MINIMAL_NUMBER_OF_STEPS) averageStepTime = std::accumulate(mTimes.begin(), mTimes.end(), 0, [](long long sum, long long s) { return sum + s; }); averageStepTime /= nSteps; double timeBetweenSteps = std::max(1.0 * MIN_TIME_BETWEEN_STEPS_MS, 0.6 * averageStepTime); for (size_t i = mMagnitudeSize; i < mFilteredAccMagnitudes.size(); i++) { Magnitude curAcc = mFilteredAccMagnitudes[i]; Magnitude prevAcc = mFilteredAccMagnitudes[i-1]; //True if cross threshold and if new detection isn't too early. if (!mIsStep && prevAcc.value < MINIMAL_THRESHOLD_VALUE && curAcc.value > MINIMAL_THRESHOLD_VALUE && curAcc.ts - mPossibleStepTs > MIN_TIME_BETWEEN_STEPS_MS && timeBetweenSteps > 0) // && curAcc.ts - mPossibleStepTs > timeBetweenSteps { mIsStep = true; mPossibleStepTs = curAcc.ts; } if (mIsStep) { double stepLength = calculateStepLength(UPDATE_TIME_INTERVAL_MS, mFilteredAccMagnitudes.begin()+i); if (mPossibleStepTs - mStepTime < MAX_STEP_TIME) mTimes.push_back(mPossibleStepTs - mStepTime); steps.push_back(Step(mPossibleStepTs, stepLength)); #if defined (DEBUG_OUTPUT_PEDOMETER) printDebugPedometer("STEP", mPossibleStepTs, stepLength); #endif mStepTime = mPossibleStepTs; mIsStep = false; // prepare to detect new step } } while(mFilteredAccMagnitudes.back().ts - mFilteredAccMagnitudes.front().ts > 2 * UPDATE_TIME_INTERVAL_MS) mFilteredAccMagnitudes.pop_front(); while (mTimes.size() > MAXIMUM_NUMBER_OF_STEPS) mTimes.pop_front(); return steps; } double Pedometer::calculateStepLength(long long timeIntervalMs, std::deque::const_iterator rightBorderIt) { // find left border std::deque::const_iterator leftBorderIt = rightBorderIt; while (rightBorderIt->ts - leftBorderIt->ts <= timeIntervalMs && leftBorderIt != mFilteredAccMagnitudes.begin() ) leftBorderIt--; double maxMagn = std::max_element(leftBorderIt, rightBorderIt, [](Magnitude m1, Magnitude m2) {return m1.value < m2.value; })->value; double minMagn = std::min_element(leftBorderIt, rightBorderIt, [](Magnitude m1, Magnitude m2) {return m1.value < m2.value; })->value; double accAmplitude = maxMagn - minMagn; double stepLen = STEP_LENGTH_CONST * sqrt(sqrt(accAmplitude)); return stepLen; } #if defined (DEBUG_OUTPUT_PEDOMETER) void clearPedometerDebugOutput() { std::cout << "print debug output to " << PREPROCESSED_PEDOMETER_MSR_DEBUG_OUTPUT_FILE << std::endl; std::ofstream debugOutputFile; debugOutputFile.open(PREPROCESSED_PEDOMETER_MSR_DEBUG_OUTPUT_FILE); debugOutputFile << "type ts value" << std::endl; debugOutputFile.close(); } void printDebugPedometer(const std::string &type, long long ts, double value) { std::ofstream debugOutputFile; debugOutputFile.open(PREPROCESSED_PEDOMETER_MSR_DEBUG_OUTPUT_FILE, std::ofstream::out | std::ofstream::app); debugOutputFile << type << " " << ts << " " << value << std::endl; debugOutputFile.close(); } #endif } } // namespace navigine::navigation_core ================================================ FILE: src/sensor_fusion/pedometer.h ================================================ /** pedometer.h * * Copyright (c) 2018 Navigine. * */ #ifndef NAVIGINE_PEDOMETER_H #define NAVIGINE_PEDOMETER_H #include #include #include "../measurement_types.h" namespace navigine { namespace navigation_core { struct Step { long long ts; double len; Step() : ts(0), len(0) {}; Step(long long stepTs, double stepLength) : ts(stepTs), len(stepLength) {}; }; struct Magnitude { long long ts {}; double value {}; Magnitude() : ts(0), value(0) {}; Magnitude(long long msrTs, double magnitudeValue) : ts(msrTs), value(magnitudeValue) {}; }; /** * This class detects step of the human on the basis of accelerometer measurements * and empirical model of humans gate. * Step length is calculated using minimal and maximal accelerations */ class Pedometer { public: Pedometer() = default; void update(const SensorMeasurement& accMsr); std::deque calculateSteps(); private: Magnitude calculateFilteredAccMagnitudes() const; double calculateStepLength(long long timeIntervalMs, std::deque::const_iterator rightBorderIt); //we use deque in order to fast delete measurements of the array when we got new ones std::deque mAccelMeasurements; // Array for saving acceleration measurements std::deque mFilteredAccMagnitudes; // Array for saving filtered acceleration magnitudes std::deque mTimes; // Array for saving step durations std::size_t mMagnitudeSize = 0; long long mPossibleStepTs = -1; // The time when step possibly occurred (sec in unix) long long mStepTime = -1; // The time when previous step possibly occured (sec in unix) bool mIsStep = false; // We use this variable to mark possible step }; } } // namespace navigine::navigation_core #endif // NAVIGINE_PEDOMETER_H ================================================ FILE: src/sensor_fusion/quaternion.cpp ================================================ /** quaternion.cpp * * Copyright (c) 2018 Navigine. * */ #include #include "quaternion.h" namespace navigine { namespace navigation_core { Quaternion::Quaternion(double _w, double _x, double _y, double _z) : w{_w}, x{_x}, y{_y}, z{_z} { } Quaternion::Quaternion(double _x, double _y, double _z) : w{0}, x{_x}, y{_y}, z{_z} { } Quaternion::Quaternion(const Vector3d& vec) : w{0.0}, x{vec.x}, y{vec.y}, z{vec.z} { } Quaternion Quaternion::normalized() const { double norm = std::sqrt(w * w + x * x + y * y + z * z); return (std::fabs(norm) > std::numeric_limits::epsilon()) ? Quaternion(w /norm, x/norm, y / norm, z / norm) : *this; } std::tuple Quaternion::toEuler() const { double sinr = 2.0 * (w * x + y * z); double cosr = 1.0 - 2.0 * (x * x + y * y); auto roll = std::atan2(sinr, cosr); double sinp = 2.0 * (w * y - z * x); double pitch = 0.0; if (std::fabs(sinp) >= 1) pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range else pitch = asin(sinp); double siny = 2.0 * (w * z + x * y); double cosy = 1.0 - 2.0 * (y * y + z * z); double yaw = std::atan2(siny, cosy); return std::make_tuple(roll, pitch, yaw); } Quaternion& Quaternion::operator+=(const Quaternion& r_q) { this->w += r_q.w; this->x += r_q.x; this->y += r_q.y; this->z += r_q.z; return *this; } Quaternion& Quaternion::operator-=(const Quaternion& r_q) { this->w -= r_q.w; this->x -= r_q.x; this->y -= r_q.y; this->z -= r_q.z; return *this; } Quaternion& Quaternion::operator*=(const double& scale) { this->w *= scale; this->x *= scale; this->y *= scale; this->z *= scale; return *this; } Quaternion& Quaternion::operator*=(const Quaternion& other) { double qw = w, qx = x, qy = y, qz = z; this->w = qw*other.w - qx*other.x - qy*other.y - qz*other.z; this->x = qw*other.x + qx*other.w + qy*other.z - qz*other.y; this->y = qw*other.y - qx*other.z + qy*other.w + qz*other.x; this->z = qw*other.z + qx*other.y - qy*other.x + qz*other.w; return *this; } Quaternion operator+(Quaternion l_q, const Quaternion& r_q) { return l_q += r_q; } Quaternion operator-(Quaternion l_q, const Quaternion& r_q) { return l_q -= r_q; } Quaternion operator*(Quaternion l_q, const double& scale) { return l_q *= scale; } Quaternion operator*(const double& scale, Quaternion l_q) { return l_q *= scale; } Quaternion operator*(Quaternion l_q, const Quaternion& r_q) { return l_q *= r_q; } } } // namespace navigine::navigation_core ================================================ FILE: src/sensor_fusion/quaternion.h ================================================ /** quaternion.h * * Copyright (c) 2018 Navigine. * */ #ifndef NAVIGINE_QUATERNION_H #define NAVIGINE_QUATERNION_H #include #include #include #include #include namespace navigine { namespace navigation_core { class Quaternion { public: double w = 1.0; double x = 0.0; double y = 0.0; double z = 0.0; public: Quaternion(){}; Quaternion(double _w, double x, double _y, double _z); Quaternion(double _x, double _y, double _z); Quaternion(const Vector3d& vec); Quaternion normalized() const; Quaternion conj() const { return Quaternion(w, -x, -y, -z); } Vector3d toVector3d() const { return Vector3d(x, y, z); } std::tuple toEuler() const; Quaternion& operator+=(const Quaternion& r_q); Quaternion& operator-=(const Quaternion& r_q); Quaternion& operator*=(const double& scale); Quaternion& operator*=(const Quaternion& r_q); }; Quaternion operator+(Quaternion l_q, const Quaternion& r_q); Quaternion operator-(Quaternion l_q, const Quaternion& r_q); Quaternion operator*(Quaternion l_q, const double& scale); Quaternion operator*(const double& scale, Quaternion l_q); Quaternion operator*(Quaternion q1, const Quaternion& q2); } } // namespace navigine::navigation_core #endif //NAVIGINE_QUATERNION_H ================================================ FILE: src/sensor_fusion/sensor_fusion.cpp ================================================ #include #include #include #include "sensor_fusion.h" namespace navigine { namespace navigation_core { SensorFusion::SensorFusion(const NavigationSettings& navProps) : mUseCalcAzimuth (navProps.commonSettings.useCalculatedAzimuth) , mStepMultiplier (navProps.commonSettings.stepMultiplier) , mComplFilter ( ComplementaryFilter(navProps) ) , mPedometer ( Pedometer() ) { } MotionInfo SensorFusion::calculateDisplacement(const SensorMeasurement& msr, long long tsMs) { mPedometer.update(msr); std::deque steps = mPedometer.calculateSteps(); mComplFilter.update(msr); MotionInfo motionInfo; motionInfo.heading = -mComplFilter.getFusedAzimuth(); // the counter-clockwise rotation needed if (mUseCalcAzimuth) { motionInfo.azimuth = mComplFilter.getMagneticAzimuth(); motionInfo.isAzimuthValid = true; } else if (!mComplFilter.isDeviceAzimuthOutdated()) { motionInfo.azimuth = mComplFilter.getDeviceAzimuth(); motionInfo.isAzimuthValid = true; } motionInfo.gyroHeading = mComplFilter.getGyroHeading(); motionInfo.isStepDetected = !steps.empty(); if (motionInfo.isStepDetected) { motionInfo.deltaAngle = motionInfo.heading - mPrevHeading; mPrevHeading = motionInfo.heading; motionInfo.distance = std::accumulate(steps.begin(), steps.end(), 0.0, [](double sum, const Step& s) { return sum + s.len; }); motionInfo.distance *= mStepMultiplier; motionInfo.lastMotionTs = tsMs; mStepCounter += steps.size(); motionInfo.stepCounter = mStepCounter; } return motionInfo; } } } // namespace navigine::navigation_core ================================================ FILE: src/sensor_fusion/sensor_fusion.h ================================================ /** sensor_measurement.h * * Copyright (c) 2018 Navigine. * */ #ifndef _SENSOR_FUSION_H_ #define _SENSOR_FUSION_H_ #include #include #include #include #include "complementary_filter.h" #include "pedometer.h" namespace navigine { namespace navigation_core { class SensorFusion { public: SensorFusion(const NavigationSettings& navProps); MotionInfo calculateDisplacement(const SensorMeasurement& msr, long long tsMs); private: const bool mUseCalcAzimuth; const double mStepMultiplier; ComplementaryFilter mComplFilter; Pedometer mPedometer; int mStepCounter = 0; double mPrevHeading = 0.0; }; } } // namespace navigine::navigation_core #endif // _SENSOR_FUSION_H_ ================================================ FILE: src/triangulation.cpp ================================================ #include "triangulation.h" namespace navigine { namespace navigation_core { static const double INDENT_METERS = 10.0; std::vector TriangulateVertices(const std::vector& vertices, double maxedge) { std::vector T; if (vertices.size() < 3) return T; // Borders of the rectangle double minX = std::numeric_limits::max(); double maxX = -std::numeric_limits::max(); double minY = std::numeric_limits::max(); double maxY = -std::numeric_limits::max(); // find rectangle containing all the vertices for (std::vector::const_iterator it = vertices.begin(); it != vertices.end(); it++) { const TriangleVertex& v = *it; if (v.x > maxX) maxX = v.x; if (v.x < minX) minX = v.x; if (v.y > maxY) maxY = v.y; if (v.y < minY) minY = v.y; } // Adding indents to avoid checks for boundary cases minX -= INDENT_METERS; maxX += INDENT_METERS; minY -= INDENT_METERS; maxY += INDENT_METERS; std::vector V; // Defining "super-triangle" V.push_back(TriangleVertex(minX + minY - maxY, minY, ReferencePointId("super1"))); V.push_back(TriangleVertex(maxX + maxY - minY, minY, ReferencePointId("super2"))); V.push_back(TriangleVertex((minX + maxX) / 2, maxY + (maxX - minX) / 2, ReferencePointId("super3"))); V.insert(V.end(), vertices.begin(), vertices.end()); T.push_back(Triangle(V[0], V[1], V[2])); // Adding vertices to the triangulation for(size_t i = 0; i < V.size(); ++i) { // Looking for triangles, for which the circumscribed circle contains V[i] // Removing those triangles, saving their edges in edge buffer E std::vector E; for(size_t j = 0; j < T.size(); ) { if (T[j].circumcircle_contains(V[i])) { E.push_back(T[j].getAB()); E.push_back(T[j].getBC()); E.push_back(T[j].getCA()); T.erase(T.begin() + j); continue; } ++j; } // Adding all triangles between edges and current vertex // ignoring the duplicating edges std::sort(E.begin(), E.end()); for(size_t j = 0; j < E.size(); ++j) { if (j + 1 < E.size() && E[j] == E[j+1]) ++j; else T.push_back(Triangle(V[i], E[j].begin(), E[j].end())); } } double minsin = -1; for(size_t i = 0; i < T.size(); ++i) { if (T[i].getA() == V[0] || T[i].getB() == V[0] || T[i].getC() == V[0] || T[i].getA() == V[1] || T[i].getB() == V[1] || T[i].getC() == V[1] || T[i].getA() == V[2] || T[i].getB() == V[2] || T[i].getC() == V[2] || T[i].getAB().length() > maxedge || T[i].getBC().length() > maxedge || T[i].getAC().length() > maxedge || T[i].sinA() < minsin || T[i].sinB() < minsin || T[i].sinC() < minsin) T.erase(T.begin() + i--); } V.erase(V.begin(), V.begin() + 3); // Erasing elements V[0], V[1], V[2] std::sort(V.begin(), V.end()); std::sort(T.begin(), T.end()); return T; } } } // namespace navigine::navigation_core ================================================ FILE: src/triangulation.h ================================================ #ifndef NAVIGINE_TRIANGULATION_H #define NAVIGINE_TRIANGULATION_H #include #include #include #include "geometry.h" #include namespace navigine { namespace navigation_core { struct TriangleVertex { TriangleVertex(double x_, double y_, ReferencePointId id_): x(x_), y(y_), id(id_) { } bool operator==(const TriangleVertex& v)const { return id == v.id; } bool operator!=(const TriangleVertex& v)const { return id != v.id; } bool operator< (const TriangleVertex& v)const { return x < v.x || (x == v.x && y < v.y); } bool operator> (const TriangleVertex& v)const { return x > v.x || (x == v.x && y > v.y); } bool operator<=(const TriangleVertex& v)const { return !(*this > v); } bool operator>=(const TriangleVertex& v)const { return !(*this < v); } double x; double y; ReferencePointId id; }; class TriangleEdge { public: TriangleEdge(const TriangleVertex& a, const TriangleVertex& b) : mBegin(std::min(a, b)), mEnd(std::max(a, b)) { } TriangleVertex begin()const { return mBegin; } TriangleVertex end()const { return mEnd; } double length()const { return sqrt((mBegin.x - mEnd.x) * (mBegin.x - mEnd.x) + (mBegin.y - mEnd.y) * (mBegin.y - mEnd.y)); } bool operator==(const TriangleEdge& e)const { return mBegin == e.mBegin && mEnd == e.mEnd; } bool operator!=(const TriangleEdge& e)const { return mBegin != e.mBegin || mEnd != e.mEnd; } bool operator< (const TriangleEdge& e)const { return mBegin < e.mBegin || (mBegin == e.mBegin && mEnd < e.mEnd); } bool operator> (const TriangleEdge& e)const { return mBegin > e.mBegin || (mBegin == e.mBegin && mEnd > e.mEnd); } bool operator<=(const TriangleEdge& e)const { return !(*this > e); } bool operator>=(const TriangleEdge& e)const { return !(*this < e); } private: TriangleVertex mBegin; TriangleVertex mEnd; }; class Triangle { public: Triangle(const TriangleVertex& a, const TriangleVertex& b, const TriangleVertex& c) : mA(a), mB(b), mC(c) { } TriangleVertex getA()const { return mA; } TriangleVertex getB()const { return mB; } TriangleVertex getC()const { return mC; } TriangleEdge getAB()const { return TriangleEdge(mA, mB); } TriangleEdge getBA()const { return TriangleEdge(mA, mB); } TriangleEdge getBC()const { return TriangleEdge(mB, mC); } TriangleEdge getCB()const { return TriangleEdge(mB, mC); } TriangleEdge getAC()const { return TriangleEdge(mA, mC); } TriangleEdge getCA()const { return TriangleEdge(mA, mC); } bool operator==(const Triangle& t)const { return mA == t.mA && mB == t.mB && mC == t.mC; } bool operator!=(const Triangle& t)const { return mA != t.mA || mB != t.mB || mC != t.mC; } bool operator< (const Triangle& t)const { return mA < t.mA || (mA == t.mA && mB < t.mB) || (mA == t.mA && mB == t.mB && mC < t.mC); } bool operator> (const Triangle& t)const { return mA > t.mA || (mA == t.mA && mB > t.mB) || (mA == t.mA && mB == t.mB && mC > t.mC); } bool operator<=(const Triangle& t)const { return !(*this > t); } bool operator>=(const Triangle& t)const { return !(*this < t); } double square()const { return 0.5 * fabs(Determinant(mA.x, mB.x, mC.x, mA.y, mB.y, mC.y, 1, 1, 1)); } double sinA()const { return 2 * square() / (getAC().length() * getAB().length()); } double sinB()const { return 2 * square() / (getBC().length() * getAB().length()); } double sinC()const { return 2 * square() / (getBC().length() * getAC().length()); } bool circumcircle_contains(const TriangleVertex& X)const { if (X == mA || X == mB || X == mC) return false; double D1 = Determinant(mA.x, mB.x, mC.x, mA.y, mB.y, mC.y, 1, 1, 1); double D2 = Determinant(mA.x - X.x, mA.y - X.y, (mA.x * mA.x - X.x * X.x) + (mA.y * mA.y - X.y * X.y), mB.x - X.x, mB.y - X.y, (mB.x * mB.x - X.x * X.x) + (mB.y * mB.y - X.y * X.y), mC.x - X.x, mC.y - X.y, (mC.x * mC.x - X.x * X.x) + (mC.y * mC.y - X.y * X.y)); return D1 * D2 > GEOMETRY_DOUBLE_EPSILON; } private: TriangleVertex mA; TriangleVertex mB; TriangleVertex mC; }; std::vector TriangulateVertices(const std::vector &vertices, double maxedge); } } // namespace navigine::navigation_core #endif // TRIANGULATION_H ================================================ FILE: src/trilateration.cpp ================================================ /** trilateration.cpp * * Copyright (c) 2021 Navigine. * */ #include "trilateration.h" #include namespace navigine { namespace navigation_core { RadioMeasurementsData getIntersectedMeasurements(const Level &level, const RadioMeasurementsData& radioMsr) { RadioMeasurementsData newMsr = radioMsr; RadioMeasurementsData badMsr; // Sorting distances in ascending order std::sort(newMsr.begin(), newMsr.end(), [](const RadioMeasurementData& lhs, const RadioMeasurementData& rhs) { return lhs.distance < rhs.distance; }); // Method for determining the Access Point that fails for (const auto& msr1 : newMsr) { if (std::find_if(badMsr.begin(), badMsr.end(), [msr1](const RadioMeasurementData& msr) { return msr1.id == msr.id; }) == badMsr.end()) { Transmitter tx1 = level.transmitter(msr1.id); const double x1 = tx1.point.x; const double y1 = tx1.point.y; const double z1 = tx1.point.z; for (const auto& msr2 : newMsr) { Transmitter tx2 = level.transmitter(msr2.id); const double x2 = tx2.point.x; const double y2 = tx2.point.y; const double z2 = tx2.point.z; const double dx = x2 - x1; const double dy = y2 - y1; const double dz = z2 - z1; const double delta = std::sqrt(dx * dx + dy * dy + dz * dz); if (delta > (msr1.distance + msr2.distance)) badMsr.push_back(msr2); } } } // Update measurement vector with bad Access Points for (const auto& msr : badMsr) { newMsr.erase(std::remove_if(newMsr.begin(), newMsr.end(), [msr](const RadioMeasurementData& elem) { return msr.id == elem.id; } ),newMsr.end()); } return newMsr; } boost::optional Trilateration::calculateAltitude(const Level& level, const RadioMeasurementsData& radioMeasurements) { const RadioMeasurementsData levelMsrs = getLevelRadioMeasurements(level, radioMeasurements); const RadioMeasurementsData radioMsrs = getIntersectedMeasurements(level, levelMsrs); if (radioMsrs.size() < 4) return boost::none; // Define the matrix that we are going to use size_t count = radioMsrs.size(); size_t rows = count - 1; Eigen::MatrixXd m(rows, 3); Eigen::VectorXd b(rows); // Fill in matrix according to the equations size_t row = 0; double x1, x2, y1, y2, z1, z2, r1, r2; Transmitter tx1 = level.transmitter(radioMsrs[0].id); x1 = tx1.point.x, y1 = tx1.point.y, z1 = tx1.point.z; r1 = radioMsrs[0].distance; for (size_t i = 1; i < count; ++i) { Transmitter tx2 = level.transmitter(radioMsrs[i].id); x2 = tx2.point.x, y2 = tx2.point.y, z2 = tx2.point.z; r2 = radioMsrs[i].distance; m(row, 0) = x2 - x1; m(row, 1) = y2 - y1; m(row, 2) = z2 - z1; b(row) = (pow(r1, 2) - pow(r2, 2) + (pow((x2 - x1), 2) + pow((y2 - y1), 2) + pow((z2 - z1), 2))) / 2; row++; } Eigen::Vector3d rawLocation = m.bdcSvd(Eigen::ComputeThinU|Eigen::ComputeThinV).solve(b); double x = rawLocation[0] + x1; double y = rawLocation[1] + y1; double sum1 = 0.0; // double sum2 = 0.0; bool nanFlag = true; for (const auto& msr : radioMsrs) { Transmitter tx = level.transmitter(msr.id); double deltaZ = std::sqrt(pow(msr.distance, 2) - pow((x - tx.point.x), 2 - pow((y - tx.point.y), 2))); if (!std::isnan(deltaZ)) { nanFlag = false; sum1 += (tx.point.z + deltaZ) / msr.distance; //if the tracked position is below all beacons, then use the sign '-', else: '+' //in theory it should also be calculated: sum2 += 1.0 / msr.distance, but in practice gives worse results } } if (nanFlag) return boost::none; else return sum1; //sum1 / sum2; } } } // namespace navigine::navigation_core ================================================ FILE: src/trilateration.h ================================================ #pragma once #include #include namespace navigine { namespace navigation_core { class Trilateration { public: boost::optional calculateAltitude(const Level& level, const RadioMeasurementsData& radioMeasurements); }; } } // namespace navigine::navigation_core ================================================ FILE: src/vector3d.cpp ================================================ /** vector3d.cpp * ** Copyright (c) 2018 Navigine. * */ #include #include namespace navigine { namespace navigation_core { namespace { static constexpr double DOUBLE_EPSILON = 1e-8; } Vector3d::Vector3d(double _x, double _y, double _z) { x = _x; y = _y; z = _z; } double Vector3d::magnitude() const { return std::sqrt(x*x + y*y + z*z); } Vector3d Vector3d::normalized() const { double length = this->magnitude(); return (std::fabs(length) > std::numeric_limits::epsilon()) ? Vector3d(x / length, y / length, z / length) : *this; } Vector3d& Vector3d::normalize() { double length = this->magnitude(); if (std::fabs(length) > std::numeric_limits::epsilon()) *this /= length; return *this; } bool Vector3d::isNull() const { return x == 0 && y == 0 && z == 0; } bool Vector3d::operator==(const Vector3d& v) { return std::fabs(this->x - v.x) < DOUBLE_EPSILON && std::fabs(this->y - v.y) < DOUBLE_EPSILON && std::fabs(this->z - v.z) < DOUBLE_EPSILON; } bool Vector3d::operator!=(const Vector3d& v) { return !(*this == v); } Vector3d& Vector3d::operator+=(const Vector3d& v) { this->x += v.x; this->y += v.y; this->z += v.z; return *this; } Vector3d& Vector3d::operator-=(const Vector3d& v) { this->x -= v.x; this->y -= v.y; this->z -= v.z; return *this; } Vector3d& Vector3d::operator*=(double multiplier) { this->x *= multiplier; this->y *= multiplier; this->z *= multiplier; return *this; } Vector3d& Vector3d::operator/=(double divisor) { this->x /= divisor; this->y /= divisor; this->z /= divisor; return *this; } double Vector3d::dotProduct(const Vector3d& v1, const Vector3d& v2) { return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; } Vector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x); } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/README.md ================================================ # Indoor-navigation-algorithms This repository includes a set of different algorithms combined in Navigine core positioning library. The application scope of Navigine positioning library includes (and is not limited to) cases of indoor navigation in airports, shopping malls, universities, and so on. ## Useful Links - Refer to the [Navigine official website](https://navigine.com/) for complete list of downloads, useful materials, information about the company, and so on. - [Get started](http://client.navigine.com/login) with Navigine to get full access to Navigation services, SDKs, and applications. - Refer to the Navigine [User Manual](http://docs.navigine.com/) for complete product usage guidelines. - Find company contact information at the official website under [Contacts](https://navigine.com/contacts/) tab. ## Project roadmap for 2021 Q1 ### January-February 2021 Add positioning block utilizing simple positioning algorithm and all the processing blocks: - Add C++ Code to repository - Add set of tests for position estimation - Add comprehensive documentation and references to related scientific articles. ### March 2021 Add optional positioning algorithms to be used in core positioning block - Add C++ Code - Add set of tests for position estimation ### April 2021 Add first version on indoor navigation algorithms - Add C++ basic code - Add set of tests for algorithms ### May 2021 - Upfload first version of navgine navigation library ================================================ FILE: standalone_algorithms/System description.md ================================================ The performance of indoor positioning system depends on correct description of building structure that is used for navigation and correct installation of infrastructure such as various signals transmitters, beacons and base stations. To describe the indoor environment we propose the API of indoor positioning algorithms described below. ## Navigation structures Any indoor space, such as building or warehouse is further referred to as `Location` and is represented by a set of 2D (plane) areas called `Level`'s. A typical example of `Location` is a multistored building, where each `Level`s corresponds to one of the floors of the building. ![Alt text](illustrations/Location.jpg?raw=true "Location") [Figure 1. Location with levels] Each `Level` is specified by all the data, necessary to calculate position on this `Level`. This data includes: geometry of the `Level` (walls, open/closed areas, bounding box), route graph (information about available routes on this `Level`), list of installed devices, called `Transmitters` (bluetooth beacons, wifi base stations, etc) and collected radio map (if present) that is called RSSI fingerprints [1, 2]. All these pieces of data will be described below. ![Alt text](illustrations/Level.jpg?raw=true "Level") [Figure 2. The scheme of the level] All these infrastructure is described in the following struct: ```cpp struct Level { Id id; ... std::vector transmitters; std::map referencePoints; Geometry geometry; Graph graph; }; ``` ### Walls, open areas and routes The physical structure of the `Level` is described by struct `Geometry` which includes boundind box `Box` of the `Level` area and the set of polygons of free space joint together in `Multipolygon` class. Bounding box is basically a rectangle in physical space defining the boundaries of the area, selected for positioning. Allowed Area (`Multipolygon`) is necessary to introduce a natural restriction on possible movements inside the area of this `Level`. These restrictions maybe introduced by closed rooms, walls, holes, and etc. Each of such obstacle is presented by a `Polygon` in physical space. And altogether these obstacle are joined in a `Multipolygon` structure. During positioning, only the area, that is not covered by this `Multipolygon` is available for positioning. ```cpp struct Geometry { Multipolygon allowedArea; Box boundingBox; }; ``` `Multipolygon` here is just an alias for boost multi_polygon: `using Multipolygon = boost::geometry::model::multi_polygon;` Most of positioning engines must provide the functionality of building Routes (between points A and B, or from current position to some destination point B) on top of positioning. To make it possible, `Level` should contain routing information. This information is presented by a set of physical points - `Vertices`, and a set of links (`Edged`) between them. A route can be built from point `A` to point `B` only if these points are connencted by a multi-link of `Edges`. Altogether, `Vertices` and `Edges` form a mathmetical structure called `Graph` of routes. ```cpp struct Graph { struct Vertex { double x; double y; }; std::vector vertices; std::set> edges; }; ``` ### Transmitters The indoor positioning system can use different technologies to calculate position of a mobile object. One of the most popular technology is based on measurements of received signal strength from base stations or beacons with known positions to a mobile tag. The signal strength can then be transformed to distance to an object. The set of such distances with corresponding coordinates of base stations is used to perform thrilateration and to calculate the position. The struct `Transmitter` is used to describe the type, the position and other properties of a base station or a beacon. The essential parameters of each transmitter are: its position on Level (`x`,`y`) and its unique identificator `id`. `Position` of device is essential as all solvers are based on measurements of distance (some of device can measure distance explicitly, while other provide some other types of signal, that will be converted to distance using existing physical formulas) to the transmitter. Thus to get correct position estimate, one has to correctly specify the positions of all transmitters. `ID` of device is essential as it is used to match the signal, received on the device side, to the specific transmitter from which this signal was emitted. Other parameters include the type `Type` of the device and parameters, specific to different types of devices. ```cpp struct Transmitter { enum class Type { WIFI, BEACON, BLUETOOTH, RTT, UNKNOWN }; Id id; double x; double y; Type type; ... //other parameters describing transmition power, etc ... }; ``` ### RSS Fingerprints Another way of calculation of position of a mobile device is to use radiomap of received signal strength of available stationary devices such as wifi hotspots and ble devices. This approach is used even when position of base stations are not known. The idea is based on comparison of the set of wifi or ble signals registered by mobile device at current time moment with statistics of corresponding signals registered on different positions inside the building captured during preliminary experiments. The set of all available signals `SignalStatistics` captured during preliminary experiments in given position from different transmitters is stored in `std::map fingerprints`. These information together with coordinates of the points, at which the signals were registered, is joint into `ReferencePoint` struct. To describe radiomap on certain `Level` the map of `ReferencePoint` with corresponding ids is used ```cpp struct SignalStatistics { Transmitter::Type type; double mean; double variance; int nMeasurements; ... }; struct ReferencePoint { Id id; double x; double y; double magnIntensity; std::map fingerprints; ... }; ``` ### References 1. Panyov, A., Golovan, A. A., Smirnov, A. S., Kosyanchuk, V. V. Indoor Positioning Using Wi-Fi Fingerprinting, Magnetometer and Redestrian Dead Reckoning. 21st Saint Petersburg International Conference on Integrated Navigation Systems. Saint-Petersburg 2014 2. Panyov, A., Golovan, A. A., Smirnov, A. S., Kosyanchuk, V. V. Efficient Localization Using Different Mean Offset Models In Gaussian Processes. Proceeding of the 5th IEEE International Conference on Indoor Positioning Indoor Navigation 2014 ================================================ FILE: standalone_algorithms/complementary_filter/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.7) project(Navigation-core) option(BUILD_TESTS "Build tests" ON) option(BUILD_EXAMPLES "Build examples" OFF) set(BOOST_ENABLE_CMAKE ON) set(Boost_USE_STATIC_LIBS OFF) find_package(Boost COMPONENTS system unit_test_framework REQUIRED) IF (Boost_FOUND) INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR}) message(${Boost_INCLUDE_DIR}) include_directories(${Boost_SOURCE_DIR}) ENDIF () set("target" "complementary-filter") set("test-target" "test-filter") if(BUILD_TESTS) file(GLOB ${PROJECT_NAME}_TEST_SOURCES src/*.cpp tests/*.cpp) add_executable("${test-target}" ${${PROJECT_NAME}_TEST_SOURCES}) target_include_directories("${test-target}" PRIVATE "include/") set_property(TARGET "${test-target}" PROPERTY CXX_STANDARD 17) set_target_properties("${test-target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;") set_target_properties("${test-target}" PROPERTIES COMPILE_FLAGS "-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions ") target_link_libraries("${test-target}" #"-lboost_unit_test_framework" # Boost::unit_test_framework ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} ) endif() if(BUILD_EXAMPLES) list(APPEND "${target}__cxx_srcs" "examples/main.cpp" "src/complementary_filter.cpp" "src/quaternion.cpp" "src/vector3d.cpp") add_executable("${target}" ${${target}__cxx_srcs}) target_include_directories("${target}" PRIVATE "include/") set_property(TARGET "${target}" PROPERTY CXX_STANDARD 17) set_target_properties("${target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;") set_target_properties("${target}" PROPERTIES COMPILE_FLAGS "-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions ") endif() ================================================ FILE: standalone_algorithms/complementary_filter/README.md ================================================ ### Complementary filter Complementary filter fuses data from IMU (Intertial Measurement Units) sensors such as accelerometer, magnetometer, gyroscope and pedometer. Configuration of sensors that are in use during the position estimation process may vary depending on the quality of signal from sensors. For instance, it is common to work with accelerometer and gyroscope, rejecting other sensors if signal from corresponding sensors is noisy. The example directory contains code that simulates IMU measuremetns of pendulum and estimates pendulum orientation with the complementary filter. It is possible to plot the result of orientation estimation with python script located at helpers directory. ### What is a complementary filter The basic complementary filter is shown in a picture below. Markdown Monster icon Where z is an input signal, x and y are noisy measurements of this signal. an estimation of the output signal produced buy the filter. Assume that the noise in y is mostly high frequency, and the noise in x is mostly low frequency. Then G(s) can be made a low-pass filter to filter out the high-frequency noise in y. If G(s) is low-pass, [1 - G(s)] is the complement, i.e., a high-pass filter which filters out the low-frequency noise in x. The complementary filter can be reconfigured as in figure B. In this case the input to G(s) is

so that the filter G(s) just operates on the noise or error in the measurements x and y. In the case of noise less or error-free measurements

i.e., the signal is estimated perfectly. Gyroscope obtains the angular position of the object by integrating the angular velocity over time. Gyroscope is a very precise sensor and not susceptible to external forces. But because of integration, the measurement tends to drift, not returning to zero when the system goes back to its original position. Accelerometer is a sensor which takes into consideration every small forse therefore it can be easily disturbed. That is why the accelerometer data is reliable only on a long term. The advantage of accelerometer is that it does not drift. A magnetometer is a device that measures magnetic field or magnetic dipole moment. In case if the measurements from different sources give precise information in different frequency regions, it is convenient to use a weighted sum of measurements from these sources. [//]: # "angle = alpha * (angle + gyrData * dt) + beta * accData + gamma * magnData)"

angle = \alpha -10\mu \log \frac{d}{d_0} + w

where alpha, beta, gamma are constant weights of different measurement sources. ### Low and high pass filters Low-pass filter is a filter that passes signals with a frequency lower than a selected cutoff frequency. Transfer function of low-pass filter [//]: # "https://en.wikipedia.org/wiki/Low-pass_filter"

where is a cutoff frequency of a filter Low-pass filter is the complement of a high-pass filter. High-pass filter is a filter that passes signals with a frequency higher than a certain cutoff frequency. Transffer function of high-pass filter

Consider a first-order integrator

with the folowing measurement characteristics

Measurements can be fused into an estimate of a state via the filter

A filter is called a complementary filter if

### Comparison with Kalman filter Complementary filter outperforms Kalman filter significantly by using less computational and processing power and providing more accuracy. The Complementary filter can be applied by having only vector and quaternion mathematical operators. On other hand, the traditional Kalman filter needs an enormous number of matrix operations, including multiplications and taking inverses of these matrices, which, besides the complexity, also results in high computational and processing costs. ### Calculations Acceleration is stored in quaternion form. The weight part of it is a numeric value of the acceleration, the vector part -- vector components. The goal of developing an estimator is to provide a smooth estimate of a state R(t) that is evolving due to some external input based on a set of measurements. Frame of reference allows to determine the error. It can be calculated as follows

The direct complementary filter dynamics are specified by

The R* operation is an inverse operation on SO(3) The kinematics can be written directly in terms of the quaternion representation of SO(3) by

Quaternion is multiplied by the pure rotation quaternion , where real part is equal to zero. In the source code provided 'updateQuaternion(mQ, rotation)' function which does the same calculations. The gyroscope data is integrated every timestep with the current angle value: ```cpp omega = gyro * dt + mIntegralError * dt; ``` ### Components The following constant coefficients are the weights applied to accelerometer, gyroscope and magnetometer components. ```cpp double mKaccelerometer; double mKmagnetometer; double mKintegralGain; ``` ### Build To build the project for running tests make sure that `BUILD_TESTS` option is turned on. ```sh cd /standalone_algorithms/complementary_filter cmake -Bbuild -H. cmake --build build ``` Run tests: ```sh ./build/test-filter ``` ### Results of orientation Markdown Monster icon ================================================ FILE: standalone_algorithms/complementary_filter/examples/main.cpp ================================================ #include #include #include #include #include using namespace navigine::navigation_core; static const double MAGNETIC_FLUX_DENSITY = 0.48; static const double MAGNETIC_FIELD_INCLINATION = 60.48 * M_PI / 180; static const double G = 9.81; const std::string TEST_DATA_FOLDER = "test_data/"; /** * duration - duration of pendulum motion in seconds * freq - sampling frequency in Hz * L - pendulum length * theta0 - initial angle in radians * * Returns: * the angle, angular velocity and angular acceleration for pendulum motion */ std::tuple, std::vector, std::vector> simulatePendulumMotion(double durationStaticSec, double durationDynamicSec, double freqHz = 100, double L = 1.0, double theta0 = M_PI / 60.0) { std::vector times; std::vector thetas; std::vector thetaVels; double t = 0; double tick = 1.0 / freqHz; //first 10 seconds keep static pendulum tilt for (int i = 0; i < durationStaticSec / tick; i++) { times.emplace_back(t); thetas.emplace_back(theta0); thetaVels.emplace_back(0); t += tick; } double thetaPrev = theta0; for (int i = 0; i < durationDynamicSec / tick; i++) { double theta = theta0 * cos(sqrt(G / L) * t); double thetaVel = (theta - thetaPrev) / tick; times.emplace_back(t); thetas.emplace_back(theta); thetaVels.emplace_back(thetaVel); t += tick; thetaPrev = theta; } return std::make_tuple(times, thetas, thetaVels); } std::tuple, std::vector, std::vector> generateImuPendulum(std::vector t, std::vector theta, std::vector thetaVel, double freqHz = 100, double L = 1) { long long refTs = 1583315232492; std::random_device rd; std::mt19937 gen(rd()); std::vector accelMeasurements; for (std::size_t i = 0; i < t.size(); i++) { //the variance is choosen to be equal to typical accelerometer noise variance of cheap IMU sensor double var = 2 * M_PI * freqHz * G * pow(0.001, 2); std::normal_distribution distr(0.0, var); double ax = -G * sin(theta[i]) + distr(gen); double ay = 0.0 + distr(gen); double az = -L * pow(thetaVel.at(i), 2) + G * cos(theta[i]) + distr(gen); SensorMeasurement msr; msr.ts = refTs + 1000 * t.at(i); msr.values = Vector3d(ax, ay, az); msr.type = SensorMeasurement::Type::ACCELEROMETER; accelMeasurements.emplace_back(msr); } std::vector gyroMeasurements; for (std::size_t i = 0; i < t.size(); i++) { //the variance is choosen to be equal to typical gyroscope noise variance of cheap IMU sensor double var = 2 * M_PI * freqHz * pow(0.01 * M_PI / 180.0, 2); std::normal_distribution distr(0.005, var); double wx = 0.0 + distr(gen); double wy = thetaVel.at(i) + distr(gen); double wz = 0.0 + distr(gen); SensorMeasurement msr; msr.ts = refTs + 1000 * t.at(i); msr.values = Vector3d(wx, wy, wz); msr.type = SensorMeasurement::Type::GYROSCOPE; gyroMeasurements.emplace_back(msr); } std::vector magnMeasurements; for (std::size_t i = 0; i < t.size(); i++) { double mx = MAGNETIC_FLUX_DENSITY * cos(MAGNETIC_FIELD_INCLINATION + theta[i]); double my = 0.0; double mz = MAGNETIC_FLUX_DENSITY * sin(MAGNETIC_FIELD_INCLINATION + theta[i]); SensorMeasurement msr; msr.ts = refTs + 1000 * t.at(i); msr.values = Vector3d(mx, my, mz); msr.type = SensorMeasurement::Type::MAGNETOMETER; magnMeasurements.emplace_back(msr); } return std::make_tuple(accelMeasurements, gyroMeasurements, magnMeasurements); } int main() { std::string testDataFolder = TEST_DATA_FOLDER; std::vector times; std::vector thetas; std::vector thetaVels; std::tie(times, thetas, thetaVels) = simulatePendulumMotion(10, 10); std::vector accelMeasurements; std::vector gyroMeasurements; std::vector magnMeasurements; std::tie(accelMeasurements, gyroMeasurements, magnMeasurements) = generateImuPendulum(times, thetas, thetaVels); std::ofstream os; std::string simulatedMsrFileName = testDataFolder + "simulation.log"; os.open(simulatedMsrFileName); for (std::size_t i = 0; i < times.size(); i++) { SensorMeasurement accelMsr = accelMeasurements.at(i); SensorMeasurement gyroMsr = gyroMeasurements.at(i); SensorMeasurement magnMsr = magnMeasurements.at(i); double theta = thetas.at(i); os << accelMsr.ts << " " << theta << " " << accelMsr.values.x << " " << accelMsr.values.y << " " << accelMsr.values.z << " " << gyroMsr.values.x << " " << gyroMsr.values.y << " " << gyroMsr.values.z << " " << magnMsr.values.x << " " << magnMsr.values.y << " " << magnMsr.values.z << std::endl; } os.close(); std::cout << "simulated data was written to " << simulatedMsrFileName << std::endl; ComplementaryFilter complementaryFilter; std::string calculatedAngleFileName = testDataFolder + "calculated_angles.log"; os.open(calculatedAngleFileName); for (std::size_t i = 0; i < accelMeasurements.size(); i++) { const SensorMeasurement& accelMsr = accelMeasurements.at(i); const SensorMeasurement& gyroMsr = gyroMeasurements.at(i); const SensorMeasurement& magnMsr = magnMeasurements.at(i); complementaryFilter.update(accelMsr); complementaryFilter.update(gyroMsr); complementaryFilter.update(magnMsr); Orientation ori = complementaryFilter.getFusedOrientation(); os << accelMsr.ts << " " << ori.roll << " " << ori.pitch << " " << ori.yaw << std::endl; } os.close(); std::cout << "calculated angles were written to " << calculatedAngleFileName << std::endl; return 0; } ================================================ FILE: standalone_algorithms/complementary_filter/helpers/plot_angles.py ================================================ import pandas as pd import matplotlib.pylab as plt import numpy as np df1 = pd.read_csv('./../test_data/simulation.log', sep=' ', index_col=None, names=['ts', 'angle', 'ax', 'ay', 'az', 'wx', 'wy', 'wz', 'mx', 'my', 'mz']) df2 = pd.read_csv('./../test_data/calculated_angles.log', sep=' ', names=['ts', 'roll', 'pitch', 'yaw']) t = (df1['ts'] - df1['ts'][0]) / 1000.0 plt.plot(t, 180 * df1.angle / 3.14, '-', label='simulated') plt.plot(t, 180 * df2.roll / 3.14, '-', label='roll') plt.plot(t, 180 * df2.pitch / 3.14, '-', label='pitch') plt.plot(t, 180 * df2.yaw / 3.14, '-', label='yaw') plt.legend() plt.show() ================================================ FILE: standalone_algorithms/complementary_filter/include/complementary_filter.h ================================================ #pragma once #include #include namespace navigine { namespace navigation_core { struct SensorMeasurement { enum class Type { ACCELEROMETER, MAGNETOMETER, GYROSCOPE, BAROMETER, LOCATION, ORIENTATION}; Type type; long long ts = -1; Vector3d values = Vector3d(); }; double to_minus_Pi_Pi(double x); struct Orientation { double roll = 0.0; double pitch = 0.0; double yaw = 0.0; long long ts = 0; Orientation(double roll_, double pitch_, double yaw_, long long ts_) : roll {roll_ } , pitch {pitch_} , yaw {yaw_ } , ts {ts_ } {} }; class ComplementaryFilter { public: ComplementaryFilter () {} ComplementaryFilter (double Ka, double Km, double Ki) : mKaccelerometer {Ka} , mKmagnetometer {Km} , mKintegralGain {Ki} {} double getMagneticAzimuth() const; double getFusedAzimuth() const; double getDeviceAzimuth() const; Orientation getFusedOrientation() const; void update(const SensorMeasurement& msr); private: void updateUsingGyroscope (const SensorMeasurement& gyro); void updateUsingAccelerometer (const SensorMeasurement& accel); void updateUsingMagnetometer (const SensorMeasurement& magn); double caclulateMagneticAzimuth (const SensorMeasurement& magn); long long mCurrentTs = -1; long long mLastGyroTs = -1; double mKaccelerometer = 0.1; double mKmagnetometer = 0.05; double mKintegralGain = 0.0; Vector3d mIntegralError = {}; Quaternion mQ = {}; double mMagneticAzimuth = 0.0; double mDeviceAzimuth = 0.0; SensorMeasurement mMagnLowPassed; // Magnetometer measurements passed through low-pass filter SensorMeasurement mAccelLowPassed; // Accelerometer measurements passed through low-pass filter }; } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/complementary_filter/include/quaternion.h ================================================ #pragma once #include #include #include #include #include namespace navigine { namespace navigation_core { class Quaternion { public: double w = 1.0; double x = 0.0; double y = 0.0; double z = 0.0; public: Quaternion(){} Quaternion(double _w, double x, double _y, double _z); Quaternion(double _x, double _y, double _z); Quaternion(const Vector3d& vec); Quaternion normalized() const; Quaternion conj() const { return Quaternion(w, -x, -y, -z); } Vector3d toVector3d() const { return Vector3d(x, y, z); } std::tuple toEuler() const; Quaternion& operator+=(const Quaternion& r_q); Quaternion& operator-=(const Quaternion& r_q); Quaternion& operator*=(const double& scale); Quaternion& operator*=(const Quaternion& r_q); }; Quaternion operator+(Quaternion l_q, const Quaternion& r_q); Quaternion operator-(Quaternion l_q, const Quaternion& r_q); Quaternion operator*(Quaternion l_q, const double& scale); Quaternion operator*(const double& scale, Quaternion l_q); Quaternion operator*(Quaternion q1, const Quaternion& q2); } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/complementary_filter/include/vector3d.h ================================================ #pragma once #include #include #include namespace navigine { namespace navigation_core { struct Vector3d { double x = 0.0; double y = 0.0; double z = 0.0; Vector3d() = default; Vector3d(double _x, double _y, double _z); double magnitude() const; Vector3d normalized() const; Vector3d& normalize(); bool isNull() const; static Vector3d crossProduct(const Vector3d& v1, const Vector3d& v2); static double dotProduct (const Vector3d& v1, const Vector3d& v2); Vector3d& operator+=(const Vector3d& v2); Vector3d& operator-=(const Vector3d& v2); Vector3d& operator*=(double multiplier); Vector3d& operator/=(double divisor); }; inline bool operator==(const Vector3d& v1, const Vector3d& v2) { return std::fabs(v1.x - v2.x) < std::numeric_limits::epsilon() && std::fabs(v1.y - v2.y) < std::numeric_limits::epsilon() && std::fabs(v1.z - v2.z) < std::numeric_limits::epsilon(); } inline bool operator!=(const Vector3d& v1, const Vector3d& v2) { return !(v1 == v2); } inline Vector3d operator*(double multiplier, const Vector3d& v) { return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier); } inline Vector3d operator*(const Vector3d& v, double multiplier) { return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier); } inline Vector3d operator+(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z); } inline Vector3d operator-(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); } inline Vector3d operator-(const Vector3d& v) { return Vector3d(-v.x, -v.y, -v.z); } inline Vector3d operator/(const Vector3d& v1, double divisor) { return Vector3d(v1.x / divisor, v1.y / divisor, v1.z / divisor); } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/complementary_filter/src/complementary_filter.cpp ================================================ #include "complementary_filter.h" namespace navigine { namespace navigation_core { static const double MIN_SAMPLING_PERIOD_SEC = 0.02; static const double LPF_TIME_CONSTANT_SEC = 1.0; static const double DOUBLE_EPSILON = 1e-8; double to_minus_Pi_Pi(double x) { while ( x >= M_PI ) x -= 2 * M_PI; while ( x < -M_PI ) x += 2 * M_PI; return x; } Quaternion updateQuaternion(const Quaternion& q, const Vector3d& v) { Quaternion qDot = q * Quaternion(v) * 0.5; Quaternion qOut = q + qDot; return qOut.normalized(); } SensorMeasurement lowPassFilter(const SensorMeasurement& prevMsr, const SensorMeasurement& currMsr) { double dt = (currMsr.ts - prevMsr.ts) / 1000.0; double alpha = LPF_TIME_CONSTANT_SEC / (LPF_TIME_CONSTANT_SEC + std::max(dt, MIN_SAMPLING_PERIOD_SEC)); SensorMeasurement res = currMsr; res.values = alpha * prevMsr.values + (1 - alpha) * currMsr.values; return res; } /** This function returns calculated orientation (in radians) and current timestamp. * The orientation is right-handed, therefore counter-clockwise rotation has positive sign */ Orientation ComplementaryFilter::getFusedOrientation() const { double roll, pitch, yaw; std::tie(roll, pitch, yaw) = mQ.toEuler(); return Orientation{roll, pitch, yaw, mCurrentTs}; } /** This function returns calculated fused azimuth (in radians), clockwise has positive sign */ double ComplementaryFilter::getFusedAzimuth() const { double roll, pitch, yaw; std::tie(roll, pitch, yaw) = mQ.toEuler(); return -yaw; } /** This function returns calculated magnetic azimuth (in radians), clockwise has positive sign */ double ComplementaryFilter::getMagneticAzimuth() const { return mMagneticAzimuth; } /** This function returns azimuth calculated on device (in radians), clockwise has positive sign */ double ComplementaryFilter::getDeviceAzimuth() const { return mDeviceAzimuth; } /** measurements time must be in ascending order! */ void ComplementaryFilter::update(const SensorMeasurement& msr) { if(msr.type == SensorMeasurement::Type::ACCELEROMETER) { mAccelLowPassed = lowPassFilter(mAccelLowPassed, msr); updateUsingAccelerometer(msr); } else if (msr.type == SensorMeasurement::Type::MAGNETOMETER) { mMagnLowPassed = lowPassFilter(mMagnLowPassed, msr); if (mLastGyroTs == -1) updateUsingMagnetometer(msr); mMagneticAzimuth = caclulateMagneticAzimuth(msr); } else if (msr.type == SensorMeasurement::Type::GYROSCOPE) { if (mLastGyroTs == -1) mLastGyroTs = msr.ts; updateUsingGyroscope(msr); } else if (msr.type == SensorMeasurement::Type::ORIENTATION) { mDeviceAzimuth = msr.values.x; } mCurrentTs = msr.ts; return; } void ComplementaryFilter::updateUsingGyroscope(const SensorMeasurement& gyroMeas) { double dt = static_cast(gyroMeas.ts - mLastGyroTs) / 1000.0; mLastGyroTs = gyroMeas.ts; Vector3d gyro = gyroMeas.values; Vector3d omega = gyro * dt + mIntegralError * dt; mQ = updateQuaternion(mQ, omega); } void ComplementaryFilter::updateUsingAccelerometer(const SensorMeasurement& accelMeas) { Vector3d acc = accelMeas.values.normalized(); Vector3d gravSensorFrame = (mQ.conj() * Quaternion(0.0, 0.0, 0.0, 1.0) * mQ).toVector3d(); Vector3d error = Vector3d::crossProduct(acc, gravSensorFrame); Vector3d rotation = mKaccelerometer * error; mQ = updateQuaternion(mQ, rotation); mIntegralError += (mKintegralGain > DOUBLE_EPSILON) ? (mKintegralGain * error) : Vector3d(0.0, 0.0, 0.0); } void ComplementaryFilter::updateUsingMagnetometer(const SensorMeasurement& magnMeas) { Vector3d magn = magnMeas.values.normalized(); if (magn.magnitude() < DOUBLE_EPSILON) return; Vector3d magnGlobalFrame = (mQ * Quaternion(magn) * mQ.conj()).toVector3d(); magnGlobalFrame.y = std::sqrt(magnGlobalFrame.y * magnGlobalFrame.y + magnGlobalFrame.x * magnGlobalFrame.x); magnGlobalFrame.x = 0.0; Vector3d magnSensorFrame = (mQ.conj() * Quaternion(magnGlobalFrame) * mQ).toVector3d(); Vector3d error = Vector3d::crossProduct(magn, magnSensorFrame); Vector3d rotation = mKmagnetometer * error; mQ = updateQuaternion(mQ, rotation); mIntegralError += (mKintegralGain > DOUBLE_EPSILON) ? (mKintegralGain * error) : Vector3d(0.0, 0.0, 0.0); } double ComplementaryFilter::caclulateMagneticAzimuth(const SensorMeasurement& magn) { double roll, pitch, yaw; std::tie(roll, pitch, yaw) = mQ.toEuler(); double halfYaw = yaw / 2.0; Quaternion quatDifference = Quaternion(sin(halfYaw), 0, 0, cos(halfYaw)).normalized(); Quaternion quatZeroYaw = quatDifference * mQ; Vector3d magnetometer = magn.values.normalized(); Vector3d magnGlobalFrame = (quatZeroYaw * Quaternion(magnetometer) * quatZeroYaw.conj()).toVector3d(); return to_minus_Pi_Pi(M_PI / 2.0 + atan2(magnGlobalFrame.y, magnGlobalFrame.x)); } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/complementary_filter/src/quaternion.cpp ================================================ #include "quaternion.h" namespace navigine { namespace navigation_core { Quaternion::Quaternion(double _w, double _x, double _y, double _z) : w{_w}, x{_x}, y{_y}, z{_z} { } Quaternion::Quaternion(double _x, double _y, double _z) : w{0}, x{_x}, y{_y}, z{_z} { } Quaternion::Quaternion(const Vector3d& vec) : w{0.0}, x{vec.x}, y{vec.y}, z{vec.z} { } Quaternion Quaternion::normalized() const { double norm = std::sqrt(w * w + x * x + y * y + z * z); return std::fabs(norm) > std::numeric_limits::epsilon() ? Quaternion(w / norm, x / norm, y / norm, z / norm) : *this; } std::tuple Quaternion::toEuler() const { double sinr = 2.0 * (w * x + y * z); double cosr = 1.0 - 2.0 * (x * x + y * y); auto roll = std::atan2(sinr, cosr); double sinp = 2.0 * (w * y - z * x); double pitch = 0.0; if (std::fabs(sinp) >= 1) pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range else pitch = asin(sinp); double siny = 2.0 * (w * z + x * y); double cosy = 1.0 - 2.0 * (y * y + z * z); double yaw = std::atan2(siny, cosy); return std::make_tuple(roll, pitch, yaw); } Quaternion& Quaternion::operator+=(const Quaternion& r_q) { this->w += r_q.w; this->x += r_q.x; this->y += r_q.y; this->z += r_q.z; return *this; } Quaternion& Quaternion::operator-=(const Quaternion& r_q) { this->w -= r_q.w; this->x -= r_q.x; this->y -= r_q.y; this->z -= r_q.z; return *this; } Quaternion& Quaternion::operator*=(const double& scale) { this->w *= scale; this->x *= scale; this->y *= scale; this->z *= scale; return *this; } Quaternion& Quaternion::operator*=(const Quaternion& other) { double qw = w, qx = x, qy = y, qz = z; this->w = qw*other.w - qx*other.x - qy*other.y - qz*other.z; this->x = qw*other.x + qx*other.w + qy*other.z - qz*other.y; this->y = qw*other.y - qx*other.z + qy*other.w + qz*other.x; this->z = qw*other.z + qx*other.y - qy*other.x + qz*other.w; return *this; } Quaternion operator+(Quaternion l_q, const Quaternion& r_q) { return l_q += r_q; } Quaternion operator-(Quaternion l_q, const Quaternion& r_q) { return l_q -= r_q; } Quaternion operator*(Quaternion l_q, const double& scale) { return l_q *= scale; } Quaternion operator*(const double& scale, Quaternion l_q) { return l_q *= scale; } Quaternion operator*(Quaternion l_q, const Quaternion& r_q) { return l_q *= r_q; } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/complementary_filter/src/vector3d.cpp ================================================ #include namespace navigine { namespace navigation_core { Vector3d::Vector3d(double _x, double _y, double _z) { x = _x; y = _y; z = _z; } double Vector3d::magnitude() const { return std::sqrt(x*x + y*y + z*z); } Vector3d Vector3d::normalized() const { double length = this->magnitude(); return (std::fabs(length) > std::numeric_limits::epsilon()) ? Vector3d(x / length, y / length, z / length) : *this; } Vector3d& Vector3d::normalize() { double length = this->magnitude(); if (std::fabs(length) > std::numeric_limits::epsilon()) *this /= length; return *this; } bool Vector3d::isNull() const { return x == 0 && y == 0 && z == 0; } Vector3d& Vector3d::operator+=(const Vector3d& v) { this->x += v.x; this->y += v.y; this->z += v.z; return *this; } Vector3d& Vector3d::operator-=(const Vector3d& v) { this->x -= v.x; this->y -= v.y; this->z -= v.z; return *this; } Vector3d& Vector3d::operator*=(double multiplier) { this->x *= multiplier; this->y *= multiplier; this->z *= multiplier; return *this; } Vector3d& Vector3d::operator/=(double divisor) { this->x /= divisor; this->y /= divisor; this->z /= divisor; return *this; } double Vector3d::dotProduct(const Vector3d& v1, const Vector3d& v2) { return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; } Vector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x); } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/complementary_filter/tests/complementary_filter_test.cpp ================================================ #include #include #include #include #include #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MODULE ComplementaryFilter #include using namespace navigine::navigation_core; static const double MAGNETIC_FLUX_DENSITY = 0.48; static const double MAGNETIC_FIELD_INCLINATION = 60.48 * M_PI / 180; static const double G = 9.81; std::tuple, std::vector, std::vector> getTiltedImuMeasurements(double durationStaticSec, double tiltPitch = 0.0, double freqHz = 100) { std::random_device rd; std::mt19937 gen(rd()); std::vector times; long long t = 1583315232492; double tick = 1.0 / freqHz; for (int i = 0; i < durationStaticSec / tick; i++) { times.emplace_back(t); t += 1000 * tick; } std::vector accelMeasurements; for (long long ts: times) { //the variance is choosen to be equal to typical accelerometer noise variance of cheap IMU sensor double var = 2 * M_PI * freqHz * G * pow(0.001, 2); std::normal_distribution distr(0.0, var); double ax = -G * sin(tiltPitch) + distr(gen); double ay = 0.0 + distr(gen); double az = G * cos(tiltPitch) + distr(gen); SensorMeasurement msr; msr.ts = ts; msr.values = Vector3d(ax, ay, az); msr.type = SensorMeasurement::Type::ACCELEROMETER; accelMeasurements.emplace_back(msr); } std::vector gyroMeasurements; for (long long ts: times) { //the variance is choosen to be equal to typical gyroscope noise variance of cheap IMU sensor double var = 2 * M_PI * freqHz * pow(0.01 * M_PI / 180.0, 2); std::normal_distribution distr(0.005, var); double wx = 0.0 + distr(gen); double wy = 0.0 + distr(gen); double wz = 0.0 + distr(gen); SensorMeasurement msr; msr.ts = ts; msr.values = Vector3d(wx, wy, wz); msr.type = SensorMeasurement::Type::GYROSCOPE; gyroMeasurements.emplace_back(msr); } std::vector magnMeasurements; for (long long ts: times) { double mx = MAGNETIC_FLUX_DENSITY * cos(MAGNETIC_FIELD_INCLINATION + tiltPitch); double my = 0.0; double mz = MAGNETIC_FLUX_DENSITY * sin(MAGNETIC_FIELD_INCLINATION + tiltPitch); SensorMeasurement msr; msr.ts = ts; msr.values = Vector3d(mx, my, mz); msr.type = SensorMeasurement::Type::MAGNETOMETER; magnMeasurements.emplace_back(msr); } return std::make_tuple(accelMeasurements, gyroMeasurements, magnMeasurements); } BOOST_AUTO_TEST_CASE(staticTilt) { std::vector accelMeasurements; std::vector gyroMeasurements; std::vector magnMeasurements; double durationStaticSec = 10.0; double tiltPitch = 0.0; std::tie(accelMeasurements, gyroMeasurements, magnMeasurements) = getTiltedImuMeasurements(durationStaticSec, tiltPitch); Orientation ori(0, 0, 0, 0); ComplementaryFilter complementaryFilter; for (std::size_t i = 0; i < accelMeasurements.size(); i++) { const SensorMeasurement& accelMsr = accelMeasurements.at(i); const SensorMeasurement& gyroMsr = gyroMeasurements.at(i); const SensorMeasurement& magnMsr = magnMeasurements.at(i); complementaryFilter.update(accelMsr); complementaryFilter.update(gyroMsr); complementaryFilter.update(magnMsr); ori = complementaryFilter.getFusedOrientation(); } BOOST_CHECK(std::abs(ori.roll) < M_PI * 1.0 / 180); BOOST_CHECK(std::abs(ori.pitch - tiltPitch) < M_PI * 1.0 / 180); } ================================================ FILE: standalone_algorithms/pedometer/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.7) project(Navigation-core) option(BUILD_TESTS "Build tests" OFF) option(BUILD_EXAMPLES "Build examples" ON) set(BOOST_ENABLE_CMAKE ON) set(Boost_USE_STATIC_LIBS OFF) find_package(Boost COMPONENTS system unit_test_framework REQUIRED) IF (Boost_FOUND) INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR}) message(${Boost_INCLUDE_DIR}) include_directories(${Boost_SOURCE_DIR}) ENDIF () set("target" "pedometer") set("test-target" "test") if(BUILD_TESTS) file(GLOB ${PROJECT_NAME}_TEST_SOURCES src/*.cpp tests/*.cpp) add_executable("${test-target}" ${${PROJECT_NAME}_TEST_SOURCES}) target_include_directories("${test-target}" PRIVATE "include/") set_property(TARGET "${test-target}" PROPERTY CXX_STANDARD 17) set_target_properties("${test-target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;") set_target_properties("${test-target}" PROPERTIES COMPILE_FLAGS "-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions ") target_link_libraries("${test-target}" #"-lboost_unit_test_framework" # Boost::unit_test_framework ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} ) endif() if(BUILD_EXAMPLES) list(APPEND "${target}__cxx_srcs" "examples/main.cpp" "src/pedometer.cpp" "src/vector3d.cpp") add_executable("${target}" ${${target}__cxx_srcs}) target_include_directories("${target}" PRIVATE "include/") set_property(TARGET "${target}" PROPERTY CXX_STANDARD 17) set_target_properties("${target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;") set_target_properties("${target}" PROPERTIES COMPILE_FLAGS "-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions ") endif() ================================================ FILE: standalone_algorithms/pedometer/README.md ================================================ ### Pedometer The pedometer estimates steps based on accelerometer measurements collected from the user device. The algorithm analyses three input acceleration components (ax, ay, az) and the time stamp. For details on the algorithm implementation, please refer to the corresponding unit test in GitHub. ### Algorithm On each line of a log file there are timestamp and three input acceleration components (ax, ay, az). The process starts with reading raw acceleration data. Since the data could contain noise, it is processed to get rid of any noise. Magnitudes are calculated with low-passed filter and the average magnitude values are also computed according to the time interval for average value estimation. Once filtered, the noise reduced acceleration data is analyzed to detect whether a person is walking. Step counting is only performed if a walk is detected. Otherwise, the system goes back to reading the next window of acceleration data. If walking is detected, step counting is performed using a zero crossing technique. Signal, crossing the zero mark once in the negative direction followed by the same action but in the positive direction, can be observed. This phenomena is called zero crossing, and it can be used for step counting. Even after filtering there is still some unnecessary noise in the measurements. Zero crossing is considered only if the signal has come from outside this range, otherwise it is ignored. ### Build To build the project for running tests make sure that `BUILD_TESTS` option in CMakeLists.txt is turned on. To build the project for running example turn `BUILD_EXAMPLES` on. ```sh cd /standalone_algorithms/pedometer && rm -rf build cmake -Bbuild -H. cmake --build build ``` Run tests: ```sh ./build/test-filter ``` Run example: ```sh ./build/pedometer logs/HuaweiLong1.log ``` ================================================ FILE: standalone_algorithms/pedometer/examples/main.cpp ================================================ #include #include #include #include using namespace navigine::navigation_core; std::vector parseMeasurements(const std::string& logFile) { std::vector measurements; std::ifstream is(logFile); if (!is.is_open()) { std::cout << "could not open file " << logFile << std::endl; } std::string line; std::getline(is, line); // skip test data while (std::getline(is, line)) { SensorMeasurement msr; std::istringstream iss(line); if ((iss >> msr.ts >> msr.values.x >> msr.values.y >> msr.values.z)) measurements.emplace_back(msr); else break; } is.close(); return measurements; } int main(int argc, char *argv[]) { if (argc < 2) { std::cout << "can not find configuration file in command line argument! " << std::endl; exit(-1); } std::string logFile = std::string(argv[1]); std::vector measurements = parseMeasurements(logFile); Pedometer pedometer; long stepCounter = 0; for (SensorMeasurement msr: measurements) { std::vector singleMsrVector; singleMsrVector.emplace_back(msr); pedometer.update(singleMsrVector); std::deque steps = pedometer.calculateSteps(); stepCounter += steps.size(); } std::cout << "Number of steps: " << stepCounter << std::endl; return 0; } ================================================ FILE: standalone_algorithms/pedometer/include/pedometer.h ================================================ #pragma once #include #include #include namespace navigine { namespace navigation_core { struct SensorMeasurement { long long ts = -1; Vector3d values = Vector3d(); }; struct Step { long long ts; double len; Step() : ts(0), len(0) {}; Step(long long stepTs, double stepLength) : ts(stepTs), len(stepLength) {}; }; struct Magnitude { long long ts {}; double value {}; Magnitude() : ts(0), value(0) {}; Magnitude(long long msrTs, double magnitudeValue) : ts(msrTs), value(magnitudeValue) {}; }; /** * This class detects step of the human on the basis of accelerometer measurements * and empirical model of humans gate. * Step length is calculated using minimal and maximal accelerations */ class Pedometer { public: Pedometer() = default; void update(const std::vector& accMsrs); std::deque calculateSteps(); private: Magnitude calculateFilteredAccMagnitudes() const; double calculateStepLength(long long timeIntervalMs, std::deque::const_iterator rightBorderIt); //we use deque in order to fast delete measurements of the array when we got new ones std::deque mAccelMeasurements; // Array for saving acceleration measurements std::deque mFilteredAccMagnitudes; // Array for saving filtered acceleration magnitudes std::deque mTimes; // Array for saving step durations std::size_t mMagnitudeSize = 0; long long mPossibleStepTs = -1; // The time when step possibly occurred (sec in unix) long long mStepTime = -1; // The time when previous step possibly occured (sec in unix) bool mIsStep = false; // We use this variable to mark possible step }; } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/pedometer/include/vector3d.h ================================================ #pragma once #include #include #include namespace navigine { namespace navigation_core { struct Vector3d { double x = 0.0; double y = 0.0; double z = 0.0; Vector3d() = default; Vector3d(double _x, double _y, double _z); double magnitude() const; Vector3d normalized() const; Vector3d& normalize(); bool isNull() const; static Vector3d crossProduct(const Vector3d& v1, const Vector3d& v2); static double dotProduct (const Vector3d& v1, const Vector3d& v2); Vector3d& operator+=(const Vector3d& v2); Vector3d& operator-=(const Vector3d& v2); Vector3d& operator*=(double multiplier); Vector3d& operator/=(double divisor); }; inline bool operator==(const Vector3d& v1, const Vector3d& v2) { return std::fabs(v1.x - v2.x) < std::numeric_limits::epsilon() && std::fabs(v1.y - v2.y) < std::numeric_limits::epsilon() && std::fabs(v1.z - v2.z) < std::numeric_limits::epsilon(); } inline bool operator!=(const Vector3d& v1, const Vector3d& v2) { return !(v1 == v2); } inline Vector3d operator*(double multiplier, const Vector3d& v) { return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier); } inline Vector3d operator*(const Vector3d& v, double multiplier) { return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier); } inline Vector3d operator+(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z); } inline Vector3d operator-(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); } inline Vector3d operator-(const Vector3d& v) { return Vector3d(-v.x, -v.y, -v.z); } inline Vector3d operator/(const Vector3d& v1, double divisor) { return Vector3d(v1.x / divisor, v1.y / divisor, v1.z / divisor); } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/pedometer/src/pedometer.cpp ================================================ #include #include #include namespace navigine { namespace navigation_core { namespace { static const long long AVERAGING_TIME_INTERVAL_MS = 2500; // Time interval on which we calculate averaged filtered magnitude in order to align it relative to zero value static const long long FILTER_TIME_INTERVAL_MS = 200; // Filtered acc measurements are calculated as average over this time interval static const long long UPDATE_TIME_INTERVAL_MS = 700; // The time interval(in millisec) on which we update accelMax, Min, threshold static const long long MIN_TIME_BETWEEN_STEPS_MS = 300; // We cut off possible steps if time between them is less then this thresh static const double STEP_LENGTH_CONST = 0.52; // Constant using in algorithm which is responisble for step length calculation static const double MINIMAL_THRESHOLD_VALUE = 0.05 * 9.80665; // Empirical threshold values from article p.888 static const int MINIMAL_NUMBER_OF_STEPS = 5; // First N steps in which we accumulate data about average step time static const int MAXIMUM_NUMBER_OF_STEPS = 50; // Maximum number of steps to make an assumption about average step time static const int MAX_STEP_TIME = 2000; // The maximum possible time for step duration } void Pedometer::update(const std::vector& msrs) { mMagnitudeSize = (std::max)(mFilteredAccMagnitudes.size(), size_t(1)); for (auto msr: msrs) { mAccelMeasurements.push_back(msr); Magnitude magn = calculateFilteredAccMagnitudes(); if (magn.ts!=0) mFilteredAccMagnitudes.push_back(magn); while (mAccelMeasurements.back().ts - mAccelMeasurements.front().ts > 2 * AVERAGING_TIME_INTERVAL_MS && mAccelMeasurements.size() > 0) { mAccelMeasurements.pop_front(); } } } /** * This method calculates and form array with filtered acceleration magnitude * sliding window average is used (low pass). This function also updates average * filtered magnitude */ Magnitude Pedometer::calculateFilteredAccMagnitudes() const { long long lastMeasTs = mAccelMeasurements.back().ts; std::deque::const_iterator lBorderAverIt = mAccelMeasurements.begin(); while (lastMeasTs - lBorderAverIt->ts >= AVERAGING_TIME_INTERVAL_MS) lBorderAverIt++; double averMagnitude = std::accumulate(lBorderAverIt, mAccelMeasurements.end(), 0.0, [](double sum, const SensorMeasurement msr) {return sum+msr.values.magnitude();}); double nMeasAverage = std::distance(lBorderAverIt, mAccelMeasurements.end()); std::deque::const_iterator lBorderFilterIt = mAccelMeasurements.begin(); while (lastMeasTs - lBorderFilterIt->ts >= FILTER_TIME_INTERVAL_MS) lBorderFilterIt++; double filterMagnitude = std::accumulate(lBorderFilterIt, mAccelMeasurements.end(), 0.0, [](double sum, const SensorMeasurement msr) {return sum+msr.values.magnitude();}); double nMeasFiltered = std::distance(lBorderFilterIt, mAccelMeasurements.end()); // There is no measurements at acceleration array!\n"); if (nMeasAverage == 0 || nMeasFiltered == 0) return Magnitude(0,0); filterMagnitude /= nMeasFiltered; averMagnitude += filterMagnitude; // For correct processing at the very beginning averMagnitude /= nMeasAverage; filterMagnitude -= averMagnitude; return Magnitude(lastMeasTs, filterMagnitude); } std::deque Pedometer::calculateSteps() { std::deque steps = {}; if (mFilteredAccMagnitudes.size() <= 3) //There is no enough accelerometer measurements to detect steps return steps; long long averageStepTime = 0; auto nSteps = std::max(std::distance(mTimes.begin(), mTimes.end()), std::ptrdiff_t(1)); if (nSteps >= MINIMAL_NUMBER_OF_STEPS) averageStepTime = std::accumulate(mTimes.begin(), mTimes.end(), 0, [](long long sum, long long s) { return sum + s; }); averageStepTime /= nSteps; double timeBetweenSteps = std::max(1.0 * MIN_TIME_BETWEEN_STEPS_MS, 0.6 * averageStepTime); for (size_t i = mMagnitudeSize; i < mFilteredAccMagnitudes.size(); i++) { Magnitude curAcc = mFilteredAccMagnitudes[i]; Magnitude prevAcc = mFilteredAccMagnitudes[i-1]; //True if cross threshold and if new detection isn't too early. if (!mIsStep && prevAcc.value < MINIMAL_THRESHOLD_VALUE && curAcc.value > MINIMAL_THRESHOLD_VALUE && curAcc.ts - mPossibleStepTs > MIN_TIME_BETWEEN_STEPS_MS && timeBetweenSteps > 0) // && curAcc.ts - mPossibleStepTs > timeBetweenSteps { mIsStep = true; mPossibleStepTs = curAcc.ts; } if (mIsStep) { double stepLength = calculateStepLength(UPDATE_TIME_INTERVAL_MS, mFilteredAccMagnitudes.begin()+i); if (mPossibleStepTs - mStepTime < MAX_STEP_TIME) mTimes.push_back(mPossibleStepTs - mStepTime); steps.push_back(Step(mPossibleStepTs, stepLength)); mStepTime = mPossibleStepTs; mIsStep = false; // prepare to detect new step } } while(mFilteredAccMagnitudes.back().ts - mFilteredAccMagnitudes.front().ts > 2 * UPDATE_TIME_INTERVAL_MS) mFilteredAccMagnitudes.pop_front(); while (mTimes.size() > MAXIMUM_NUMBER_OF_STEPS) mTimes.pop_front(); return steps; } double Pedometer::calculateStepLength(long long timeIntervalMs, std::deque::const_iterator rightBorderIt) { std::deque::const_iterator leftBorderIt = rightBorderIt; while (rightBorderIt->ts - leftBorderIt->ts <= timeIntervalMs && leftBorderIt != mFilteredAccMagnitudes.begin() ) leftBorderIt--; double maxMagn = std::max_element(leftBorderIt, rightBorderIt, [](Magnitude m1, Magnitude m2) {return m1.value < m2.value; })->value; double minMagn = std::min_element(leftBorderIt, rightBorderIt, [](Magnitude m1, Magnitude m2) {return m1.value < m2.value; })->value; double accAmplitude = maxMagn - minMagn; double stepLen = STEP_LENGTH_CONST * sqrt(sqrt(accAmplitude)); return stepLen; } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/pedometer/src/vector3d.cpp ================================================ #include namespace navigine { namespace navigation_core { Vector3d::Vector3d(double _x, double _y, double _z) { x = _x; y = _y; z = _z; } double Vector3d::magnitude() const { return std::sqrt(x*x + y*y + z*z); } Vector3d Vector3d::normalized() const { double length = this->magnitude(); return (std::fabs(length) > std::numeric_limits::epsilon()) ? Vector3d(x / length, y / length, z / length) : *this; } Vector3d& Vector3d::normalize() { double length = this->magnitude(); if (std::fabs(length) > std::numeric_limits::epsilon()) *this /= length; return *this; } bool Vector3d::isNull() const { return x == 0 && y == 0 && z == 0; } Vector3d& Vector3d::operator+=(const Vector3d& v) { this->x += v.x; this->y += v.y; this->z += v.z; return *this; } Vector3d& Vector3d::operator-=(const Vector3d& v) { this->x -= v.x; this->y -= v.y; this->z -= v.z; return *this; } Vector3d& Vector3d::operator*=(double multiplier) { this->x *= multiplier; this->y *= multiplier; this->z *= multiplier; return *this; } Vector3d& Vector3d::operator/=(double divisor) { this->x /= divisor; this->y /= divisor; this->z /= divisor; return *this; } double Vector3d::dotProduct(const Vector3d& v1, const Vector3d& v2) { return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; } Vector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2) { return Vector3d(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x); } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/pedometer/tests/pedometer_test.cpp ================================================ #include #include #include #include #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MODULE Pedometer #include using namespace navigine::navigation_core; namespace { static const double STEP_NUM_TEST_THRESHOLD_PERCENTAGE = 5.0; static const double STEP_LEN_TEST_THRESHOLD_PERCENTAGE = 20.0; static const std::string TEST_DATA_FOLDER = "logs"; } std::pair, std::vector> parseTestData(const std::string& logFile) { std::vector measurements; std::ifstream is(logFile); if (!is.is_open()) { std::cout << "could not open file " << logFile << std::endl; } std::string line; int stepsNum = 0; double stepLen = 0.0; while (std::getline(is, line)) { std::istringstream iss(line); if (stepsNum == 0) // first line with ground truth { iss >> stepsNum >> stepLen; } else { SensorMeasurement msr; if ((iss >> msr.ts >> msr.values.x >> msr.values.y >> msr.values.z)) measurements.emplace_back(msr); else break; } } is.close(); return std::make_pair(std::make_pair(stepsNum, stepLen), measurements); } std::pair getStepsNumLenPair(const std::vector& measuremetns) { Pedometer pedometer; int stepCounter = 0; double stepLen = 0.0; for (const SensorMeasurement& msr: measuremetns) { std::vector singleMsrVector; singleMsrVector.emplace_back(msr); pedometer.update(singleMsrVector); std::deque steps = pedometer.calculateSteps(); for (const Step& s: steps) stepLen += s.len; stepCounter += steps.size(); } stepLen /= static_cast(stepCounter); return std::make_pair(stepCounter, stepLen); } BOOST_AUTO_TEST_CASE(stepsHuaweiLong1) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/HuaweiLong1.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsHuaweiLong2) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/HuaweiLong2.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsIPhoneShort1) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/iPhoneShort1.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsIPhoneShort2) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/iPhoneShort2.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsIPhoneShort3) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/iPhoneShort3.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsIPhoneShort4) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/iPhoneShort4.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsNexusShort1) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/NexusShort1.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsNexusShort2) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/NexusShort2.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsNexusShort3) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/NexusShort3.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsXiaomiLong1) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/XiaomiLong1.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsXiaomiLong2) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/XiaomiLong2.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsXiaomiLong3) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/XiaomiLong3.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } BOOST_AUTO_TEST_CASE(stepsXiaomiLong4) { std::string testDir = TEST_DATA_FOLDER; std::pair, std::vector > testData = parseTestData(testDir + "/XiaomiLong4.log"); std::pair trueStepsNumLenPair = testData.first; std::pair stepsNumLenPair = getStepsNumLenPair(testData.second); BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first), static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE); BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE); } ================================================ FILE: standalone_algorithms/position_estimation/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.7) project(Position-estimation) set("target" "position-estimation") list(APPEND "${target}__cxx_srcs" "examples/main.cpp" "src/measurement_preprocessor.cpp" "src/nearest_transmitter_estimator.cpp" "src/position_smoother.cpp") add_executable("${target}" ${${target}__cxx_srcs}) target_include_directories("${target}" PRIVATE "include/") set_property(TARGET "${target}" PROPERTY CXX_STANDARD 17) set_target_properties("${target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;") set_target_properties("${target}" PROPERTIES COMPILE_FLAGS "-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions ") ================================================ FILE: standalone_algorithms/position_estimation/README.md ================================================ ### Position Estimation Using RSSI data from transmitters it is possible to estimate the position of moving object. Estimation is being held according to the distance between the source of signals and the destination. ### Measurement Preprocessor The information about transmitters is stored in `/logs/transmitters.txt` file in a format `tid, coordinates, type` ``` (38945,2275,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 238.52 21.3 BEACON ``` Measurements are stored in `/logs/measurements.log` in the folowing format `timestamp, tid, rssi, type` ``` 1541003875242 (56813,7748,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) -82 BEACON ``` All the measurements are divided into packets. Preprocessor checks if measurements in the packet are valid: - if the signal type is supported - if rssi is in cutoff borders After that the last measurements are added into measurements buffer. The size of measuremets package is kept limited. ### Position Smoothing [//]: # "https://en.wikipedia.org/wiki/Alpha_beta_filter" Postprocessing of the position is made by position smoother algorithm that is called alpha-beta filter. In this very low order approximation position `x` is obtained as the time integral of velocity `v`. Assuming that velocity remains approximately constant over the small time interval between measurements, the position state is projected forward to predict its value at the next sampling time using equation

Residual between output measurement and prediction

coefficients are chosen to correct the position estimation

An extra factor conventionally serves to normalize magnitudes of the multipliers. For convergence and stability, the values of the alpha and beta multipliers should be positive and small:

Initial characteristics of the smoothing are set to zero, coefficient is determined by the smoothing coefficient. ### Model of signal transferring Distance between the source of signal and destination can be converted to signal strength

### Results of Position Estimation Calculated position with precision is written into `output.log` in the folowing format `timestamp, coordinates, precision` ``` 1541003922247 172.704 27.2699 1.55804 ``` ### Build ```sh cd /standalone_algorithms/position_estimation cmake -Bbuild -H. cmake --build build ``` Run examples: ```sh ./build/position-estimation ``` ### Results of position estimation Markdown Monster icon ================================================ FILE: standalone_algorithms/position_estimation/examples/main.cpp ================================================ #include #include #include #include "measurement_preprocessor.h" #include "nearest_transmitter_estimator.h" const std::string TEST_DATA_FOLDER = "logs/"; using namespace navigine::navigation_core; struct NavigationPoint { NavigationPoint(){} NavigationPoint(long long _timeMs, double _x, double _y, double _angle, int _sublocation) : timeMs (_timeMs) , x (_x) , y (_y) , angle (_angle) , sublocation (_sublocation) { } long long timeMs = 0; double x = 0; double y = 0; double angle = 0; int sublocation = 0; }; std::vector getTransmitters(const std::string& transmittersFileName) { std::vector transmitters; std::ifstream is(transmittersFileName); if (!is.is_open()) { std::cout << "could not open file " << transmittersFileName << std::endl; } std::string line; while (std::getline(is, line)) { std::istringstream iss(line); std::string typeStr; Transmitter t; if ((iss >> t.id >> t.x >> t.y >> typeStr )) { t.type = typeStr.find("WIFI") == std::string::npos ? Transmitter::Type::BEACON : Transmitter::Type::WIFI; transmitters.push_back(t); } else break; } is.close(); std::cout << "transmitters size: " << transmitters.size() << std::endl; return transmitters; } std::vector getMeasurements(const std::string& msrFileName) { std::vector measurements; std::ifstream is(msrFileName); if (!is.is_open()) { std::cout << "could not open file " << msrFileName << std::endl; } std::string line; while (std::getline(is, line)) { std::istringstream iss(line); RadioMeasurement msr; std::string typeStr; if ((iss >> msr.ts >> msr.id >> msr.rssi >> typeStr)) { if (typeStr.find("WIFI") == std::string::npos) msr.type = RadioMeasurement::Type::BEACON; else msr.type = RadioMeasurement::Type::WIFI; measurements.push_back(msr); } else break; } is.close(); std::cout << "measurements size: " << measurements.size() << std::endl; return measurements; } std::vector splitToPackets(const std::vector& inputMeasuremetns) { MeasurementsPreprocessor measurementsPreprocessor; std::vector inputMeasuremetnsPackets; for (const RadioMeasurement& msr: inputMeasuremetns) { measurementsPreprocessor.update(msr); RadioMeasurements measurementsPacket = measurementsPreprocessor.extractRadioMeasurements(); if (!measurementsPacket.empty()) { inputMeasuremetnsPackets.emplace_back(measurementsPacket); } } std::cout << "measurements packets size: " << inputMeasuremetnsPackets.size() << std::endl; return inputMeasuremetnsPackets; } int main() { std::string testDataFolder = TEST_DATA_FOLDER; std::string transmittersFileName = testDataFolder + "transmitters.txt"; std::string msrFileName = testDataFolder + "measurements.log"; std::string outputFileName = testDataFolder + "output.log"; std::vector transmitters = getTransmitters(transmittersFileName); std::vector inputMeasuremetns = getMeasurements(msrFileName); std::vector inputMeasuremetnsPackets = splitToPackets(inputMeasuremetns); NearestTransmitterPositionEstimator nearestBeaconPositionEstimator = NearestTransmitterPositionEstimator(transmitters); std::ofstream os; os.open(outputFileName); for (const RadioMeasurements &msr: inputMeasuremetnsPackets) { Position p = nearestBeaconPositionEstimator.calculatePosition(msr); if (!p.isEmpty) { os << p.ts << " " << p.x << " " << p.y << " " << p.precision << std::endl; } } os.close(); std::cout << "calculated positions were written to " << outputFileName << std::endl; return 0; } ================================================ FILE: standalone_algorithms/position_estimation/helpers/plot_positions.py ================================================ import pandas as pd import matplotlib.pylab as plt df_calculated = pd.read_csv('../logs/output.log', sep=' ', names=['ts', 'x', 'y', 'r']) df_benchmark = pd.read_csv('../logs/benchmarks.log', sep=' ', names=['x', 'y', 'ts', 'ori', 'subloc']) plt.gca().set_aspect('equal') plt.plot(df_calculated.x, df_calculated.y, '.-', label='calculated positions') plt.plot(df_benchmark.x, df_benchmark.y, '-', label='reference trace') plt.legend(bbox_to_anchor=(1.0, -0.25)) plt.show() ================================================ FILE: standalone_algorithms/position_estimation/include/measurement_preprocessor.h ================================================ #pragma once #include #include #include #include #include "navigation_structures.h" namespace navigine { namespace navigation_core { class RadioMeasurementBuffer { public: RadioMeasurementBuffer() = default; RadioMeasurementBuffer(long long sigWindowShiftMs, long long radioKeepPeriodMs); std::vector extractMeasurements(); void addMeasurements(long long messageTs, const std::vector& signalEntries); private: std::vector mMeasurements = {}; long long mLastExtractionTs = -1; long long mCurrentTs = -1; long long mRadioKeepPeriodMs = 0; long long mSignalWindowShiftMs = 0; }; class MeasurementsPreprocessor { public: MeasurementsPreprocessor(double sigAverageTimeSec = 3.0, double sigWindowShiftSec = 2.0, bool useWifi = true, bool useBle = true); void update(const RadioMeasurement &msr); long long getCurrentTs() const; std::vector extractRadioMeasurements(); private: bool isSignalTypeSupported(RadioMeasurement::Type signalType) const; bool isRssiValid(double rssi) const; private: RadioMeasurementBuffer mRadiosBuffer; double mCutOffRssi = -100; long long mCurrentTs = -1; double mSigAverageTimeSec = 3.0; double mSigWindowShiftSec = 2.0; bool mUseWifi = true; bool mUseBle = true; bool mUseClosestAps = true; int mNumClosestAps = 5; }; } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/position_estimation/include/navigation_structures.h ================================================ #pragma once #include #include namespace navigine { namespace navigation_core { struct Point2D { Point2D() : x(0.0) , y(0.0) {} Point2D(double _x, double _y) : x(_x) , y(_y) {} double x; double y; }; struct Position { double x = 0.0; double y = 0.0; double precision = 0.0; bool isEmpty = true; long long ts = -1; }; struct RadioMeasurement { enum class Type {WIFI, BEACON}; Type type; long long ts = 0; std::string id = ""; // Entry id (bssid, mac, major+minor+uuid) double rssi = 0; double power = 0; double frequency = 0; double distance = 0; double stddev = 0; }; struct NavigationInput { int packetNumber = 0; // Message number std::string deviceId = ""; // Device identifier (MAC address/id/...) std::string checkPoint = ""; long long timeStamp = 0; std::vector signalEntries = {}; }; using RadioMeasurements = std::vector; } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/position_estimation/include/nearest_transmitter_estimator.h ================================================ #pragma once #include #include "transmitter.h" #include "navigation_structures.h" #include "position_smoother.h" namespace navigine { namespace navigation_core { class NearestTransmitterPositionEstimator { public: NearestTransmitterPositionEstimator(const std::vector& transmitters); Position calculatePosition(const RadioMeasurements& inputMeasurements); private: std::vector getRegisteredTransmittersMeasurements( const std::vector& radioMsr); Point2D getTransmitterPosition(const std::string& txId); PositionSmoother m_smoother; std::map m_transmitters; }; } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/position_estimation/include/position_smoother.h ================================================ #pragma once #include "navigation_structures.h" namespace navigine { namespace navigation_core { class PositionSmoother { public: PositionSmoother(double smoothing = 0.5); Position smoothPosition(Position pos); private: bool mWasCalled = false; double mSpeedX = 0.0; double mSpeedY = 0.0; double mAlpha = 0.0; Position mPosition; long long mTsMs; }; } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/position_estimation/include/transmitter.h ================================================ #pragma once #include #include namespace navigine { namespace navigation_core { struct Transmitter { enum class Type { WIFI, BEACON }; std::string id; double x; double y; Type type; bool operator< (const Transmitter& tx)const { return id < tx.id; } }; typedef std::vector Transmitters; } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/position_estimation/logs/transmitters.txt ================================================ (404,404,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 138.973 88.546 BEACON (284,284,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 139.712 82.5232 BEACON (46266,37698,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 75.06 91.82 BEACON (7667,53553,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 84.5999 92.99 BEACON (60989,41353,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 95.1199 92.8 BEACON (284,53983,00842522-539B-9081-5BA4-768360981005) 105.37 92.51 BEACON (46209,34756,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 114.2 92.67 BEACON (51948,50362,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 123.65 92.29 BEACON (39652,57626,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 132.94 92.21 BEACON (35304,213,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 131.88 83.34 BEACON (24210,11385,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 142.54 82.37 BEACON (61521,40127,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 150.69 90.33 BEACON (56109,26280,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 160.7 82.27 BEACON (8922,21995,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 182.81 82.1 BEACON (8367,8843,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.1 82.19 BEACON (40720,2489,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 222.46 83.57 BEACON (10085,46410,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 241.09 83.29 BEACON (36896,34867,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 240.93 94.79 BEACON (28112,40494,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 60.2799 92.17 BEACON (12604,13902,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 69.5099 88.88 BEACON (43057,47231,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 56.0599 97.22 BEACON (328,328,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 50.43 89.53 BEACON (99,99,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 60.36 87.67 BEACON (242,242,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 101.522 102.826 BEACON (156,156,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 484.076 106.81 BEACON (330,330,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 461.843 107.298 BEACON (176,176,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 436.077 106.932 BEACON (130,130,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 71.1359 103.53 BEACON (202,202,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 69.9753 90.1706 BEACON (183,183,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 47.4299 94.4125 BEACON (349,349,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 27.9794 90.2586 BEACON (149,149,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 17.463 106.645 BEACON (471,471,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 479.313 101.775 BEACON (23741,17177,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 249.12 121.64 BEACON (18464,22743,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 400.54 114.35 BEACON (23924,199,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 416.71 118.96 BEACON (54510,38378,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 428.63 103.08 BEACON (36927,9530,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 409.14 102.81 BEACON (12778,34654,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 394.37 103.21 BEACON (35265,63345,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 371.67 103.36 BEACON (17839,34553,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 342.86 103.36 BEACON (2832,16485,009E12DB-8622-2FFC-9D1F-164961CE1005) 313.08 103.62 BEACON (19342,17833,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 283.65 103.58 BEACON (53008,63284,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 269.09 104.41 BEACON (52004,53550,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 246.55 101.87 BEACON (46240,53976,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 261.24 119.47 BEACON (19652,64817,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 224.55 101.47 BEACON (48307,21358,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 228.17 89.44 BEACON (8266,24512,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 198.65 90.81 BEACON (23247,40052,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 168.9 89.98 BEACON (5678,65103,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 138.41 89.51 BEACON (21263,20026,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 132.07 103.85 BEACON (48149,28656,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.97 103.69 BEACON (2379,11009,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 93.9402 111.53 BEACON (1888,13768,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 97.3 114.69 BEACON (29394,48788,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 77.3199 67.63 BEACON (11081,29004,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 302.12 21.33 BEACON (60638,1586,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 291.18 21.28 BEACON (40719,22526,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 186.5 62.27 BEACON (47944,17364,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 99.5099 63.86 BEACON (16468,35906,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 276.94 63.82 BEACON (4230,3488,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 277.85 58.02 BEACON (47429,18703,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 290.59 58.75 BEACON (40914,14221,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 270.55 58.61 BEACON (4620,59742,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 257.07 58.73 BEACON (29981,34287,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 244.63 58.98 BEACON (22433,27932,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 231.75 58.32 BEACON (39876,5616,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 221.21 58.64 BEACON (50760,2989,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 208.96 58.81 BEACON (47225,61209,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 188.96 58.04 BEACON (15885,64531,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 195.34 68.06 BEACON (38584,19822,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 180.61 59.05 BEACON (17595,6881,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 167.44 58.04 BEACON (41004,65021,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 154.69 58.02 BEACON (3722,6190,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 143.77 58.43 BEACON (19944,60209,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 135.08 58 BEACON (7200,29838,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 125.12 58.62 BEACON (1484,7639,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 116.04 58.99 BEACON (22296,23760,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.41 68.62 BEACON (37837,18741,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 89.7 61.18 BEACON (27819,63658,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 96.56 58.63 BEACON (60589,31837,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 107.97 58.72 BEACON (27369,16528,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 188.29 2.33001 BEACON (15823,16948,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 97.0499 8.04998 BEACON (43834,6573,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 95.9099 12.1 BEACON (22790,12825,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.89 12 BEACON (42883,2294,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 113.11 12.7 BEACON (17028,59968,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106 1.08999 BEACON (22344,41020,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 118.01 12.09 BEACON (55543,22194,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 130.19 11.97 BEACON (65209,31481,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 142.27 11.51 BEACON (41698,57655,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 155.14 11.77 BEACON (5190,14288,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 166.94 11.94 BEACON (6957,49198,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 176.27 11.95 BEACON (10561,36193,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.01 11.38 BEACON (64077,47494,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 192.64 2.81998 BEACON (53011,18269,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 198.1 11.98 BEACON (45460,17762,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 210.12 11.76 BEACON (13815,9970,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 222.54 11.56 BEACON (55589,9525,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 233.46 12 BEACON (58545,46080,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 242.38 12.05 BEACON (41990,26484,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 253.09 11.96 BEACON (57640,41428,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 263.06 11.96 BEACON (40317,44189,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 271.94 11.96 BEACON (29401,9995,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 277.6 11.97 BEACON (62753,9949,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 286.82 11.95 BEACON (63704,14402,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 281.72 2.91001 BEACON (2832,58658,00AE562C-DDDF-3479-B2EA-B8B0CCF91005) 277.1 3.32003 BEACON (27962,20960,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 306.24 6.91 BEACON (48808,22987,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 300.62 12.02 BEACON (19921,32863,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 298.88 58.82 BEACON (14474,30357,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 306.17 58.6 BEACON (44082,26385,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 312.89 58.44 BEACON (2832,48303,00F1E611-08B6-0756-F54B-5697CA341005) 320.94 58.66 BEACON (10532,57861,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 321.01 52.97 BEACON (11036,16315,0102202B-990F-0100-0082-B38ADD861005) 308.89 52.93 BEACON (49151,50749,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 301.51 52.96 BEACON (14989,10986,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 298.81 68.06 BEACON (19799,60213,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 317.39 68.06 BEACON (42577,11390,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 198.8 58.91 BEACON (32303,20995,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 306.37 44.8 BEACON (15305,27092,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 299.89 49.39 BEACON (52278,59347,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 291.34 49.25 BEACON (30285,39395,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 281.91 49.78 BEACON (56449,62259,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 275.97 49.39 BEACON (37937,32127,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 268.18 49.62 BEACON (32545,64503,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 261.25 49.56 BEACON (56937,18553,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 252.75 46.81 BEACON (11775,51443,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 244.27 49.36 BEACON (1858,36038,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 232.71 49.45 BEACON (28491,27829,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 227.81 47.09 BEACON (5486,32062,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 212.33 49.48 BEACON (61500,20121,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 208.65 41.6 BEACON (42453,13118,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 200.93 49.33 BEACON (47343,57014,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.17 49.51 BEACON (5392,63591,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 181.42 49.47 BEACON (17849,42956,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 173.98 45.62 BEACON (57772,16444,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 167.85 49.39 BEACON (5379,35746,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 156.5 41.8 BEACON (20472,41943,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 156.53 49.5 BEACON (28251,31240,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 145.11 49.48 BEACON (44619,54563,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 138.31 45.48 BEACON (47176,48366,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 130.54 45.75 BEACON (13431,60500,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 123.55 49.51 BEACON (991,13251,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 118.14 44.16 BEACON (49080,42158,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.01 49.5 BEACON (60472,36645,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 107.25 49.8 BEACON (19631,21792,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 101.76 49.62 BEACON (59930,7234,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 92.7101 49.37 BEACON (34207,61481,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 86.1298 49.78 BEACON (22266,24912,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 80.8001 53.89 BEACON (13010,42000,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 74.2999 55.89 BEACON (3748,62169,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 67.5899 55.96 BEACON (46930,2535,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 61.2899 53.82 BEACON (26712,61595,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 55.89 49.56 BEACON (7152,42738,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 54.1801 43.94 BEACON (40197,9211,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 54.1202 35.4 BEACON (62292,32638,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 54.4101 27.57 BEACON (42768,6484,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 47.0599 28.22 BEACON (64614,56909,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 42.4501 32.41 BEACON (34987,39476,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 40.1198 27.56 BEACON (49702,4902,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 30.1399 28.1 BEACON (2413,38956,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 21.3399 24.83 BEACON (10994,21572,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 8.79004 24.84 BEACON (40867,13112,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 12.81 45.85 BEACON (58618,31280,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 21.4601 46.25 BEACON (61437,48199,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 29.9601 42.15 BEACON (2145,62241,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 36.7301 35.63 BEACON (23798,53937,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 40.1599 42.07 BEACON (37270,7063,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 47.0301 46.22 BEACON (25055,33212,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 45.9001 38.87 BEACON (19598,60849,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 56.7099 20.3 BEACON (27386,13875,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 61.2201 17.04 BEACON (10527,11854,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 66.72 15.13 BEACON (21374,52176,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 73.3 14.82 BEACON (40773,47752,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 80.8299 17.09 BEACON (32110,7548,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 86.9401 21.17 BEACON (63533,13204,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 98.4301 21.26 BEACON (62668,50359,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 105.07 27.05 BEACON (60434,1366,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 105.03 34.12 BEACON (22082,40380,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.77 42.22 BEACON (54158,28577,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 116.21 31.77 BEACON (57382,11246,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.05 21.36 BEACON (50190,53479,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 121.18 21.3 BEACON (17996,39932,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 131.13 26.98 BEACON (31499,42427,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 131.34 21.34 BEACON (34664,56100,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 135.99 22.72 BEACON (14964,50089,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 141.62 21.26 BEACON (31813,31451,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 147.55 25.09 BEACON (15253,33808,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 153.13 21.27 BEACON (42124,24780,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 164.47 21.32 BEACON (63069,1400,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 175.69 21.19 BEACON (14143,28299,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.24 21.22 BEACON (49741,50969,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 196.27 20.99 BEACON (55479,26935,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.93 21.28 BEACON (287,17074,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 212.21 21.34 BEACON (52401,25667,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 223.69 21.23 BEACON (35744,46356,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 230.92 23.54 BEACON (41119,31078,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 225.7 28.52 BEACON (25620,58895,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 236.06 32.02 BEACON (33707,10271,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 245.58 26.31 BEACON (38945,2275,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 238.52 21.3 BEACON (37237,1865,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 252.15 21.21 BEACON (56337,58963,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 257.24 25.16 BEACON (38362,9852,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 264.64 21.28 BEACON (61348,8722,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 268.22 23.48 BEACON (26849,38969,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 275.75 21.32 BEACON (12383,4127,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 285.13 21.08 BEACON (57301,21859,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 302.3 35.44 BEACON (22686,31629,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 277.03 40.83 BEACON (43226,11185,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 271.66 41.02 BEACON (18794,50683,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 270.74 27.25 BEACON (11368,23693,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 265.57 30.95 BEACON (57172,64088,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 260.44 37.69 BEACON (32551,30232,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 251.9 31.46 BEACON (18110,9010,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 233.58 42.02 BEACON (57111,61572,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 222.94 38.89 BEACON (41337,42866,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 211.24 37.22 BEACON (50894,14185,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.73 36.32 BEACON (32043,44935,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 119 40.58 BEACON (64841,49079,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 125.38 37.92 BEACON (41740,16004,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 134.51 38.65 BEACON (50306,20793,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 151.8 41.68 BEACON (49364,57737,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 162.3 38.66 BEACON (627,55268,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 172.34 41.7 BEACON (63714,8737,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 178.82 36.13 BEACON (56647,38560,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.28 37.71 BEACON (42303,60963,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 119.02 29.51 BEACON (21236,33521,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 140.09 29.37 BEACON (38825,2584,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 149.19 32.48 BEACON (46573,45203,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 158.19 34.24 BEACON (14200,25697,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 165.74 30.95 BEACON (366,14532,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 91.3001 21.2 BEACON (56813,7748,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 115.73 38.86 BEACON (34241,24381,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 135.1 49.34 BEACON (65397,49904,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 221.92 49.7 BEACON (34574,59997,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 243.01 36.17 BEACON (47515,53879,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 194.19 35.88 BEACON (49320,44414,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 194.25 36.48 BEACON (59662,5429,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 283.03 35.22 BEACON (65307,27568,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 282.97 35.77 BEACON (23239,33483,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 159.34 25.03 BEACON (23,23,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 93.3202 9.53999 BEACON (457,457,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 82.8198 9.06997 BEACON (14,14,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 71.17 8.74003 BEACON (478,478,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 60.39 8.16003 BEACON (479,479,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 54.6001 12.72 BEACON (434,434,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 53.5101 17.81 BEACON (16294,59149,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 307.01 25.9 BEACON (37438,30212,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 297.29 13.22 BEACON (49178,13451,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 288.9 13.33 BEACON (25643,15815,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 289.86 3.57002 BEACON (32946,48985,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 284.12 13.57 BEACON (31865,25980,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 274.44 13.74 BEACON (38672,41376,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 263.68 13.65 BEACON (5324,16642,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 253.32 13.83 BEACON (38641,23297,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 242.54 13.96 BEACON (39790,3048,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 233.08 13.44 BEACON (11783,41227,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 220.81 13.64 BEACON (39758,60188,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 212.48 13.56 BEACON (33989,55136,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.65 13.68 BEACON (32023,55448,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 197.04 3.19002 BEACON (27239,45545,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 191.32 13.7 BEACON (27254,17421,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 181.64 13.55 BEACON (9631,49805,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 169.7 13.6 BEACON (38510,34350,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 159.48 13.41 BEACON (24322,40944,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.11 9.67001 BEACON (37104,21414,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.05 3.19002 BEACON (40167,7053,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 52.1502 22.63 BEACON (23953,12738,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 52.1502 23.82 BEACON (25406,1613,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 51.49 31.93 BEACON (16328,885,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 48.75 38.4 BEACON (2691,3439,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 50.79 44.54 BEACON (39728,12993,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 52.1502 52.98 BEACON (10582,30372,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 194.41 67.52 BEACON (47044,55767,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 109.48 22.8 BEACON (26153,1691,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 109.43 52.82 BEACON (10950,16425,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 110.52 53.78 BEACON (30118,32251,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 52.2502 54.02 BEACON (20124,11994,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.05 67.36 BEACON (54880,10370,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 99.84 62.85 BEACON (21425,49970,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.17 63.18 BEACON (30975,62343,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 109.44 75.49 BEACON (30464,52580,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 122.73 62.69 BEACON (30954,64214,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 136.59 62.72 BEACON (31620,10493,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 149.23 63.27 BEACON (51353,41429,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 161.94 62.84 BEACON (31294,9902,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 174.78 62.87 BEACON (24511,37763,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.1 62.68 BEACON (40660,18549,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 202.29 22.28 BEACON (9091,23537,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.23 23.68 BEACON (28768,44909,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 202.37 52.74 BEACON (56802,50744,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.29 53.88 BEACON (5444,56164,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.02 72.91 BEACON (42471,14138,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 215.67 63 BEACON (61334,54776,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 230.3 62.59 BEACON (42913,51408,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 255.76 62.97 BEACON (14517,39941,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 270 62.4 BEACON (49519,60215,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 281.15 62.68 BEACON (1848,10242,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 297.12 72.94 BEACON (35599,49120,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 295.16 52.77 BEACON (1753,42535,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.19 53.79 BEACON (20396,60939,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.09 23.62 BEACON (51213,55797,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 295.22 22.86 BEACON (39644,49117,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 298.42 62.87 BEACON (34290,59075,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 290.02 62.17 BEACON (58273,63634,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 287.15 72.32 BEACON (30523,15802,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.36 62.82 BEACON (62784,40038,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 243.53 62.46 BEACON (186,186,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 45.6201 44.35 BEACON (493,493,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 40.4699 38.37 BEACON (92,92,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 45.6599 31.87 BEACON (33734,56790,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 154.47 13.47 BEACON (56216,42840,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 146.5 13.72 BEACON (18995,14191,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 137.89 13.53 BEACON (57925,33476,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 130.71 13.43 BEACON (53193,43741,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 122.96 13.43 BEACON (1813,28558,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 115.36 13.34 BEACON (41988,39495,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.59 13.69 BEACON (46183,37165,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 100.18 13.28 BEACON (36662,40201,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 297.08 46.15 BEACON (43855,127,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 294.38 38.33 BEACON (21470,51534,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.83 30.4 BEACON (244,244,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 308.13 63.19 BEACON (109,109,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 320.54 63.46 BEACON (354,354,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 329.24 62.94 BEACON (282,282,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 338.76 63.7 BEACON (1651,12932,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.46 46.19 BEACON (42974,4557,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.15 38.36 BEACON (34972,7118,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.6 30.39 BEACON (33765,30115,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 111.37 30.39 BEACON (59611,12333,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 108.6 38.37 BEACON (56314,17772,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 111.39 46.13 BEACON (32105,45524,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 84.9901 13.45 BEACON (47765,10946,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 92.4699 13.48 BEACON (58976,53535,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 68.18 14.06 BEACON (13698,61998,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 60.14 17.66 BEACON (392,392,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 62.1101 60.99 BEACON (251,251,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 70.7999 62.59 BEACON (178,178,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 79.7201 61.95 BEACON (66,66,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 89.5002 63.05 BEACON (23876,53520,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 76.1999 13.58 BEACON (182,182,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 118.212 16.8812 BEACON (175,175,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 61.9681 72.7781 BEACON (55085,22630,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 32.0699 60.66 BEACON (24688,53739,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 42.4 60.52 BEACON (23624,39024,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 50.2199 60.46 BEACON (33051,7125,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 53.26 65.65 BEACON (46614,41903,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 50.0099 37.95 BEACON (41998,49438,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 50.0999 49.64 BEACON (56931,53602,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 41.4499 49.53 BEACON (23671,3574,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 53.1499 33.33 BEACON (10860,36840,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 105.4 23.8 BEACON (23535,64754,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 111.59 11.36 BEACON (59134,25091,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 114.24 24.59 BEACON (60265,23475,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 122.1 24.1 BEACON (28869,7392,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 116.49 33.64 BEACON (54223,8064,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.32 34.56 BEACON (28446,12100,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.16 63.99 BEACON (62043,20652,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 103.57 21.12 BEACON (17260,53988,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.31 14.79 BEACON (42801,62584,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 61.87 67.75 BEACON (44312,39244,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 70.7402 71.88 BEACON (2832,64197,0006FBCA-2FFB-B2D6-6BB8-6E9AA8821005) 82.0901 74.83 BEACON (33748,2108,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 88.7998 68.57 BEACON (20028,15740,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 97.9999 74.8 BEACON (13830,33068,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.6 74.34 BEACON (612,59359,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 125.68 23.91 BEACON (9508,6979,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 136.34 24.18 BEACON (9,47698,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 145.12 23.97 BEACON (47468,42482,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 153.48 24.68 BEACON (18068,32673,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 161.34 23.51 BEACON (34942,62772,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 176.05 24.18 BEACON (6293,31077,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 192.12 23.39 BEACON (19563,7586,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 200.84 18.66 BEACON (10679,11847,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.74 24.26 BEACON (46839,36006,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 206.67 33.74 BEACON (27325,28878,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 207.46 34.32 BEACON (36711,22246,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 206.89 64.22 BEACON (43600,30858,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 216.28 23.19 BEACON (38649,16562,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 231.32 24.47 BEACON (20119,27949,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 244.87 23.6 BEACON (17182,44119,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 260.27 24.57 BEACON (48792,21373,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 274.71 23.81 BEACON (1646,20179,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 288.22 24.31 BEACON (21274,44606,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.02 18.7 BEACON (49284,37602,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 301.82 34.37 BEACON (3992,32979,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 302.53 64.45 BEACON (9385,13584,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 302.47 33.7 BEACON (22534,32770,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 309.36 23.44 BEACON (12701,38145,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 299.79 24.22 BEACON (49353,43721,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 310.35 73.94 BEACON (23532,1906,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 301.56 65.34 BEACON (33537,39533,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 299.04 73.58 BEACON (33646,25253,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.08 80.09 BEACON (55732,41553,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 293.41 84.76 BEACON (63398,1207,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 288.86 73.89 BEACON (25150,47521,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 278.06 74.27 BEACON (51059,4548,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 266.13 74.73 BEACON (24940,10271,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 255.62 74.63 BEACON (23453,58075,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 245.41 74.58 BEACON (24451,48690,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 219.43 75.1 BEACON (28082,59191,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 232.63 74.84 BEACON (47879,36416,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 211.69 74.71 BEACON (14600,3364,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.56 74.83 BEACON (42400,26195,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 206.82 65.21 BEACON (5401,6275,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.11 78.38 BEACON (13676,48626,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 198.47 85.22 BEACON (52236,1708,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 194.84 74.82 BEACON (47782,62713,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 174.65 74.4 BEACON (8921,30860,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 165.27 74.39 BEACON (41992,132,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 155.03 74.51 BEACON (59883,21840,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 145.71 74.08 BEACON (52566,61097,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 136.49 74.31 BEACON (11286,2926,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 126.11 74.32 BEACON (4664,28948,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 116.87 74.55 BEACON (49208,33117,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 113.01 65.21 BEACON (52397,25104,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.19 79.56 BEACON (12341,59620,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 111.56 88.09 BEACON (41242,60177,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 184.61 74.36 BEACON (31877,41174,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 206.58 88.09 BEACON (62212,29708,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 110.93 41.24 BEACON (5642,18086,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 110.96 57.41 BEACON (42926,37532,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.62 41.25 BEACON (58529,2251,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 205.83 49.53 BEACON (42856,51925,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.56 57.36 BEACON (70,70,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 315.08 24.01 BEACON (445,445,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 341.97 23.6 BEACON (55659,16422,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 303.43 41.32 BEACON (27639,35916,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 300.55 49.46 BEACON (39909,40638,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 303.61 57.39 BEACON (476,476,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 315.11 74.56 BEACON (232,232,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 327.42 74.89 BEACON (382,382,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 340.54 75.07 BEACON (472,472,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 329 23.83 BEACON (160,160,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 61.2101 28.74 BEACON (193,193,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 64.7201 24.59 BEACON (325,325,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 75.7001 23.8 BEACON (309,309,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 85.0499 23.87 BEACON (113,113,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 94.5499 24.31 BEACON (7046,52633,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 113.33 49.83 BEACON ================================================ FILE: standalone_algorithms/position_estimation/src/measurement_preprocessor.cpp ================================================ #include #include #include "measurement_preprocessor.h" namespace navigine { namespace navigation_core { MeasurementsPreprocessor::MeasurementsPreprocessor(double sigAverageTimeSec, double sigWindowShiftSec, bool useWifi, bool useBle) : mCurrentTs ( -1 ) , mSigAverageTimeSec ( sigAverageTimeSec ) , mSigWindowShiftSec ( sigWindowShiftSec ) , mUseWifi ( useWifi ) , mUseBle ( useBle ) { long long sigAverageTime = (long long)(1000 * mSigAverageTimeSec); long long sigWindowShift = (long long)(1000 * mSigWindowShiftSec); if (sigWindowShift == 0 || sigWindowShift > sigAverageTime) sigWindowShift = sigAverageTime; long long keepRadioTimeMs = sigAverageTime - sigWindowShift; mRadiosBuffer = RadioMeasurementBuffer(sigWindowShift, keepRadioTimeMs); } void MeasurementsPreprocessor::update(const RadioMeasurement& msr) { if (isSignalTypeSupported(msr.type) && isRssiValid(msr.rssi)) { std::vector msrVect; msrVect.push_back(msr); mRadiosBuffer.addMeasurements(msr.ts, msrVect); } if (msr.ts > mCurrentTs) mCurrentTs = msr.ts; } std::vector MeasurementsPreprocessor::extractRadioMeasurements() { std::vector radioMeasurements = mRadiosBuffer.extractMeasurements(); if (mUseClosestAps) { std::sort(radioMeasurements.rbegin(), radioMeasurements.rend(), [](const RadioMeasurement& lhs, const RadioMeasurement& rhs) { return lhs.rssi < rhs.rssi; }); if (radioMeasurements.size() > (unsigned int) mNumClosestAps) radioMeasurements.resize(mNumClosestAps); } return radioMeasurements; } long long MeasurementsPreprocessor::getCurrentTs() const { return mCurrentTs; } bool MeasurementsPreprocessor::isSignalTypeSupported(RadioMeasurement::Type signalType) const { return (signalType == RadioMeasurement::Type::WIFI && mUseWifi) || (signalType == RadioMeasurement::Type::BEACON && mUseBle); } bool MeasurementsPreprocessor::isRssiValid(double rssi) const { return rssi > mCutOffRssi && rssi < 0.0; } RadioMeasurementBuffer::RadioMeasurementBuffer( long long sigWindowShiftMs, long long radioKeepPeriodMs) { mRadioKeepPeriodMs = radioKeepPeriodMs; mSignalWindowShiftMs = sigWindowShiftMs; } void RadioMeasurementBuffer::addMeasurements(long long messageTs, const std::vector& msrs) { if (mCurrentTs == -1) { mCurrentTs = messageTs; mLastExtractionTs = messageTs; } for (const RadioMeasurement& msr : msrs) { RadioMeasurement radioMsr = msr; radioMsr.rssi = msr.rssi; // check if it is a fresh measurement if (radioMsr.ts > mLastExtractionTs) mMeasurements.push_back(radioMsr); mCurrentTs = std::max(radioMsr.ts, mCurrentTs); } } std::vector RadioMeasurementBuffer::extractMeasurements() { std::vector measurements; if (mCurrentTs - mLastExtractionTs < mSignalWindowShiftMs) { // if not enough time passed return measurements; } mLastExtractionTs = mCurrentTs; // Averaging measurements from same Transmitters std::sort(mMeasurements.begin(), mMeasurements.end(), [](const RadioMeasurement& lhs, const RadioMeasurement& rhs) { return lhs.id < rhs.id; }); auto sameIdFirst = mMeasurements.cbegin(); while (sameIdFirst != mMeasurements.cend()) { auto sameIdLast = std::upper_bound(sameIdFirst, mMeasurements.cend(), *sameIdFirst, [](const RadioMeasurement& lhs, const RadioMeasurement& rhs) { return lhs.id < rhs.id; }); double nSignals = std::distance (sameIdFirst, sameIdLast); double sumRssi = std::accumulate (sameIdFirst, sameIdLast, 0.0, [](double sum, const RadioMeasurement& m) { return sum + m.rssi; }); double averRssi = sumRssi / nSignals; double sumDistances = std::accumulate (sameIdFirst, sameIdLast, 0.0, [](double sum, const RadioMeasurement& m) { return sum + m.distance; }); double averDistance = sumDistances / nSignals; RadioMeasurement radioMsr = *sameIdFirst; radioMsr.rssi = averRssi; radioMsr.distance = averDistance; measurements.push_back(radioMsr); sameIdFirst = sameIdLast; } // Erasing old measurements if (mRadioKeepPeriodMs == 0) { mMeasurements.clear(); } else { long long keepFromTs = mCurrentTs - mRadioKeepPeriodMs; std::sort(mMeasurements.begin(), mMeasurements.end(), [](const RadioMeasurement& lhs, const RadioMeasurement& rhs) { return lhs.ts < rhs.ts; }); auto eraseUpperBound = std::find_if(mMeasurements.begin(), mMeasurements.end(), [keepFromTs](const RadioMeasurement& rm) { return rm.ts >= keepFromTs; }); mMeasurements.erase(mMeasurements.begin(), eraseUpperBound); } return measurements; } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/position_estimation/src/nearest_transmitter_estimator.cpp ================================================ #include #include #include "nearest_transmitter_estimator.h" namespace navigine { namespace navigation_core { static const double A = -82.0; static const double B = 3.0; NearestTransmitterPositionEstimator::NearestTransmitterPositionEstimator(const std::vector& transmitters) { m_smoother = PositionSmoother(0.9); for (const Transmitter& t: transmitters) m_transmitters[t.id] = t; } std::vector NearestTransmitterPositionEstimator::getRegisteredTransmittersMeasurements( const std::vector& radioMsr) { std::vector registeredMeasurements; for (const RadioMeasurement& msr: radioMsr) if (m_transmitters.find(msr.id) != m_transmitters.end()) registeredMeasurements.push_back(msr); return registeredMeasurements; } Position NearestTransmitterPositionEstimator::calculatePosition(const RadioMeasurements& inputMeasurements) { Position position; position.isEmpty = true; std::vector radioMeasurements = getRegisteredTransmittersMeasurements(inputMeasurements); if (radioMeasurements.empty()) return position; auto nearestTx = std::max_element(radioMeasurements.begin(), radioMeasurements.end(), [](RadioMeasurement msr1, RadioMeasurement msr2) {return msr1.rssi < msr2.rssi; }); std::string nearestTxId = nearestTx->id; double nearestTxRssi = nearestTx->rssi; if (m_transmitters.find(nearestTxId) == m_transmitters.end()) return position; else { const Transmitter& t= m_transmitters[nearestTxId]; position.x = t.x; position.y = t.y; position.precision = std::sqrt(std::exp((A - nearestTxRssi) / B)) + 1.0; position.isEmpty = false; position.ts = radioMeasurements.back().ts; return m_smoother.smoothPosition(position); } } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/position_estimation/src/position_smoother.cpp ================================================ #include "position_smoother.h" namespace navigine { namespace navigation_core { static const double MAX_ALPHA = 0.9; static const double MIN_ALPHA = 0.1; static const double TIME_THRESHOLD_SEC = 10; PositionSmoother::PositionSmoother(double smoothing) : mWasCalled ( false ) , mSpeedX ( 0.0 ) , mSpeedY ( 0.0 ) { smoothing = std::min(1.0, std::max(0.0, smoothing)); mAlpha = MIN_ALPHA * smoothing + MAX_ALPHA * (1.0 - smoothing); } Position PositionSmoother::smoothPosition(Position pos) { if (pos.ts > mPosition.ts) { double t = (pos.ts - mPosition.ts) / 1000.0; double a = mAlpha; double b = a * a / (2.0 - a); double xp = mPosition.x + mSpeedX * t; double vxp = mSpeedX + (b / t) * (pos.x - xp); double xs = xp + a * (pos.x - xp); double yp = mPosition.y + mSpeedY * t; double vyp = mSpeedY + (b / t) * (pos.y - yp); double ys = yp + a * (pos.y - yp); bool timeIsTooLong = t > TIME_THRESHOLD_SEC; if (!mWasCalled || timeIsTooLong) { mPosition = pos; mSpeedX = 0.0; mSpeedY = 0.0; mWasCalled = true; return pos; } mSpeedX = vxp; mSpeedY = vyp; pos.x = xs; pos.y = ys; mPosition = pos; } return mPosition; } } } // namespace navigine::navigation_core ================================================ FILE: standalone_algorithms/trilateteration/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.7) project(Trilateration) set("test-target" "test") file(GLOB ${PROJECT_NAME}_TEST_SOURCES src/*.cpp) add_executable("${test-target}" ${${PROJECT_NAME}_TEST_SOURCES}) target_include_directories("${test-target}" PRIVATE "include/") set_property(TARGET "${test-target}" PROPERTY CXX_STANDARD 17) set_target_properties("${test-target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;") set_target_properties("${test-target}" PROPERTIES COMPILE_FLAGS "-fno-strict-aliasing -funwind-tables -fPIC -m64 -march=x86-64 -fstack-protector-strong -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -std=c++17 -frtti -fexceptions ") target_link_libraries("${test-target}") ================================================ FILE: standalone_algorithms/trilateteration/README.md ================================================ ### Trilateration This is a method of determining coordinates of a device using measurements of pseudo distances to all visible beacons or signal transmitters. For navigation purposes you may also operate with RSSI readings. We use this algorithm for indoor navigation along with the distance/RSSI measurements from Bluetooth LE 4.0 Beacons. Algorithm with all required functions and data structures is presented as class in trilateration.h, beacon.h files. Example of the algorithm usage and filling of data structures you can find in a unit test for this source code in the test_trilateration.cpp file. ### Algorithm Most indoor positioning are implemented based on Received Signal Strength Index (RSSI). Mobile device receives signal from beacon with known location. The distance between mobile device and beacon is calculated using RSSI of received signal. The algorithm consists of several phases: 1. Preprocessing 2. Trilateration 3. Triangulation 4. Least squares estimation Preprocessing includes deleting duplicated measurements and filtering unknowing beacons. ### Triangulation Triangulation is the process of determining the location of a point by forming triangles to the point from known points. Once mobile device knows distance from three known beacon, triangulation is performed to determine its coordinates. In the figure below there are three circles, centered at each beacon with radius equals to the distance between each beacon and mobile device. The triangulation location is the centroid of the triangle ABC, which consists of cords of the intersection part of three circles. Markdown Monster icon However if we get all distances between the mobile and reference nodes, they are not likely intersecting into one point. Therefore we need some algorithms to calculate the estimated location of the mobile. ### Least Squares Estimation LSE is widely used in the distance based positioning systems. Equation on the mobile location can be set as

where Y is a known n-dimensional vector, A is NxM matrix. If N>M, the number of equation is greater than the number of unknown numbers, we can obtain the optimal X using LSE. The idea of LSE is to make the least value of sum of square of error

We get the minimum of the above function, if is nonsingular

### Trilateration Algorithm Trilateration is the geometrical problem of determining an unknown position on a plane based on the distance to other two known vertices of a triangle (the length of two sides). Positioning by ranging is the determination of an object’s position by measuring the range to a number of objects at known locations. It is also known as lateration with trilateration denoting ranging using three signals and multilateration ranging using more than three signals. ### Build ```sh cd /standalone_algorithms/trilateration cmake -Bbuild -H. cmake --build build ``` Run tests: ```sh ./build/test ``` ================================================ FILE: standalone_algorithms/trilateteration/src/beacon.cpp ================================================ /** beacon.cpp * * Author: Aleksei Smirnov * Copyright (c) 2014 Navigine. All rights reserved. * */ #include "beacon.h" #include using namespace std; Beacon::Beacon() : mX(0.) , mY(0.) , mLocationId(0) { } //copy constructor Beacon::Beacon(const Beacon& beacon) : mX (beacon.mX) , mY (beacon.mY) , mId (beacon.mId) , mName (beacon.mName) , mLocationName (beacon.mLocationName) , mLocationId (beacon.mLocationId) { } void Beacon::setBeaconId(const string& beaconId) { Beacon::mId = beaconId; } void Beacon::fillData(const double x, /*planar coordinate: x */ const double y, /*planar coordinate: y */ const std::string& id, /*beacon identifier */ const std::string& name, /*beacon name */ const std::string& locName) { Beacon::mX = x; Beacon::mY = y; Beacon::mId = id; Beacon::mName = name; Beacon::mLocationName = locName; } const char* Beacon::getId () const { return mId.c_str(); } double Beacon::getX() const { return Beacon::mX; } double Beacon::getY( ) const { return Beacon::mY; } void Beacon::setX( const double x ) { Beacon::mX = x; } void Beacon::setY( const double y ) { Beacon::mY = y; } void Beacon::setLocationName( const char* name ) { Beacon::mLocationName += name; } void Beacon::setLocationId( int sublocId ) { Beacon::mLocationId = sublocId; } int Beacon::getLocationId() { return Beacon::mLocationId; } bool Beacon::operator==(const Beacon& entry)const { return strcmp(getId(), entry.getId()) == 0; } bool Beacon::operator!=(const Beacon& entry)const { return !(*this == entry); } IBeacon::IBeacon() : Beacon() , major(0) , minor(0) { } //copy constructor IBeacon::IBeacon(const IBeacon& iBeacon) : Beacon(iBeacon) , uuid(iBeacon.uuid) , major(iBeacon.major) , minor(iBeacon.minor) { } const char* IBeacon::getUuid ()const { return uuid.c_str(); } int IBeacon::getMajor()const { return major; } int IBeacon::getMinor()const { return minor; } void IBeacon::setMajor( const unsigned int major ) { IBeacon::major = major; } void IBeacon::setMinor( const unsigned int minor ) { IBeacon::minor = minor; } void IBeacon::setUuid( const char * uuid ) { IBeacon::uuid = uuid; } // constructor BeaconMeas::BeaconMeas() : mBeaconPtr (0) , mRssi (TRANSMITTER_POINT_UNUSED) { } //constructor BeaconMeas::BeaconMeas( Beacon* beacon, double rssi, double distance ) { BeaconMeas::mBeaconPtr = beacon; BeaconMeas::mRssi = rssi; BeaconMeas::mDistance = distance; mBeaconId = beacon->getId(); } //copy constructor BeaconMeas::BeaconMeas(const BeaconMeas& beaconMeas) : mBeaconId (beaconMeas.mBeaconId) , mBeaconPtr (beaconMeas.mBeaconPtr) , mRssi (beaconMeas.mRssi) , mDistance (beaconMeas.mDistance) { } BeaconMeas::~BeaconMeas() { mBeaconPtr = 0; } double BeaconMeas::getRssi()const { return mRssi; } double BeaconMeas::getDistance()const { return mDistance; } void BeaconMeas::setRssi(const double rssi) { BeaconMeas::mRssi = rssi; } void BeaconMeas::setDistance(const double distance) { BeaconMeas::mDistance = distance; } bool BeaconMeas::operator<(const BeaconMeas& entry)const { return mDistance < entry.mDistance; } bool BeaconMeas::operator>(const BeaconMeas& entry)const { return entry < *this; } bool BeaconMeas::operator==(const BeaconMeas& entry)const { if (getBeaconId() == 0 || entry.getBeaconId() == 0) { printf ("ERROR: id of measurement == NULL!\n"); } if ( *getBeaconId() == '\0' || *entry.getBeaconId() == '\0') printf("ERROR: id is empty\n"); return strcmp( getBeaconId(), entry.getBeaconId() ) == 0; } bool BeaconMeas::operator!=(const BeaconMeas& entry)const { return !(*this == entry); } // specify Ptr that correspond to beacon from wich we got *this measurement void BeaconMeas::setBeaconPtr( const Beacon* beaconPtr ) { BeaconMeas::mBeaconPtr = const_cast (beaconPtr); } Beacon* BeaconMeas::getBeaconPtr() const { if (BeaconMeas::mBeaconPtr == NULL) printf( "beaconId = %s : mBeaconPtr == NULL\n", BeaconMeas::getBeaconId() ); //throw exception here return BeaconMeas::mBeaconPtr; } void BeaconMeas::setBeaconId( const string& beaconId ) { BeaconMeas::mBeaconId = beaconId; } const char* BeaconMeas::getBeaconId() const { return mBeaconId.c_str(); } ================================================ FILE: standalone_algorithms/trilateteration/src/beacon.h ================================================ /** beacon.h * * Author: Aleksei Smirnov * Copyright (c) 2014 Navigine. All rights reserved. * */ #ifndef NAVIGINE_BEACON_CLASS #define NAVIGINE_BEACON_CLASS #include #include static const int TRANSMITTER_POINT_UNUSED = 1; // Beacon is transmitter emitting the signal, that we can use to obtain pseudoDistances // and process trilateration navigation algorithms class Beacon { public: Beacon(); Beacon(const Beacon&); ~Beacon() {}; //specify unique beacon identifier that allows us to distinguish beacon // and its measurements void setBeaconId(const std::string& beaconId); // func allows simultaneously fill the data: void fillData(const double x, //planar coordinate: x const double y, //planar coordinate: y const std::string& id, //beacon identifier const std::string& name, //beacon name const std::string& locName); //name of location const char * getId() const; double getX() const; double getY() const; // specify x,y, beacon planar coordinates void setX( const double x ); void setY( const double y ); // specify x,y, beacon planar coordinates void setLocationName( const char* name); void setLocationId( int id ); int getLocationId(); bool operator==(const Beacon& beacon)const; bool operator!=(const Beacon& beacon)const; private: double mX, mY; std::string mId; // id of the beacon (e.g. major+minor+uuid) std::string mName; // name of the beacon std::string mLocationName; // name of sublocation int mLocationId; // sublocation id }; // IBeacon is a beacon, invented by apple, that has additional fields: // Major,Minor, UUID identifiers class IBeacon : public Beacon { public: IBeacon(); IBeacon(const IBeacon&); const char* getUuid() const; void setUuid( const char * uuid ); int getMajor() const; int getMinor() const; void setMajor( const unsigned int major ); void setMinor( const unsigned int minor ); private: std::string uuid; unsigned int major; unsigned int minor; }; // class that keep measurements from certain transmitter (e.g. beacon) class BeaconMeas { public: BeaconMeas(); BeaconMeas( Beacon* beacon, double rssi, double distance ); ~BeaconMeas(); // copy constructor BeaconMeas( const BeaconMeas& ); double getRssi()const; //get RSSI of current measurement double getDistance()const; //get distance of current measurement const char* getBeaconId()const; //get beacon identifier Beacon* getBeaconPtr()const; //get pointer to beacon void setRssi( const double ); //set measurement RSSI void setDistance( const double ); //set measurement distance void setBeaconPtr( const Beacon* beaconPtr ); //set pointer to beacon (from which we got meas) void setBeaconId( const std::string& beaconId ); //set beacon it (that identifies measurement) bool operator<(const BeaconMeas& entry)const; bool operator>(const BeaconMeas& entry)const; bool operator==(const BeaconMeas& entry)const; bool operator!=(const BeaconMeas& entry)const; private: std::string mBeaconId; // id that allow us identify beacon and set mBeaconPtr Beacon* mBeaconPtr; // pointer to beacon from which we got measurement double mRssi; // RSSI of the measurement double mDistance; // Distance to the beacon }; #endif // NAVIGINE_BEACON_CLASS ================================================ FILE: standalone_algorithms/trilateteration/src/test_trilateration.cpp ================================================ /** test_trilateration.cpp * * Author: Aleksei Smirnov * Copyright (c) 2014 Navigine. All rights reserved. * */ #include "test_trilateration.h" #include void PrintHelp() { printf( "This is unit test that demonstrates how you can use \n" "Navigine trilateration class in order to get planar coordinates \n" "of the user on the basis of distance measurements \n" "For more questions please contact aleksei.smirnov@navigine.com \n\n" ); } int main( void ) { PrintHelp(); int sublocationIndex = 0; //fill map beacons and beaconEntries Trilateration trilateration; //fill beacon measurements depending from scenario std::vector beaconMeasurements; //fill mapBeacons entries depending from scenario std::vector mapBeacons; Beacon beacon; //test scenario. Set 3 beacons on the map beacon.fillData( 54.69, 29.51, "(53580,22667,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)", "beacon1", "floor13" ); mapBeacons.push_back( beacon ); beacon.fillData( 54.68, 29.51, "(49599,56896,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)", "beacon2", "floor13" ); mapBeacons.push_back( beacon ); beacon.fillData( 49.05, 32.16, "(57506,19633,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)", "beacon3", "floor13" ); mapBeacons.push_back( beacon ); //specify beacon Measurements BeaconMeas measurement; measurement.setBeaconId( "(53580,22667,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)" ); measurement.setRssi( -86.57 ); measurement.setDistance( 4.47 ); beaconMeasurements.push_back( measurement ); measurement.setBeaconId( "(49599,56896,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)" ); measurement.setRssi( -90.80 ); measurement.setDistance( 14.13 ); beaconMeasurements.push_back( measurement ); measurement.setBeaconId( "(57506,19633,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)"); measurement.setRssi( -86.41 ); measurement.setDistance( 15.85 ); beaconMeasurements.push_back( measurement ); //measurement from unknown beacon measurement.setBeaconId( "22211,00231" ); measurement.setRssi( -21.41 ); measurement.setDistance( 15.85 ); beaconMeasurements.push_back( measurement ); if (beaconMeasurements.size() == 0) { printf( "Number of measurements == 0\n" ); return 0; } trilateration.updateMeasurements( beaconMeasurements ); trilateration.setCurrentLocationId( 0 ); trilateration.fillLocationBeacons(mapBeacons ); int errorCode = trilateration.calculateCoordinates(); double x = trilateration.getX(); double y = trilateration.getY(); printf( "x = %lf y = %lf \n", trilateration.getX(), trilateration.getY() ); if (x != 53.692 && y != 29.977 && errorCode) { printf( "test not passed !\n check whether you changed " "something in code\n"); } else { printf("test passed!\n"); } if (errorCode) return errorCode; // std::cin.get(); return 0; } ================================================ FILE: standalone_algorithms/trilateteration/src/test_trilateration.h ================================================ /** test_trilateration.h * * Author: Aleksei Smirnov * Copyright (c) 2014 Navigine. All rights reserved. * */ #ifndef NAVIGINE_TEST_TRILATETARATION #define NAVIGINE_TEST_TRILATETARATION #include #include "beacon.h" #include "trilateration.h" #endif ================================================ FILE: standalone_algorithms/trilateteration/src/trilateration.cpp ================================================ /** trilateration.cpp * * Author: Aleksei Smirnov * Copyright (c) 2014 Navigine. All rights reserved. * */ #if defined WIN32 || defined _WIN32 #define _CRT_SECURE_NO_WARNINGS #endif #include #include "trilateration.h" using namespace std; Trilateration::Trilateration() { mXY.assign( 2, 0.0 ); mCurLocationId = 0; } Trilateration::Trilateration( const Trilateration& trilat ) : mLocationBeacons( trilat.mLocationBeacons ) , mBeaconMeas ( trilat.mBeaconMeas ) , mXY ( trilat.mXY ) , mCurLocationId ( trilat.mCurLocationId ) , mErrorMessage ( trilat.mErrorMessage ) { } Trilateration::~Trilateration() { } void Trilateration::updateMeasurements( std::vector < BeaconMeas >& beaconMeas ) { Trilateration::mBeaconMeas = beaconMeas; } void Trilateration::fillLocationBeacons( std::vector & beaconsOnFloor ) { Trilateration::mLocationBeacons = beaconsOnFloor; } int Trilateration::calculateCoordinates() { int errorCode = deleteDuplicateMeasurements( Trilateration::mBeaconMeas ); if (errorCode) return errorCode; // sort into ascending order (from -100 to 0) std::sort( mBeaconMeas.begin(), mBeaconMeas.end() ); //filter out beacons that are not in the map filterUnknownBeacons (mBeaconMeas, mLocationBeacons); if (mBeaconMeas.size () == 0 ) { printf("The number of visible beacon = %ld \n", mBeaconMeas.size ()); return ERROR_NO_SOLUTION_TRILATERATION; } // number of measurements that we'll use to obtain coordinates unsigned int nOfMeas = (mBeaconMeas.size() >= 3 ? 3 : mBeaconMeas.size()); std::vector mostStrongMeas; if (mBeaconMeas.size() >= 3) { for (unsigned int i = 0; i < nOfMeas; i++) mostStrongMeas.push_back(mBeaconMeas.at(i)); } errorCode = calculateTrilaterationCoordinates(); if (errorCode) return errorCode; const int UseOls = 0; if (UseOls) { getLeastSquaresCoordinates(); getLeastSquaresCoordinates(); } #ifdef TRILATERATION_DEBUG_ printXyToFile ("trilateration_xy_debug.txt"); printf ("x = %lf \n y = %lf \n",mXY[0],mXY[1]); #endif return 0; } int Trilateration::getCurrentLocationId() const { return Trilateration::mCurLocationId; } void Trilateration::setCurrentLocationId( int curSubloc ) { Trilateration::mCurLocationId = curSubloc; } // this function erase unknown transmitters that don't presence in a map // and fill x,y, coordinates of beacons void Trilateration::filterUnknownBeacons( vector & beaconMeas, const vector& mapBeacons ) { // don't increment iterator in for 'cause there is erase inside loop. // otherwise we'll skip 1 element after erased for (vector ::iterator it = beaconMeas.begin(); it != beaconMeas.end(); ) { std::vector::const_iterator itBeacon; itBeacon = findBeaconForMeas( mapBeacons, it->getBeaconId() ); if (itBeacon == mapBeacons.end()) { it = beaconMeas.erase( it ); if (it == beaconMeas.end()) break; } else { it->setBeaconPtr( &(*itBeacon) ); ++it; } } return; } double Trilateration::getX() const { return Trilateration::mXY[ 0 ]; } double Trilateration::getY() const { return Trilateration::mXY[ 1 ]; } const char* Trilateration::getErrorMessage() const { return mErrorMessage.c_str(); } void Trilateration::printXyToFile( char* filename ) const { FILE *f; f = fopen( filename, "w" ); fprintf( f, "%lf %lf \n", mXY[ 0 ], mXY[ 1 ] ); fclose( f ); } int Trilateration::calculateTrilaterationCoordinates() { double normalizeCoefficient = 0.0; //take revert values, because lower is the distance, greater the weight gets for (unsigned int i = 0; i < mBeaconMeas.size(); i++) normalizeCoefficient += 1.0 / fabs( mBeaconMeas[ i ].getDistance() ); vector weight( mBeaconMeas.size(), 0.0 ); for (unsigned int i = 0; i < mBeaconMeas.size(); i++) { if (mBeaconMeas[i].getBeaconPtr() == 0) { setErrorMessage("ERROR: BeaconMes it =%s : Beacon ptr == NULL\n"); printf( "%s\n", getErrorMessage() ); return ERROR_IN_TRILATER; } // calculate probability of being at beacons x,y coordinates weight[ i ] += 1.0 / (fabs( mBeaconMeas[ i ].getDistance() * normalizeCoefficient )); double beaconX = 0, beaconY = 0.; beaconX = mBeaconMeas[ i ].getBeaconPtr()->getX(); beaconY = mBeaconMeas[ i ].getBeaconPtr()->getY(); //find final coordinates according to probability mXY[ 0 ] += weight[ i ] * beaconX; mXY[ 1 ] += weight[ i ] * beaconY; } return 0; } // this function deletes Duplicate beacon measurements and averages rssi signals // from equal beacons int Trilateration::deleteDuplicateMeasurements( vector& beaconMeas ) { //sort measurements in beacon name order std::sort( beaconMeas.begin(), beaconMeas.end(), compareBeaconMeasByName ); //for (int i = 0; i < beaconEntry.size(); i++) for (std::vector::iterator itBeaconMeas = beaconMeas.begin(); itBeaconMeas != beaconMeas.end(); ++itBeaconMeas) { //count number of occurrences of itBeaconMeas std::vector::iterator it = beaconMeas.begin(); //find first occurrence std::count( beaconMeas.begin(), beaconMeas.end(), *itBeaconMeas); it = std::find( it, beaconMeas.end(), *itBeaconMeas ); int nOfMeasFromSameAp = 0; double rssi = 0.0, distance = 0.0; //find all similar entries while (it != beaconMeas.end()) { nOfMeasFromSameAp++; if ( it->getDistance() < 0) { printf( "beacon id = %s distance = %lf < 0\n", it->getBeaconId(), it->getDistance() ); return ERROR_IN_TRILATER; } //calc sum to get average rssi += it->getRssi(); distance += it->getDistance(); //clear duplicate entries // we don't clear first occurrence! if (nOfMeasFromSameAp != 1) it = beaconMeas.erase( it ); else it++; if (it == beaconMeas.end()) break; it = std::find( it, beaconMeas.end(), *itBeaconMeas ); } if (nOfMeasFromSameAp == 0) { setErrorMessage("number of measurements from the same AP == 0 ! something wrong!\n"); printf( "%s\n", getErrorMessage() ); return ERROR_IN_TRILATER; } //set average rssi to the beacon that doesn't have duplicates now rssi /= (std::max)(nOfMeasFromSameAp, 1); distance /= (std::max)(nOfMeasFromSameAp, 1); itBeaconMeas->setRssi( rssi ); } if (beaconMeas.size() < 3) { setErrorMessage( "less then 3 visible beacons in measurements " "It's not enough for trilateration\n" ); printf( "%s\n", getErrorMessage() ); return ERROR_NO_SOLUTION_TRILATERATION; } return 0; } void Trilateration::getLinearSystem( vector &matrixA, vector &b, int dim ) { int nOfVisibleBeacons = mBeaconMeas.size (); BeaconMeas firstBeacon = mBeaconMeas.front (); double xFirstBeacon = 0, yFirstBeacon = 0; xFirstBeacon = mBeaconMeas[ 0 ].getBeaconPtr()->getX(); yFirstBeacon = mBeaconMeas[ 0 ].getBeaconPtr()->getY(); double firstBeaconDistance = mBeaconMeas.front().getDistance(); double normaFirstBeacon = xFirstBeacon * xFirstBeacon + yFirstBeacon * yFirstBeacon; for ( int i = 0; i < nOfVisibleBeacons - 1; i++ ) { // fill the matrix A and right part of linear system b double x = 0.0, y = 0.0; x = mBeaconMeas[ i + 1 ].getBeaconPtr()->getX(); y = mBeaconMeas[ i + 1 ].getBeaconPtr()->getY(); double distance = mBeaconMeas[ i + 1].getDistance(); matrixA[ i * dim ] = 2 * (x - xFirstBeacon); matrixA[ i * dim + 1 ] = 2 * ( y - yFirstBeacon ); double norma = x * x + y * y; b[ i ] = firstBeaconDistance * firstBeaconDistance - distance * distance - normaFirstBeacon + norma; } return; } // This function estimate x,y coordinates by triangulation algorithm // using ordinary least squares method for solving linear system void Trilateration::getLeastSquaresCoordinates() { // How many coordinates do we consider? In planar case it equal to 2 int dim = 2; int nOfVisibleBeacons = mBeaconMeas.size(); // create matrix for triangulation linear system, that we will solve // for obtaining the coordinates we need at least dim + 1 beacons // index [i][j] = i * dim + j // By subtracting the last equation from each other we bring our // nonlinear system to linear matrixA vector matrixA((nOfVisibleBeacons - 1) * dim, 0.0); vector b(nOfVisibleBeacons - 1, 0.0 ); getLinearSystem(matrixA, b, dim); solveLinearSystem (matrixA, b); } // this function solve overdetermined linear system // by ordinary least squares method x = A_ * B // where A_ - pseudo inverse matrix void Trilateration::solveLinearSystem( vector matrixA, vector b ) { int nOfEquations = b.size(); int dim = matrixA.size() / nOfEquations; vector xy(dim, 0.); vector aTransposeA(dim * dim, 0.); // find pseudoInverseMatrix for (int row = 0; row < dim; row++) { for (int col = 0; col < dim; col++) { for ( int inner = 0; inner < nOfEquations; inner++ ) { // Multiply the row of A_transpose by the column of A // to get the row, column of multiplyAATranspose. aTransposeA[ row * dim + col ] += matrixA[ inner * dim + row ] * matrixA[ inner * dim + col ]; } } } vector revertMatrix( dim * dim, 0. ); double det = aTransposeA[ 0 ] * aTransposeA[ 3 ] - aTransposeA[ 2 ] * aTransposeA[ 1 ]; //simple formula for invert matrix 2x2 revertMatrix[ 0 ] = aTransposeA[ 3 ] / det; revertMatrix[ 1 ] = -aTransposeA[ 1 ] / det; revertMatrix[ 2 ] = - aTransposeA[2] / det; revertMatrix[ 3 ] = aTransposeA[0] / det; //Multiply revertMatrix on A transpose vector matrix2xN (dim * nOfEquations, 0.0); for ( int row = 0; row < dim; row++ ) { for ( int col = 0; col < nOfEquations; col++ ) { for ( int inner = 0; inner < dim; inner++ ) { // Multiply the row of A_transpose by the column of A // to get the row, column of multiplyAATranspose. matrix2xN[ row * nOfEquations + col ] += revertMatrix[ row * dim + inner] * matrixA[ col * dim + inner ]; } } } //Multiply matrix2xN on B vector for ( int col = 0; col < dim; col++ ) { for ( int inner = 0; inner < nOfEquations; inner++ ) { xy[col] += matrix2xN[col * nOfEquations + inner] * b[inner]; } } return; } void Trilateration::setErrorMessage(const char* errorMsg ) { Trilateration::mErrorMessage += errorMsg; } bool compareBeaconMeasByName( BeaconMeas first, BeaconMeas second ) { return strcmp( first.getBeaconId(), second.getBeaconId() ) < 0; } std::vector::const_iterator findBeaconForMeas( const vector& mapBeacons, const string& measureBeaconId ) { std::vector::const_iterator it; for (it = mapBeacons.begin(); it != mapBeacons.end(); ++it) { if (it->getId() == measureBeaconId) return it; } return it; } ================================================ FILE: standalone_algorithms/trilateteration/src/trilateration.h ================================================ /** trilateration.h * * Author: Aleksei Smirnov * Copyright (c) 2014 Navigine. All rights reserved. * */ #ifndef NAVIGINE_TRILATERATION_ALGORITHM #define NAVIGINE_TRILATERATION_ALGORITHM static const int ERROR_NO_SOLUTION_TRILATERATION = 4; static const int ERROR_IN_TRILATER = 28; #include #include #include #include #include "beacon.h" // this class allows calculate coordinates of the user using distance or RSSI // measurements to nearest transmitters such as bluetooth Ibeacons or WiFi Access Points // class Trilateration { public: //constructor and destructor Trilateration(); ~Trilateration(); //copy constructor Trilateration(const Trilateration& trilat); // update measurements if we got new ones void updateMeasurements( std::vector < BeaconMeas >& beaconMeasurements ); // fill all the beacons located at the map void fillLocationBeacons( std::vector & beaconsOnFloor ); // calculate desired coordinates on the measurement basis int calculateCoordinates( ); //get id of current location int getCurrentLocationId() const; //set id of current location void setCurrentLocationId( int curLoc ); static void filterUnknownBeacons( std::vector & beaconMeas, const std::vector& mapBeacons ); // get X and Y coordinate calculated by trilateration double getX() const; double getY() const; //get error message in a case of error const char* getErrorMessage() const; //print X,Y to file in "%lf %lf \n" format void printXyToFile (char* filename) const; private: std::vector mLocationBeacons; //set of beacons at the current location std::vector mBeaconMeas; //set of beacon measurements std::vector mXY; //desired X,Y coordinates int mCurLocationId; //id of location std::string mErrorMessage; //descriptive error message int calculateTrilaterationCoordinates( ); //calculate coordinates (when data is prepared) int deleteDuplicateMeasurements (std::vector& BeaconMeasurements ); ////delete duplicate meas and get average RSSI, Dist. void getLinearSystem( //if we have precise measurements, std::vector &matrixA, //we determine coordinates by OLS method std::vector &b, //by solving overdetermined system int dim ); void getLeastSquaresCoordinates( ); void solveLinearSystem( //solve overdetermined system if std::vector matrixA, //pseudo distance equations std::vector b ); void setErrorMessage( const char* errorMsg ); //set error message }; //we use this func in order to sort the beacon measurements bool compareBeaconMeasByName( BeaconMeas first, BeaconMeas second ); //find iterator of beacon, from which we got measurements std::vector::const_iterator findBeaconForMeas( const std::vector& mapBeacons, const std::string& measureBeaconId ); #endif ================================================ FILE: tests/navigation_test.cpp ================================================ /** navigation_test.cpp * * Copyright (c) 2014 Navigine. * */ #include #include #include #include #include #include #include #include #include #include "../tools/verification/helpers.h" #include using namespace navigine::navigation_core; void PrintUsage(const char* program) { printf("Usage:\n" "\t%s {MAP_FILE} {LOG_FILE} {SETTINGS_JSON} {NAV_BATCH_SIZE}\n", program); } void BuildPdrTrack(const double pdrDistance, const double pdrHeading, double& pdrX, double& pdrY) { static double prevPdrDistance = 0.0, prevPdrHeading = 0.0; if (pdrDistance != prevPdrDistance && pdrHeading != prevPdrHeading) { prevPdrDistance = pdrDistance; prevPdrHeading = pdrHeading; pdrX += pdrDistance * std::cos(pdrHeading); pdrY += pdrDistance * std::sin(pdrHeading); } } int main(int argc, char** argv) { if (argc < 5) { PrintUsage(argv[0]); return -1; } std::string geojsonFile = argv[1]; std::string logFile = argv[2]; std::string settingsFile = argv[3]; const int navBatchSize = std::stoi(argv[4]); // This variable responsible for how many messages we want to parse and pass to navigation in one batch. int errorCode = 0; GeoLevels geolevels = ParseGeojson(geojsonFile, errorCode); NavigationSettings navProps = CreateSettingsFromJson(settingsFile, errorCode); if (errorCode != 0) { std::cerr << "error parsing json file " << errorCode << std::endl; return errorCode; } std::shared_ptr levelCollector = createLevelCollector(); for (const auto& geolevel: geolevels) { levelCollector->addGeoLevel(*(geolevel.get())); } std::shared_ptr navClient = createNavigationClient(levelCollector, navProps); double pdrX = 0.0, pdrY = 0.0; auto navMessages = GetNavMessages(logFile); std::vector> navBatchMessages; for(size_t i = 0; i < navMessages.size(); i += navBatchSize) { auto last = std::min(navMessages.size(), i + navBatchSize); navBatchMessages.emplace_back(navMessages.begin() + i, navMessages.begin() + last); } long long firstTs = 0; for (const auto& navBatchInput: navBatchMessages) { if (firstTs == 0) { firstTs = navBatchInput[0].ts; } const std::vector navBatchOutput = navClient->navigate(navBatchInput); const std::vector navStates = navClient->getStates(); for (std::size_t i = 0; i < navBatchOutput.size(); ++i) { if (navBatchOutput[i].status == NavigationStatus::OK) { BuildPdrTrack(navStates[i].getStepLen(), navStates[i].getHeading(), pdrX, pdrY); } std::string providerStr; switch (navBatchOutput[i].provider) { case Provider::GNSS: providerStr = "GNSS"; break; case Provider::INDOOR: providerStr = "Indoor"; break; case Provider::FUSED: providerStr = "Fused"; break; case Provider::NONE: providerStr = "none"; break; default: providerStr = "unknown provider"; } std::cout << std::setprecision(10); std::cout << std::fixed; long long currentTs = navBatchInput[i].ts; std::cout << ((currentTs - firstTs) / 1000) << " " << navBatchOutput[i].posLatitude << " " << navBatchOutput[i].posLongitude << " " << navBatchOutput[i].posAltitude << " " << pdrX << " " << pdrY << " " << navBatchOutput[i].posOrientation << " " << navBatchOutput[i].posLevel.value.c_str() << " " << navBatchOutput[i].status << std::endl; } } return 0; } ================================================ FILE: tools/verification/helpers.cpp ================================================ #include #include #include #include #include #include #include #include #include "helpers.h" #include #include "../../src/navigation_error_codes.h" using namespace navigine::navigation_core; static double const GRAPH_SAME_POINTS_GEO_DIST = 0.1; namespace nlohmann { NLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::RadioMeasurementData::Type, { {navigine::navigation_core::RadioMeasurementData::Type::WIFI, 201}, {navigine::navigation_core::RadioMeasurementData::Type::BEACON, 203}, {navigine::navigation_core::RadioMeasurementData::Type::BLUETOOTH, 202}, {navigine::navigation_core::RadioMeasurementData::Type::WIFI_RTT, 205} }) NLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::TransmitterType, { {navigine::navigation_core::TransmitterType::WIFI, "WIFI"}, {navigine::navigation_core::TransmitterType::BEACON, "BEACON"}, {navigine::navigation_core::TransmitterType::BLUETOOTH, "BLUETOOTH"}, {navigine::navigation_core::TransmitterType::EDDYSTONE, "EDDYSTONE"}, {navigine::navigation_core::TransmitterType::LOCATOR, "LOCATOR"}, {navigine::navigation_core::TransmitterType::UNKNOWN, "UNKNOWN"} }) NLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::SensorMeasurementData::Type, { {navigine::navigation_core::SensorMeasurementData::Type::ACCELEROMETER, 101}, {navigine::navigation_core::SensorMeasurementData::Type::GYROSCOPE, 102}, {navigine::navigation_core::SensorMeasurementData::Type::MAGNETOMETER, 103}, {navigine::navigation_core::SensorMeasurementData::Type::BAROMETER, 104}, {navigine::navigation_core::SensorMeasurementData::Type::LOCATION, 106}, {navigine::navigation_core::SensorMeasurementData::Type::ORIENTATION, 105} }) NLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::CommonSettings::UseAlgorithm, { {navigine::navigation_core::CommonSettings::UseAlgorithm::PF_PDR, "PF_PDR"}, {navigine::navigation_core::CommonSettings::UseAlgorithm::PF_NOPDR, "PF_NOPDR"}, {navigine::navigation_core::CommonSettings::UseAlgorithm::ZONES, "ZONES"}, {navigine::navigation_core::CommonSettings::UseAlgorithm::GRAPH, "GRAPH"}, {navigine::navigation_core::CommonSettings::UseAlgorithm::KNN, "KNN"} }) NLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::CommonSettings::MeasurementType, { {navigine::navigation_core::CommonSettings::MeasurementType::RSSI, "rssi"}, {navigine::navigation_core::CommonSettings::MeasurementType::DISTANCE_V1, "distance_v1"}, {navigine::navigation_core::CommonSettings::MeasurementType::DISTANCE_V2, "distance_v2"} }) NLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::CommonSettings::SignalsToUse, { {navigine::navigation_core::CommonSettings::SignalsToUse::BLE, "ble"}, {navigine::navigation_core::CommonSettings::SignalsToUse::WIFI, "wifi"}, {navigine::navigation_core::CommonSettings::SignalsToUse::BOTH, "both"} }) static void from_json(const nlohmann::json& j, SensorMeasurementData& sensorMsr) { j.at("type").get_to(sensorMsr.type); std::vector values; j.at("values").get_to(values); if (values.size() == 3) sensorMsr.values = Vector3d(values.at(0), values.at(1), values.at(2)); } static void from_json(const nlohmann::json& j, RadioMeasurementData& radioMsr) { j.at("type").get_to(radioMsr.type); std::string id; j.at("bssid").get_to(id); radioMsr.id = TransmitterId(id); if (j.count("rssi")) j.at("rssi").get_to(radioMsr.rssi); if (j.count("distance")) j.at("distance").get_to(radioMsr.distance); } static void from_json(const nlohmann::json& j, NmeaMeasurementData& nmeaMsr) { j.at("sentence_number").get_to(nmeaMsr.sentenceNumber); j.at("num_sats").get_to(nmeaMsr.satellitesNumber); } static void from_json(const nlohmann::json& j, Measurement& msr) { j.at("timestamp").get_to(msr.ts); int msrType; j.at("type").get_to(msrType); if (msrType == 101 || msrType == 102 || msrType == 103 || msrType == 104 || msrType == 105 || msrType == 106) msr.data = j.get(); else if (msrType == 201 || msrType == 202 || msrType == 203 || msrType == 204 || msrType == 205) msr.data = j.get(); else if (msrType == 300) msr.data = j.get(); else std::cerr << "Unknown measurement." << std::endl; } static void from_json(const nlohmann::json& j, CommonSettings& commonSettings) { std::vector keys; for (auto& el : j.items()) { keys.push_back(el.key()); } try { for (const auto& key : keys) { if (key == "Use_algorithm") j.at(key).get_to(commonSettings.useAlgorithm); else if (key == "Correction_noise") j.at(key).get_to(commonSettings.lklCorrectionNoise); else if (key == "Use_enu_azimuth") j.at(key).get_to(commonSettings.useEnuAzimuth); else if (key == "Differention_mode") j.at(key).get_to(commonSettings.useDifferentionMode); else if (key == "Use_beacon_power") j.at(key).get_to(commonSettings.useTxPower); else if (key == "Use_radiomap") j.at(key).get_to(commonSettings.useRadiomap); else if (key == "Use_stops") j.at(key).get_to(commonSettings.useStops); else if (key == "Use_tracking") j.at(key).get_to(commonSettings.useTracking); else if (key == "Stop_detection_time") j.at(key).get_to(commonSettings.stopDetectionTime); else if (key == "Min_deviation_m") j.at(key).get_to(commonSettings.minDeviationM); else if (key == "Average_speed") j.at(key).get_to(commonSettings.averageMovSpeed); else if (key == "Measurement_type") j.at(key).get_to(commonSettings.measurementType); else if (key == "Use_best_ref_point_level") j.at(key).get_to(commonSettings.useBestRefPointLevel); else if (key == "Use_barometer") j.at(key).get_to(commonSettings.useBarometer); else if (key == "Use_unknown_beacons") j.at(key).get_to(commonSettings.useUnknownTransmitters); else if (key == "Cutoff_rss") j.at(key).get_to(commonSettings.sigCutOffRss); else if (key == "Use_closest_aps") j.at(key).get_to(commonSettings.useClosestAps); else if (key == "N_closest_aps") j.at(key).get_to(commonSettings.numClosestAps); else if (key == "Sig_averaging_time") j.at(key).get_to(commonSettings.sigAveragingTime); else if (key == "Sig_window_shift") j.at(key).get_to(commonSettings.sigWindowShift); else if (key == "Use_Signals") j.at(key).get_to(commonSettings.signalsToUse); else if (key == "N_particles") j.at(key).get_to(commonSettings.numParticles); else if (key == "Min_msr_num_for_mutation") j.at(key).get_to(commonSettings.minMsrNumForMutation); else if (key == "Resampling_threshold_num") j.at(key).get_to(commonSettings.resamplingThreshold); else if (key == "Min_num_of_particles") j.at(key).get_to(commonSettings.minNumParticlesAlive); else if (key == "Gyro_noise_degree") j.at(key).get_to(commonSettings.gyroNoiseDegree); else if (key == "Step_noise_deviation_meters") j.at(key).get_to(commonSettings.stepNoiseDeviationMeters); else if (key == "Use_projection_to_white_area") j.at(key).get_to(commonSettings.useProjectionToWhiteArea); else if (key == "N_particles_around_closest_AP") j.at(key).get_to(commonSettings.numParticlesAroundClosestAp); else if (key == "N_particles_to_mutate") j.at(key).get_to(commonSettings.numParticlesToMutate); else if (key == "Num_sampling_attempts") j.at(key).get_to(commonSettings.numSamplingAttempts); else if (key == "Num_initialization_attempts") j.at(key).get_to(commonSettings.numInitializationAttempts); else if (key == "Use_uniform_sampling") j.at(key).get_to(commonSettings.useUniformSampling); else if (key == "K") j.at(key).get_to(commonSettings.kNeighbors); else if (key == "Use_triangles") j.at(key).get_to(commonSettings.useTriangles); else if (key == "Use_diff_mode") j.at(key).get_to(commonSettings.useDiffMode); else if (key == "Min_num_of_measurements_for_position_calculation") j.at(key).get_to(commonSettings.minNumOfMeasurementsForPositionCalculation); else if (key == "Smoothing") j.at(key).get_to(commonSettings.smoothing); else if (key == "Dead_reckon_time") j.at(key).get_to(commonSettings.deadReckoningTime); else if (key == "Stop_update_time") j.at(key).get_to(commonSettings.stopUpdateTime); else if (key == "Graph_prog_distance") j.at(key).get_to(commonSettings.graphProjDist); else if (key == "Stopped_distance_threshold_m") j.at(key).get_to(commonSettings.useStopsDistanceThresholdM); else if (key == "Use_smoothing") j.at(key).get_to(commonSettings.useSmoothing); else if (key == "Use_graph_projection") j.at(key).get_to(commonSettings.useGraphProjection); else if (key == "Use_accuracy_for_smoothing") j.at(key).get_to(commonSettings.useAccuracyForSmoothing); else if (key == "Use_speed_constraint") j.at(key).get_to(commonSettings.useSpeedConstraint); else if (key == "Use_barriers") j.at(key).get_to(commonSettings.useBarriers); else if (key == "Use_time_smoothing") j.at(key).get_to(commonSettings.useTimeSmoothing); else if (key == "Use_ab_filter") j.at(key).get_to(commonSettings.useAbFilter); else if (key == "Use_gps") j.at(key).get_to(commonSettings.useGps); else if (key == "Use_instant_gps_position") j.at(key).get_to(commonSettings.useInstantGpsPosition); else if (key == "Prefer_indoor_solution") j.at(key).get_to(commonSettings.preferIndoorSolution); else if (key == "Fuse_gps") j.at(key).get_to(commonSettings.fuseGps); else if (key == "Use_gps_outside_map") j.at(key).get_to(commonSettings.useGpsOutsideMap); else if (key == "Use_gps_sigma_filter") j.at(key).get_to(commonSettings.useGpsSigmaFilter); else if (key == "Sigma_filter_window") j.at(key).get_to(commonSettings.sigmaFilterWindow); else if (key == "Sigma_filter_delta") j.at(key).get_to(commonSettings.sigmaFilterDelta); else if (key == "Min_num_sats") j.at(key).get_to(commonSettings.minNumSats); else if (key == "Prior_deviation") j.at(key).get_to(commonSettings.priorDeviation); else if (key == "Gps_deviation") j.at(key).get_to(commonSettings.minGpsDeviation); else if (key == "Maximum_to_accept_gps_measurement") j.at(key).get_to(commonSettings.maxGpsDeviation); else if (key == "Fuse_gps_border_m") j.at(key).get_to(commonSettings.fuseGpsBorderM); else if (key == "Position_old_for_fusing_sec") j.at(key).get_to(commonSettings.positionIsTooOldSec); else if (key == "Gps_valid_time_window_sec") j.at(key).get_to(commonSettings.gpsValidTimeWindowSec); else if (key == "Use_calculated_azimuth") j.at(key).get_to(commonSettings.useCalculatedAzimuth); else if (key == "Device_azimuth_lifetime_seconds") j.at(key).get_to(commonSettings.deviceAzimuthLifetimeSeconds); else if (key == "Step_multiplier") j.at(key).get_to(commonSettings.stepMultiplier); else if (key == "Has_accelerometer") j.at(key).get_to(commonSettings.hasAccelerometer); else if (key == "Has_gyroscope") j.at(key).get_to(commonSettings.hasGyroscope); else if (key == "No_signal_time_threshold") j.at(key).get_to(commonSettings.noSignalTimeThreshold); else if (key == "No_action_time_threshold") j.at(key).get_to(commonSettings.noActionTimeThreshold); else if (key == "WiFi_RTT_Offset") j.at(key).get_to(commonSettings.wiFiRttOffset); else std::cerr << "unknown setting: " << key << '\n'; } } catch (json::exception& e) { std::cerr << "parsing json " << j << " following exception occurred" << '\n' << "message: " << e.what() << '\n'; } } static void from_json(const nlohmann::json& j, LevelSettings& levelSettings) { std::vector keys; for (auto& el : j.items()) { keys.push_back(el.key()); } try { for (const auto& key : keys) { if (key == "A") j.at(key).get_to(levelSettings.normalModeA); else if (key == "B") j.at(key).get_to(levelSettings.normalModeB); else std::cerr << "unknown setting: " << key << '\n'; } } catch (json::exception& e) { std::cerr << "parsing json " << j << " following exception occurred" << '\n' << "message: " << e.what() << '\n'; } } } // nlohmann std::vector GetNavMessages(const std::string& jsonFile) { std::vector inputs; std::ifstream infile(jsonFile); if (!infile.good()) std::cout << "GetNavMessages ERROR: CAN NOT READ FILE " << jsonFile << " !!!" << std::endl; std::string line; while (std::getline(infile, line)) { auto j = nlohmann::json::parse(line); inputs.emplace_back(j.get()); } return inputs; } double getGeoDist(const GeoPoint& p1, const GeoPoint& p2) { static const double R = 6378000; double lat1Rad = M_PI * p1.latitude / 180.0; double lat2Rad = M_PI * p2.latitude / 180.0; double lng1Rad = M_PI * p1.longitude / 180.0; double lng2Rad = M_PI * p2.longitude / 180.0; double dlat = lat1Rad - lat2Rad; double dlng = lng1Rad - lng2Rad; double a = std::pow(std::sin(dlat / 2.0), 2) + std::cos(lat1Rad) * std::cos(lat2Rad) * std::pow(std::sin(dlng / 2.0), 2); double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a)); double d = R * c; return d; } GeoPoint removeSame(std::vector& allPoints) { assert(!allPoints.empty()); GeoPoint p = allPoints.front(); auto it = allPoints.begin(); int counter = 0; while (it != allPoints.end()) { if (getGeoDist(*it, p) < GRAPH_SAME_POINTS_GEO_DIST) { it = allPoints.erase(it); counter++; } else it++; } return p; } int getVertex(const GeoPoint& firstPoint, const std::unordered_map& vertexMap) { for (const auto& it: vertexMap) { int v = it.first; GeoPoint p = it.second; if (getGeoDist(firstPoint, p) < GRAPH_SAME_POINTS_GEO_DIST) return v; } throw std::logic_error("Vertex was not found!"); return -1; } std::pair> parseGraph(const nlohmann::json& j) { auto propertiesJson = j.at("properties"); std::string levelId = propertiesJson.at("level"); auto geometryJson = j.at("geometry"); auto coordinatesJson = geometryJson.at("coordinates"); std::vector allPoints; std::vector> edgesPoints; for (const auto& lineJson: coordinatesJson) { std::vector points; for (const auto& pointJson: lineJson) { std::vector pointCoordinates; pointJson.get_to(pointCoordinates); //geojson has longitude:latitude format GeoPoint geoPoint(pointCoordinates.at(1), pointCoordinates.at(0)); points.push_back(geoPoint); allPoints.push_back(geoPoint); } edgesPoints.push_back(points); } int v = 0; std::unordered_map vertexMap; while (!allPoints.empty()) { v++; GeoPoint p = removeSame(allPoints); vertexMap[v] = p; } std::map> adjset; for (std::vector& edge: edgesPoints) { assert(edge.size() == 2); GeoPoint p1 = edge[0]; int v1 = getVertex(p1, vertexMap); GeoPoint p2 = edge[1]; int v2 = getVertex(p2, vertexMap); if (adjset.find(v1) == adjset.end()) adjset[v1] = std::set(); adjset[v1].insert(v2); if (adjset.find(v2) == adjset.end()) adjset[v2] = std::set(); adjset[v2].insert(v1); } Graph graph; std::map::Vertex> addedVertices; for (auto& it: adjset) { int v1 = it.first; std::set adjacentVertexes = it.second; GeoPoint p1 = vertexMap[v1]; Graph::Vertex vertex1 = graph.addVertex(p1); addedVertices.insert({v1, vertex1}); for (int v2: adjacentVertexes) { if (addedVertices.find(v2) == addedVertices.end()) { GeoPoint p2 = vertexMap[v2]; Graph::Vertex vertex2 = graph.addVertex(p2); addedVertices.insert({v2, vertex2}); graph.addEdge(vertex1.id, vertex2.id); } else { Graph::Vertex vertex2 = addedVertices[v2]; graph.addEdge(vertex1.id, vertex2.id); } } } return std::make_pair(LevelId(levelId), graph); } std::pair> parseTransmitter(nlohmann::json j) { auto properties = j.at("properties"); std::string levelId = properties.at("level"); //TODO transmitters do not contain pathloss model in configuration file! double A = -80.0; double B = 3.0; if (properties.count("A")) A = properties.at("A"); if (properties.count("B")) B = properties.at("B"); PathlossModel pathlossModel{A, B, 0}; std::string id = properties.at("uuid"); TransmitterType type; auto transmitterTypeJson = properties.at("transmitter_type"); transmitterTypeJson.get_to(type); std::vector pointCoordinates; auto geometryJson = j.at("geometry"); geometryJson.at("coordinates").get_to(pointCoordinates); //geojson has longitude:latitude format GeoPoint3D geopoint3D(pointCoordinates.at(1), pointCoordinates.at(0), 0.0); if (pointCoordinates.size() == 3) geopoint3D = GeoPoint3D(pointCoordinates.at(1), pointCoordinates.at(0), pointCoordinates.at(2)); Transmitter transmitter(TransmitterId(id), geopoint3D, pathlossModel, type); return std::make_pair(LevelId(levelId), transmitter); } std::pair> parseBarriers(const nlohmann::json& j) { auto propertiesJson = j.at("properties"); auto levelId = propertiesJson.at("level"); auto geometryJson = j.at("geometry"); auto barriersPolygonsJson = geometryJson.at("coordinates"); std::list levelPolygons; for (const auto& polygonJson: barriersPolygonsJson) { Polygon barrier; for (const auto& pointJson: polygonJson[0]) { std::vector pointCoordinates; pointJson.get_to(pointCoordinates); //geojson has longitude:latitude format boost::geometry::append(barrier, Point(pointCoordinates.at(1), pointCoordinates.at(0))); } levelPolygons.emplace_back(barrier); } return std::make_pair(LevelId(levelId), levelPolygons); } std::pair> parseReferencePoint(const nlohmann::json& j) { auto properties = j.at("properties"); std::string levelId = properties.at("level"); std::string id = properties.at("uuid"); std::vector pointCoordinates; auto geometry = j.at("geometry"); geometry.at("coordinates").get_to(pointCoordinates); //geojson has longitude:latitude format GeoPoint geopoint(pointCoordinates.at(1), pointCoordinates.at(0)); std::map fingerprints; auto entriesJson = properties.at("entries"); for (const auto& entryJson: entriesJson) { std::string bssid = entryJson.at("bssid"); TransmitterType sigType; entryJson.at("type").get_to(sigType); std::map distribution; for (const auto& valueJson: entryJson.at("value")) { int rssi; int count; valueJson.at("rssi").get_to(rssi); valueJson.at("count").get_to(count); distribution.insert({ rssi, count }); } // Compute number of measurements, mean and variance of signal double meanRssi = 0.0; double varianceRssi = 0.0; double sumOfSquares = 0.0; double squaredSum = 0.0; size_t nMeas = 0; for (auto const& pair : distribution) { nMeas += pair.second; meanRssi += (-pair.first) * pair.second; sumOfSquares += pair.second * pair.first * pair.first; squaredSum += pair.second * pair.first; } meanRssi /= nMeas; squaredSum *= squaredSum; SignalStatistics sigStat; sigStat.mean = meanRssi; sigStat.variance = varianceRssi; sigStat.nMeasurements = nMeas; sigStat.type = sigType; fingerprints.insert({TransmitterId(bssid), sigStat}); } return std::make_pair(LevelId(levelId), ReferencePoint(ReferencePointId(id), geopoint, fingerprints)); } std::pair> parseLevel(const nlohmann::json& j) { auto propertiesJson = j.at("properties"); auto levelId = propertiesJson.at("level"); auto geometryJson = j.at("geometry"); auto allowedAreaPolygonsJson = geometryJson.at("coordinates"); std::list levelPolygons; for (const auto& polygonJson: allowedAreaPolygonsJson) { int count = 0; Polygon allowedArea; allowedArea.inners().resize(polygonJson.size() - 1); for (const auto& circleJson: polygonJson) { for (const auto& pointJson: circleJson) { std::vector pointCoordinates; pointJson.get_to(pointCoordinates); //geojson has longitude:latitude format if (count == 0) boost::geometry::append(allowedArea.outer(), Point(pointCoordinates.at(1), pointCoordinates.at(0))); else boost::geometry::append(allowedArea.inners()[count - 1], Point(pointCoordinates.at(1), pointCoordinates.at(0))); } count++; } levelPolygons.emplace_back(allowedArea); } return std::make_pair(LevelId(levelId), levelPolygons); } GeoLevels ParseGeojson(const std::string& jsonFile, int& errorCode) { errorCode = 0; std::ifstream is(jsonFile); nlohmann::json j = nlohmann::json::parse(is); std::map> levels; auto features = j.at("features"); for (const auto& feature: features) { auto featureProperties = feature.at("properties"); std::string featureType = featureProperties.at("type"); if (featureType == "level") { std::pair> levelPair = parseLevel(feature); levels.insert(levelPair); } } std::map> barriers; std::map transmitters; std::map> graphs; std::map referencePoints; for (const auto& feature: features) { auto featureProperties = feature.at("properties"); std::string featureType = featureProperties.at("type"); if (featureType == "graph") { std::pair> graphPair = parseGraph(feature); graphs.insert(graphPair); } if (featureType == "transmitter") { std::pair> transmitterPair = parseTransmitter(feature); if (transmitters.find(transmitterPair.first) == transmitters.end()) transmitters[transmitterPair.first] = std::vector>(); transmitters[transmitterPair.first].push_back(transmitterPair.second); } if (featureType == "reference_point") { std::pair> refpointsPair = parseReferencePoint(feature); if (referencePoints.find(refpointsPair.first) == referencePoints.end()) referencePoints[refpointsPair.first] = GeoReferencePoints(); referencePoints[refpointsPair.first].push_back(refpointsPair.second); } } //TODO it is possible to have levels without transmitters so why we should //check levels mismatch??? //if (!transmitters.empty() && !haveEqualKeys(transmitters, levels)) //{ // errorCode = MISSING_LEVELS; // std::cout << "transmitters levels mismatch!" << std::endl; // return GeoLevels(); //} //if (!graphs.empty() && !haveEqualKeys(graphs, levels)) //{ // errorCode = MISSING_LEVELS; // std::cout << "graphs levels mismatch!" << std::endl; // return GeoLevels(); //} //if (!referencePoints.empty() && !haveEqualKeys(referencePoints, levels)) //{ // errorCode = MISSING_LEVELS; // std::cout << "refpoints levels mismatch!" << std::endl; // return GeoLevels(); //} std::vector > vLevels; for (const auto& entry : levels) { auto geolevel = std::make_shared(); geolevel->id.value = entry.first.value; geolevel->altitude = 100.0; //TODO if (graphs.find(entry.first) != graphs.end()) geolevel->graph = graphs.at(entry.first); geolevel->geometry = getGeometry(entry.second); if (transmitters.find(entry.first) != transmitters.end()) geolevel->transmitters = transmitters.at(entry.first); if (referencePoints.find(entry.first) != referencePoints.end()) geolevel->referencePoints = referencePoints.at(entry.first); vLevels.push_back(std::move(geolevel)); } return vLevels; } NavigationSettings CreateSettingsFromJson( const std::string& jsonFile, int& errorCode) { NavigationSettings navigationSettings; errorCode = 0; std::ifstream is(jsonFile); if (!is.good()) { std::cout << "CreateSettingsFromJson ERROR: CAN NOT READ FILE " << jsonFile << " !!!" << std::endl; errorCode = ERROR_OPEN_FILE; return navigationSettings; } nlohmann::json j = nlohmann::json::parse(is); auto levelsSettingsJson = j.at("level_settings"); for (const auto& levelSettingsJson: levelsSettingsJson) { std::string levelId = levelSettingsJson.at("level"); navigationSettings.levelsSettings[LevelId(levelId)] = levelSettingsJson.at("settings")[0].get(); } navigationSettings.commonSettings = j.at("common_settings")[0].get(); return navigationSettings; } ================================================ FILE: tools/verification/helpers.h ================================================ #ifndef _HELPERS_H_ #define _HELPERS_H_ #include #include #include #include #include struct NavigationPoint { NavigationPoint(){} NavigationPoint(long long _timeMs, double _lat, double _lng, double _angle, const navigine::navigation_core::LevelId& _level) : timeMs (_timeMs) , lat (_lat) , lng (_lng) , angle (_angle) , level (_level) { } long long timeMs = 0; double lat = 0; double lng = 0; double angle = 0; navigine::navigation_core::LevelId level; }; template bool haveEqualKeys(const Map1& lhs, const Map2& rhs) { auto pred = [](decltype(*lhs.begin()) a, decltype(*rhs.begin()) b) { return a.first == b.first; }; return lhs.size() == rhs.size() && std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred); } std::vector GetNavMessages(const std::string& jsonFile); navigine::navigation_core::GeoLevels ParseGeojson( const std::string& jsonFile, int& errorCode); navigine::navigation_core::NavigationSettings CreateSettingsFromJson( const std::string& jsonFile, int& errorCode); #endif //_HELPERS_H_