TeamA F22: External state estimation (#171)

* Added new wiki pages for robotic total stations and ultra wide band beacon positioning.

---------

Co-authored-by: John Harrington <[email protected]>

Author: nevalsar

_data/navigation.yml

@@ -110,8 +110,12 @@ wiki:
         url: /wiki/sensing/gps/
       - title: Intel Realsense
         url: /wiki/sensing/realsense/
+      - title: Robotic Total Station (Leica TS16)
+        url: /wiki/sensing/robotic-total-stations.md
       - title: Thermal Cameras
         url: /wiki/sensing/thermal-cameras/
+      - title: DWM1001 UltraWideband Positioning System
+        url: /wiki/sensing/ultrawideband-beacon-positioning.md
   - title: Actuation
     url: /wiki/actuation/
     children:
@@ -169,6 +173,8 @@ wiki:
         url: /wiki/state-estimation/visual-servoing/
       - title: Cartographer SLAM ROS Integration
         url: /wiki/state-estimation/Cartographer-ROS-Integration/
+      - title: Externally Referenced State Estimation for GPS Lacking Environments
+        url: /wiki/state-estimation/gps-lacking-state-estimation-sensors.md
   - title: Programming
     url:  /wiki/programming/
     children:

wiki/sensing/assets/decawave_example_multi_anchor.png

@@ -0,0 +1,78 @@
+---
+date: 2022-12-05
+title: Robotic Total Station (Leica TS16)
+---
+This page will go into detail about to get started with the TS16 Total Station, manufactured by Leica Geosystems. This information is likely helpful for other robotic total station integration, but is has not been tested.
+
+### How does it work?
+
+Total stations have an extended heritage in civil engineering, where they have been used to precisely survey worksites since the 1970s. The total station sends beams of light directly to a glass reflective prism, and uses the time-of-flight properties of the beam to measure distances. The robotic total station tracks it's calibration orientaiton to high precision, such that the measured distance can be converted into a high-precision 3D position mesaurement. Total stations, depending on the prism type and other factors, can accurate track with in millimeter range at up to 3.5km [Leica-Geosystems](file:///home/john/Downloads/Leica_Viva_TS16_DS-2.pdf).
+
+![Example usage of a Total Station in the Field](assets/leica_field_image.jpg)
+[Source](https://leica-geosystems.com/)
+
+### Best Use Cases & Expected Quality
+
+There are various companies that make robotic total stations including Leica and Trimble. While there is a wide variety of abilities between different models, there are certain key characteristics that matter when used as a real-time positioning system:
+
+- Range (How far does this need to work at?)
+- Angular tracking ability (Can the total station track objects moving closeby?)
+- Frequency (How often does it provide measurements?)
+
+Accuracy is the bottom line for robotic total stations. If the system is able to maintain line-of-sight with the total station, it is reasonable to expect <2cm accuracy in positioning, even over rather large distances (unknown exactly). This is especially valuable since it can provide an accurate positioning in XYZ, rather than just XY like many alternatives.
+
+### Limitations
+
+The key limitation of the total station is that it requires constant line-of-sight with the reflector prism it is tracking. Unlike other methods such as UltrawideBand or Ultrasonic positioing, recovery is not trivial.
+
+While certain companies like Caterpillar have official partnerships with total station manufacturers to allow for seemless integration with their vehicles [source](https://www.constructionequipment.com/technology/construction-technology-software/news/10757055/caterpillar-and-trimble-announce-change-to-joint-venture-providing-greater-flexibility-and-customer-focus), this technology is not currently available in such a robust manner to all. For that reason, "hacking" a total station to provide real-time sensory information made result in a unknowable latency with difficult to characterize dropout at times. While this did not become a detriment while the author was working on it, it's important to note. 
+
+Additionally, it's possible for the total station to lose it's track of the reflective prism, meaning that it ceases to provide valuable data. In a more robust system, there could be an automatic search process to return to a tracked state, but it is not explored in this wiki.
+
+## Setup
+
+In order to setup a total station tracking system, it's important to have all of the hardware:
+
+### Hardware
+
+This method has been tested on a TS16, but it would likely work on a TS15 or other Leica products. Most total stations use a [GEOCOM communication standard](https://www.naic.edu/~phil/hardware/theodolites/TPS1200_GeoCOM_Manual.pdf), which means that it's very likely that a similar serial interface could be adapted for most total station hardware. Leica hardware is particularly expensive, so a brand new total station with the best tracking specifications will run approximately [$30k](https://www.allenprecision.com/leica-viva-ts16p-series-robotic-packages).
+
+The other side of the total station is the reflector, which is used to get the high precision ranging. This is an important piece of hardware, especially in this use case. Since the prism will be moving, having high quality glass will reduce the imperfections that could cause the reflected beam to be off-center, resulting in a higher likelihood of losing tracking of the prism. A high quality prism from Leica will cost more than [$1k](https://www.allenprecision.com/360-reflector).
+
+![Leica GRZ122 360 Prism](assets/leica_360_prism.jpeg)
+[Source](https://leica-geosystems.com/)
+
+In order to connect the total station to a compute platform, it's possble to use wired or wireless methods. This wiki only explores the usage of USB connections using Leica's custom (and [expensive](https://www.baselineequipment.com/leica-instrument-to-datacollector-laptop-usb-cable)) cable.
+
+### Calibration
+
+In order to calibrate the total station, there are various ways to do it, all outlined in [various videos online](https://www.youtube.com/watch?v=ozbow6OgUlQ).
+
+The simplest way to calibrate the total station in a repeatable fashion is to use the "Orientate to Line" method. The context for the following instructions is to create fixed locations (fixtures) that allow a repeatably placed reflector location at both sites. By default, the line created by these reflectors would now become the X-axis of the local coordinate frame (which can be transformed into any other desired frame at a later point).
+
+1. Prepare reflector
+2. Attach reflector to fixture-1
+2. Change the Target settings of total station to `REFLECTOR_TYPE` (Settings -> TS Instrument -> Measure & Target)
+3. Go to Home -> Setup
+4. Choose the “Orientate to Line” method
+5. Measure two points that define an axis. 
+6. Use default settings (no changes required)
+7. The first measured point becomes (0,0)
+8. The second line determines the vector of the axis
+9. Derive setup height from “Manually enter”. Leave 0.0 as the value
+10. Place the reflector in Location A (see Figure 1) via S-hook attachment
+11. Ensure the reflector is being tracked via the top bar options (“Target tracking on”)
+12. Press “Measure” to measure the first location
+13. Move reflector to fixture-2
+14. Press “Measure” to measure the second location
+15. Press “set” to finish calibration
+
+At this point, any measurement that comes out of the total station will be with respect to the two measured points, which can be repeated for consistent testing and application.
+
+### Publicly Available ROS/ROS2 Nodes
+
+In order to setup the TS16 node into a ROS/ROS2 environment, follow the instructions in the repositories below:
+
+[ROS](https://github.com/arpit6232/Leica_Total_Station_ROS)
+
+[ROS2](https://github.com/John-HarringtonNZ/ts16_ros2_node)

wiki/sensing/assets/drtls_app.png

@@ -0,0 +1,73 @@
+---
+date: 2022-12-05
+title: DWM1001 UltraWideband Positioning System
+---
+This page will go into detail about to get started with UltraWideband positioning, particuarly with the [DWM1001-Dev module](https://www.qorvo.com/products/p/DWM1001-DEV).
+
+### How does it work?
+
+UltraWideband is a short-range wireless communication protocol, allowing connected devices to communicate across short ranges, similar to bluetooth. Unlike blueooth, UltraWideband transmits over a wider frequency, allowing for a more precise and wider bandwith of communication, albeit over a shorter range. 
+
+Ultrawideband positioning takes advantage of the communication pulses to sense distances between transmitters through a two-way ranging protocol described in detail [here](https://forum.qorvo.com/uploads/short-url/5yIaZ3A99NNf2uPHsUPjoBLr2Ua.pdf). As a simplification, it is able to measure the time period that messages take to transmit between devices, while accouting for potential clock and frequency shift between devices. 
+
+By using multiple stationary devices, a single or multiple mobile beacons can be tracked by combining ranges through trilateration. 
+
+![Example usage of a DWM1001 setup](assets/decawave_example_multi_anchor.png)
+[Source](https://www.researchgate.net/profile/Teijo-Lehtonen/publication/281346001/figure/fig4/AS:284460038803456@1444831966619/DecaWave-UWB-localization-system-SDK-5.png)
+
+### Best Use Cases & Expected Quality
+
+At the time of writing, one of the most common modules for UWB is the DWM1001. Information of how to get started with that can be seen [here](LINK TO WIKI PAGE ON DWM1001). Since these modules are mass manufactured, they can be purchased very inexpensively and should be considered one of the most affordable options for positioning systems.
+
+### Limitations
+
+UltraWideband positioning systems work best in wide open spaces, such as convention spaces or warehouses. Not only will direct line-of-sight interference causes issues for UWB systems, but also multi-pathing can cause [serious problems](https://ieeexplore.ieee.org/document/7790604).
+
+Even in the best scenario, it should be expected that a UWB positioning system will be able to get an accuracy of approximately (15-20cm) accuracy in XY, and even worse in Z (explained more in the calibration section).
+
+## Setup
+
+In order to setup a DWM1001-Dev UltraWideband positioning system, it's important to have enough DWM1001-Dev modules (`beacons`) for your use case. Each one of the `beacons` can either be configured as a `tag`, `anchor`, or `gateway`. `tag`s represent mobile `beacons` that will be on your robot, `anchors` represent stationary anchors, and `gateways` are modules that sole job are to pipe information over serial to your compute stack. While there are multiple ways to get information from these devices, this guide will describes how to access it over a direct USB serial connection. 
+
+![Architecture of a DWM1001-Dev setup with multiple gateways](assets/dwm_arch.png)
+
+Before any work can be done on the `beacons`, they must be flashed with the pre-built software provided by the manufacturer for this task. A detailed guide can be found [here](https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=&ved=2ahUKEwjNrN-T8OP7AhWTFVkFHRNTCzcQFnoECBMQAQ&url=https%3A%2F%2Fwww.qorvo.com%2Fproducts%2Fd%2Fda007972&usg=AOvVaw2va8gKJNC_mfXq5EQZAO0S)
+
+### Configuration & Calibration
+
+Once the beacons have been flashed, they need to be configured and calibrated. This can be one most easily using their provided Android app, [Decawave DRTLS APK](https://apkcombo.com/decawave-drtls-manager-r1/com.decawave.argomanager/). There is also information found [here](https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=&ved=2ahUKEwjNrN-T8OP7AhWTFVkFHRNTCzcQFnoECBMQAQ&url=https%3A%2F%2Fwww.qorvo.com%2Fproducts%2Fd%2Fda007972&usg=AOvVaw2va8gKJNC_mfXq5EQZAO0S) about performing this work directly through a command line, but it is less documented.
+
+![DRTLS App Screenshot](assets/drtls_app.png)
+
+In order to use the app, follow the instructions linked [here](https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=&ved=2ahUKEwi-id_G9-P7AhVIGVkFHVIPBZ0QFnoECA0QAQ&url=https%3A%2F%2Fwww.qorvo.com%2Fproducts%2Fd%2Fda007996&usg=AOvVaw09yRMbgwEx7hxTDpJP4G8D). 
+
+#### Instructions Addendums
+
+In the instructions, it shows how to either automatically calibrate the positions of the `anchors` as well as how to manually input their positions. For best results, manually measure the 3D positions of the `anchors` using a high-precision device, including either a MOCAP system, high-fidelity laser scanner (eg. FARO), or a total station. 
+
+It is possible to change the update rate of all `beacons`. For most real time positioning, it is reccomended to maximize this at 10Hz.
+
+During configuration, make note of the `beacon` IDS (eg. from `Tag DW11A1` the tag ID is `11A1`). This should be the ID used in the driver config to identify multiple tags independently.
+
+![DRTLS App - Beacon IDS](assets/drtls_ids.png)
+
+At this point, the DRTLS app can be used directly to test the 3D positioning. Ensure all modules are powered, and if ther are, enter the "map" page of the app to view all of the `beacons` live, including the dynamic positioning of the `tag`.
+
+#### Dilution of Precision
+
+In most cases, the `anchors` are located around a room, such that they are relatively planar with respect to each other. For 2D localization, this is perfectly acceptable, but it becomes an issue when attempting 3D localization. This is due to the concept of *[dilution of precision](https://en.wikipedia.org/wiki/Dilution_of_precision_(navigation))*, which originated from GPS. For GPS, it means that as GPS satellites attempting to provide a location to a ground side receiver get relatively close to each other, the output precision of the ground side position estimation becomes poorer, as seen below:
+
+![GDOP Example](assets/gdop_example.png)
+
+For UWB, this means that if the `anchors` are all close to the same Z plane, their Z localization precision will suffer. Ideally, all anchors should be placed at widely different heights to improve this. However, the beacons need to also be placed away from corners (such as the intersection of the floor/ceiling to the wall), leading to a difficulty.
+
+
+### Publicly Available ROS/ROS2 Nodes
+
+Now with a functioning module setup, including calibration, it is possible to link this up with a robotic system. Below are links to two existing repositories to read serial data into a ROS/ROS2 ecosystem.
+
+[ROS Node](https://github.com/TIERS/ros-dwm1001-uwb-localization)
+
+[ROS2 Node](https://github.com/John-HarringtonNZ/dwm1001_dev_ros2)
+
+If you are linking multiple UART devices to a single compute platform, it is likely that they will showup as different names by default depending on when they are plugged in, etc. To standardize this, you can assign a specific name based on the hardware ID to ensure consistent naming conventions. Follow [this](https://gist.github.com/edro15/1c6cd63894836ed982a7d88bef26e4af) guide for details.

wiki/sensing/assets/drtls_ids.png

@@ -0,0 +1,54 @@
+---
+# Jekyll 'Front Matter' goes here. Most are set by default, and should NOT be
+# overwritten except in special circumstances. 
+# You should set the date the article was last updated like this:
+date: 2022-12-05 # YYYY-MM-DD
+# This will be displayed at the bottom of the article
+# You should set the article's title:
+title: Externally Referenced State Estimation for GPS Lacking Environments
+# The 'title' is automatically displayed at the top of the page
+# and used in other parts of the site.
+---
+Robotic systems will often encounter environments where GPS access is either lacking or denied. In these environments, it's important to still maintain some accurate global representation of the robot's state, both for localization and mapping purposes. This is a relatively common problem in robotics, and is commonly dealt with through a combination of proprioceptive sensors and an estimation of the local environment in order to perform [SLAM](https://www.mathworks.com/discovery/slam.html). This is effective in environments where there are unique features to localize off of, but can struggle in uniform or dynamic environments. For that reason, there is benefit in considering sensor suites that take advantage of external infrastructure to provide a fixed positional reference. This fixed positional reference can mitigate drift or catastrophic localization error for robotic systems where signifacant state estimation errors are disallowed or not preferable. 
+
+
+## Common Types of External Referencing Positioning Systems
+
+### UltraWideband Beacons (UWB)
+
+UltraWideband is a short-range wireless communication protocol, allowing connected devices to communicate across short ranges, similar to bluetooth. Unlike blueooth, UltraWideband transmits over a wider frequency, allowing for a more precise and wider bandwith of communication, albeit over a shorter range. 
+
+Ultrawideband positioning takes advantage of the communication pulses to sense distances between transmitters through a two-way ranging protocol described in detail [here](https://forum.qorvo.com/uploads/short-url/5yIaZ3A99NNf2uPHsUPjoBLr2Ua.pdf). As a simplification, it is able to measure the time period that messages take to transmit between devices, while accouting for potential clock and frequency shift between devices. 
+
+By using multiple stationary devices, a single or multiple mobile beacons can be tracked by combining ranges through trilateration. 
+
+![Example usage of a DWM1001 setup](assets/decawave_example_multi_anchor.png)
+[Source](https://www.researchgate.net/profile/Teijo-Lehtonen/publication/281346001/figure/fig4/AS:284460038803456@1444831966619/DecaWave-UWB-localization-system-SDK-5.png)
+
+At the time of writing, one of the most common modules for UWB is the DWM1001. Since these modules are mass manufactured, they can be purchased very inexpensively and should be considered one of the most affordable options for positioning systems.
+
+### Ultrasonic Positioning
+
+Ultrasonic positioning works in a similar way to UWB, but rather than transitting frequencies at a very high frequency, the products instead rely on a combination of lower frquency communication pulses and beamforming. By using a sensor array on each device, they are able to claim a 2D positioning accuracy of +-2cm.
+
+
+![Example usage of a DWM1001 setup](assets/marvelmind_example.jpg)
+[Source](https://marvelmind.com/)
+
+As an important note, ultrasonic pulses are harmful to human hearing over an extended period of time and should not deployed around humans without ear protection.
+
+### Robotic Total Stations
+
+Total stations have an extended heritage in civil engineering, where they have been used to precisely survey worksites since the 1970s. The total station sends beams of light directly to a glass reflective prism, and uses the time-of-flight properties of the beam to measure distances. The robotic total station tracks it's calibration orientaiton to high precision, such that the measured distance can be converted into a high-precision 3D position mesaurement. Total stations, depending on the prism type and other factors, can accurate track with in millimeter range at up to 3.5km [Leica-Geosystems](file:///home/john/Downloads/Leica_Viva_TS16_DS-2.pdf).
+
+![Example usage of a Total Station in the Field](assets/leica_field_image.jpg)
+[Source](https://leica-geosystems.com/)
+
+## Key Factors to Consider
+
+When considering external referencing positioning systems, it's important to consider the following:
+
+- What is the environmental conditions? Is the robot running indoors or outdoors? interference)
+- Will there be humans in environment
+- Will the robot have line-of-sight to the fixed infrastructure?
+- What are the accuracy and precision requirements?