Merge pull request #1330 from bitcraze/krichardsson/hybrid-mode

TDoA3 hybrid mode

configs/all.config

@@ -15,3 +15,6 @@
 # Some decks use UART1 and can not be enabled when debug printing is directed to UART1
 # Turn it off to verify the deck drivers.
 # CONFIG_DEBUG_PRINT_ON_UART1 is not set
+#
+# Do not build with hybrid mode as we run out of CCM memory in combination with some other configurations
+# CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE is not set

docs/functional-areas/loco-positioning-system/index.md

@@ -9,71 +9,4 @@ Most of the documentation for the Loco Positioning System can be found in the [l
 
 This section contains details specific to the implementation in the Crazyflie, mainly estimator related information.
 
-## Position estimation
-
-When using the Loco Positioning System the [Kalman estimator](/docs/functional-areas/sensor-to-control/state_estimators.md#extended-kalman-filter) is used for position estimation. The two main ranging schemes used for UWB localization are (i) two-way ranging (TWR) and (ii) time-difference-of-arrival (TDoA). In this page, we elaborate the measurement model, Kalman filter update and robust estimation for both TWR and TDoA.
-
-### Two-way ranging (TWR)
-
-In TWR, the UWB tag mounted on the Crazyflie communicates with fixed UWB anchors and acquires range measurements through two-way communication. The measurement model is as follows:
-
-$$d_i = \sqrt{(x-x_i)^2 +(y-y_i)^2 + (z-z_i)^2}$$,
-
-where $$(x, y, z)$$ is the position of the Crazyflie and $$(x_i, y_i, z_i)$$ is the position of the fixed anchor i. For the conventional extended Kalman filter, we have
-
-$$g_x = (x-x_i) / d_i$$
-
-$$g_y = (y-y_i) / d_i$$
-
-$$g_z = (z-z_i) / d_i$$.
-
-The H vector is
-
-$$H = (g_x, g_y, g_z, 0, 0, 0, 0, 0, 0)$$.
-
-Then, we call the function `kalmanCoreScalarUpdate()` in `kalman_core.c` to update the states and covariance matrix.
-
-### Time-difference-of-arrival (TDoA)
-In TDoA, UWB tags receive signals from anchors passively and compute the difference in distance beween two anchors as TDoA measurements. Since in TDoA scheme, UWB tags only listen to the messages from anchors, a TDoA-based localization system allows a theoretically unlimited number of robots to localize themselves with a small number of fixed anchors. However, TDoA measurements are more noisy than TWR measurements, leading to a less accurate localization performance. Two types of TDoA protocols ([TDoA2](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa2_protocol/) and [TDoA3](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa3_protocol/)) are implemented in LPS system. The main difference between the two TDoA protocols is that TDoA3 protocol achieves the scalability at the cost of localization accuracy. The readers are refer to [TDoA2 VS TDoA3](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/functional-areas/tdoa2-vs-tdoa3/) for detailed information.
-
-The TDoA measurement model is as follows:
-
-$$d_{ij} = d_j - d_i = \sqrt{(x-x_j)^2 +(y-y_j)^2 + (z-z_j)^2} - \sqrt{(x-x_i)^2 +(y-y_i)^2 + (z-z_i)^2}$$,
-
-where $$(x, y, z)$$ is the position of the Crazyflie and $$(x_i, y_i, z_i)$$ and $$(x_j, y_j, z_j)$$ are the positions of fixed anchor i and j, respectively. For the conventional extended Kalman filter, we have
-
-$$g_x = (x-x_j) / d_j - (x-x_i) / d_i$$
-
-$$g_y = (y-y_j) / d_j - (y-y_i) / d_i$$
-
-$$g_z = (z-z_j) / d_j - (z-z_i) / d_i$$.
-
-The H vector is
-
-$$H = (g_x, g_y, g_z, 0, 0, 0, 0, 0, 0)$$.
-
-Then, we call the function `kalmanCoreScalarUpdate()` in `kalman_core.c` to update the states and covariance matrix.
-
-### M-estimation based robust Kalman filter
-UWB radio signal suffers from outlier measurements caused by radio multi-path reflection and non-line-of-sight propagation. The large erroneous measurements often deteriorate the accuracy of UWB localization. The conventional Kalman filter is sensitive to measurement outliers due to its intrinsic minimum mean-square-error (MMSE) criterion. Here, we provide a robust estiamtion approach based on M-estimation robust cost function. We will explain the general idea of the robust Kalman filter and readers are encouraged to look into the firmware `mm_distance_robust.c` and `mm_tdoa_robust.c`. The implementation is based on paper [1] and please read the paper for implementation details.
-
-From the Bayesian maximum a posteriori perspective, the Kalman filter state estimation framework can be derived by solving the following minimization problem:
-
-![equation](/docs/images/rkf-eq1.png)
-
-Therein, $$x_k$$ and $$y_k$$ are the system state and measurements at timestep k, $$P_k$$ and $$R_k$$ denote the prior covariance and measurement covariance, respectively. Through Cholesky factorization of $$P_k$$ and $$R_k$    $, the original optimization problem is equivalent to:
-
-![equation](/docs/images/rkf-eq2.png)
-
-where $$e_{x,k,i}$$ and $$e_{y,k,i}$$ are the elements of $$e_{x,k}$$ and $$e_{y,k}$$. To reduce the influence of outliers, we incorporate a robust cost function into the Kalman filter framework as follows:
-
-![equation](/docs/images/rkf-eq3.png)
-
-where $$\rho()$$ could be any robust function (e.g., G-M, SC-DCS, Huber, Cauchy, etc.)
-
-By introducing a weight function for the process and measurement uncertainties---with e as input---we can translate the optimization problem into an Iterative Reweight Least-Square (IRLS) problem. Then, the optimal posterior estimate can be computed through iteratively solving the least-square problem using the robust weights computed from the previous solution. In our implementation, we use the G-M robust cost function and the maximum iteration is set to be two for computational frugality. Then, we call the function `kalmanCoreUpdateWithPKE()` in `kalman_core.c` with the weighted covariance matrix $$P_{w_m}$$, kalman gain Km, and innovation error to update the states and covariance matrix.
-
-This functionality can be turned on through setting a parameter (kalman.robustTwr or kalman.robustTdoa).
-
-## References
-[1] Zhao, Wenda, Jacopo Panerati, and Angela P. Schoellig. "Learning-based Bias Correction for Time Difference of Arrival Ultra-wideband Localization of Resource-constrained Mobile Robots." IEEE Robotics and Automation Letters 6, no. 2 (2021): 3639-3646.
+{% sub_page_menu %}

docs/functional-areas/loco-positioning-system/kalman_measurement_models.md

@@ -0,0 +1,71 @@
+---
+title: Loco kalman measurment model
+page_id: loco_measurement_models
+---
+
+When using the Loco Positioning System the [Kalman estimator](/docs/functional-areas/sensor-to-control/state_estimators.md#extended-kalman-filter) is used for position estimation. The two main ranging schemes used for UWB localization are (i) two-way ranging (TWR) and (ii) time-difference-of-arrival (TDoA). In this page, we elaborate the measurement model, Kalman filter update and robust estimation for both TWR and TDoA.
+
+### Two-way ranging (TWR)
+
+In TWR, the UWB tag mounted on the Crazyflie communicates with fixed UWB anchors and acquires range measurements through two-way communication. The measurement model is as follows:
+
+$$d_i = \sqrt{(x-x_i)^2 +(y-y_i)^2 + (z-z_i)^2}$$,
+
+where $$(x, y, z)$$ is the position of the Crazyflie and $$(x_i, y_i, z_i)$$ is the position of the fixed anchor i. For the conventional extended Kalman filter, we have
+
+$$g_x = (x-x_i) / d_i$$
+
+$$g_y = (y-y_i) / d_i$$
+
+$$g_z = (z-z_i) / d_i$$.
+
+The H vector is
+
+$$H = (g_x, g_y, g_z, 0, 0, 0, 0, 0, 0)$$.
+
+Then, we call the function `kalmanCoreScalarUpdate()` in `kalman_core.c` to update the states and covariance matrix.
+
+### Time-difference-of-arrival (TDoA)
+In TDoA, UWB tags receive signals from anchors passively and compute the difference in distance beween two anchors as TDoA measurements. Since in TDoA scheme, UWB tags only listen to the messages from anchors, a TDoA-based localization system allows a theoretically unlimited number of robots to localize themselves with a small number of fixed anchors. However, TDoA measurements are more noisy than TWR measurements, leading to a less accurate localization performance. Two types of TDoA protocols ([TDoA2](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa2_protocol/) and [TDoA3](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa3_protocol/)) are implemented in LPS system. The main difference between the two TDoA protocols is that TDoA3 protocol achieves the scalability at the cost of localization accuracy. The readers are refer to [TDoA2 VS TDoA3](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/functional-areas/tdoa2-vs-tdoa3/) for detailed information.
+
+The TDoA measurement model is as follows:
+
+$$d_{ij} = d_j - d_i = \sqrt{(x-x_j)^2 +(y-y_j)^2 + (z-z_j)^2} - \sqrt{(x-x_i)^2 +(y-y_i)^2 + (z-z_i)^2}$$,
+
+where $$(x, y, z)$$ is the position of the Crazyflie and $$(x_i, y_i, z_i)$$ and $$(x_j, y_j, z_j)$$ are the positions of fixed anchor i and j, respectively. For the conventional extended Kalman filter, we have
+
+$$g_x = (x-x_j) / d_j - (x-x_i) / d_i$$
+
+$$g_y = (y-y_j) / d_j - (y-y_i) / d_i$$
+
+$$g_z = (z-z_j) / d_j - (z-z_i) / d_i$$.
+
+The H vector is
+
+$$H = (g_x, g_y, g_z, 0, 0, 0, 0, 0, 0)$$.
+
+Then, we call the function `kalmanCoreScalarUpdate()` in `kalman_core.c` to update the states and covariance matrix.
+
+### M-estimation based robust Kalman filter
+UWB radio signal suffers from outlier measurements caused by radio multi-path reflection and non-line-of-sight propagation. The large erroneous measurements often deteriorate the accuracy of UWB localization. The conventional Kalman filter is sensitive to measurement outliers due to its intrinsic minimum mean-square-error (MMSE) criterion. Here, we provide a robust estiamtion approach based on M-estimation robust cost function. We will explain the general idea of the robust Kalman filter and readers are encouraged to look into the firmware `mm_distance_robust.c` and `mm_tdoa_robust.c`. The implementation is based on paper [1] and please read the paper for implementation details.
+
+From the Bayesian maximum a posteriori perspective, the Kalman filter state estimation framework can be derived by solving the following minimization problem:
+
+![equation](/docs/images/rkf-eq1.png)
+
+Therein, $$x_k$$ and $$y_k$$ are the system state and measurements at timestep k, $$P_k$$ and $$R_k$$ denote the prior covariance and measurement covariance, respectively. Through Cholesky factorization of $$P_k$$ and $$R_k$    $, the original optimization problem is equivalent to:
+
+![equation](/docs/images/rkf-eq2.png)
+
+where $$e_{x,k,i}$$ and $$e_{y,k,i}$$ are the elements of $$e_{x,k}$$ and $$e_{y,k}$$. To reduce the influence of outliers, we incorporate a robust cost function into the Kalman filter framework as follows:
+
+![equation](/docs/images/rkf-eq3.png)
+
+where $$\rho()$$ could be any robust function (e.g., G-M, SC-DCS, Huber, Cauchy, etc.)
+
+By introducing a weight function for the process and measurement uncertainties---with e as input---we can translate the optimization problem into an Iterative Reweight Least-Square (IRLS) problem. Then, the optimal posterior estimate can be computed through iteratively solving the least-square problem using the robust weights computed from the previous solution. In our implementation, we use the G-M robust cost function and the maximum iteration is set to be two for computational frugality. Then, we call the function `kalmanCoreUpdateWithPKE()` in `kalman_core.c` with the weighted covariance matrix $$P_{w_m}$$, kalman gain Km, and innovation error to update the states and covariance matrix.
+
+This functionality can be turned on through setting a parameter (kalman.robustTwr or kalman.robustTdoa).
+
+## References
+[1] Zhao, Wenda, Jacopo Panerati, and Angela P. Schoellig. "Learning-based Bias Correction for Time Difference of Arrival Ultra-wideband Localization of Resource-constrained Mobile Robots." IEEE Robotics and Automation Letters 6, no. 2 (2021): 3639-3646.

docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md

@@ -0,0 +1,121 @@
+---
+title: Loco TDoA3 hybrid mode
+page_id: loco_tdoa3_hybrid_mode
+---
+
+TDoA3 Hybrid mode is an experimental extension to the standard TDoA3 positioning mode. In standard TDoA3 the Crazyflies
+are passively listening to the UWB traffic between anchors for positioning, while the hybrid mode also enables the
+Crayzflies to transmit UWB packets. This makes it possible to use TWR for positioning the Crazyflie and use a Crazyflie
+as an anchor.
+
+## Functionality
+
+In a TDoA3 system the anchors are doing TWR with each other and the TDoA positioning that is used by the Crazyflies
+is almost a "side effect". For positioning to work the anchors are also including their own position in the transmitted
+packets, see [the TDoA3 protocol](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa3_protocol/).
+
+The TDoa3 implementation is designed to be dynamic in terms of availability of anchors and if an anchor is added or
+removed the system will adopt to the new environment, similarly if a Crazyflie passes through a large system it will
+adopt to the availability of anchors that are reachable in the current location. As long as each anchor uses a unique
+id all parts of the system will be able to handle dynamic situations.
+This property makes it possible to convert a passively listening Crazyflie to an actively transmitting node, the other
+members of the network will simply see it as another anchor and respond accordingly.
+
+## Use cases
+
+The most basic use case is to let a Crazyflie temporarily do TWR to measure the absolute distance to anchors instead of
+the relative distance measurements used in TDoA. TWR is more stable than TDoA when flying outside the convex hull of the
+anchors and it enables better positioning.
+
+If two Crazyflies turns on TWR it is possible to measure the distance between the two Crazyflies which could be used
+for collision avoidance or relative positioning for instance.
+
+Note that it is possible to use TWR and TDoA at the same time for estimating the position, the Crazyflie receives all
+packets and will use them for TDoA and/or TWR depending on if the remote party has received a packet from the Crazyflie
+that can be used for TWR.
+
+If a Crazyflie includes its position in the transmitted packets, other Crazyflies can also use the UWB traffic for TDoA
+or TWR positioning, just like another anchor. This makes it possible for a Crazyflie to fly to a new position, land and act
+as an anchor for other Crazyflies to use for positioning and thus extend the reach of the positioning system dynamically.
+There is an option to samples the current estimated position when the Crazyflie starts to transmit its position, the
+sampled position will be used in all transmissions to avoid a dynamically changing position.
+
+It is also possible to use the currently estimated position in the transmissions to use a Crazyflie as moving anchor.
+It might work for one moving anchor, but most likely the system will be instable if many Crazyflies do this at the
+same time without extended error handling as they will use each others position estimates (including errors) to
+estimate their own position. An extended error handling is not included in the implementation but hopefully it can be
+used as a base if someone is interested in experimenting in this area.
+
+## Implementation
+
+The hybrid mode is mainly implemented in the `lpsTdoa3Tag.c` file and is normally not compiled into the firmware, to
+enable it use the `CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE` kbuild flag (`Expansion deck configuration/Support TDoA3 hybrid mode`
+in `menuconfig`).
+
+There are no changes or extensions to the UWB protocol and the standard TDoA3 protocol is used. There are also no special
+handling in relation to position estimation, the same measurement models in the kalman estimator are used as in standard
+TDoA and TWR.
+
+## Params and logs
+
+There are a few parameters that must be configured to use the hybrid mode functionality, they are all part of the
+`tdoa3` parameter group.
+
+| Parameter     | Functionality                                                                                        |
+|---------------|------------------------------------------------------------------------------------------------------|
+| `hmId`        | The anchor id used in transmissions, this id must be unique in the system. Don't use 255 as it means "broadcast" and is received by all nodes |
+| `hmTdoa`      | Indicates if received packets should be used for TDoA positioning                                    |
+| `hmTwr`       | Indicates if TWR packets should be transmitted                                                       |
+| `hmTwrTXPos`  | Indicates if a position should be included in transmitted packets, that is enable other Crazyflies to use the transmissions for TDoA positioning |
+| `hmTwrEstPos` | Indicates if received packets should be used for TWR positioning, requires hmTwr to be set           |
+| `twrStd`      | The measurement noise to use when sending TWR measurements to the estimator                          |
+| `hmAnchLog`   | Select an anchor id to use for logging distance to a specific anchor. The distance is available in the `hmDist` log variable |
+
+Similarly all logs are in the `tdoa3` log group.
+
+| Log       | Functionality                                                                      |
+|-----------|------------------------------------------------------------------------------------|
+| `hmDist`  | Measured distance to the anchor selected by the `hmAnchLog`` parameter [m]         |
+| `hmTx`    | Transmission rate of TWR packets in hybrid mode [packets/s]                        |
+| `hmSeqOk` | Rate of received TWR packets with matching seq nr in hybrid mode [packets/s]       |
+| `hmEst`   | Rate of TWR measurements that are sent to the estimator in hybrid mode [samples/s] |
+
+## Example configurations
+
+### Use TDoA and TWR for positioning
+
+| Parameter     | Value | Comment                                                    |
+|---------------|-------|------------------------------------------------------------|
+| `hmId`        | X     | Unique ids for each Crazyflie, it is set to 254 by default |
+| `hmTwr`       | 1     | to start transmission                                      |
+| `hmTwrEstPos` | 1     | to use TWR in position estimation                          |
+
+### Measure distance between two Crazyflies
+
+Settings for Crazyflie 1
+
+| Parameter     | Value | Comment                |
+|---------------|--------------------------------|
+| `hmId`        | 254   | the anchor id to use   |
+| `hmTwr`       | 1     | to start transmission  |
+| `hmAnchLog`   | 253   | the id of the other CF |
+
+Settings for Crazyflie 2
+
+| Parameter     | Value | Comment                |
+|---------------|--------------------------------|
+| `hmId`        | 253   | the anchor id to use   |
+| `hmTwr`       | 1     | to start transmission  |
+| `hmAnchLog`   | 254   | the id of the other CF |
+
+The distance to the other Crazyflie is available in the `tdoa3.hmDist` log variable.
+
+### Use the Crazyflie as a new anchor
+
+Assuming the Crazyflie is landed in a position where the estimated position is OK
+
+| Parameter     | Value                                                                  |
+|---------------|------------------------------------------------------------------------|
+| `hmId`        | Unique anchor id                                                       |
+| `hmTwr`       | 1 to start transmission                                                |
+| `hmTwrTXPos`  | 2 to sample the current estimated position and use it in transmissions |

src/deck/drivers/src/Kconfig

@@ -337,6 +337,15 @@ config DECK_LOCO_LONGER_RANGE
     help
         Note that anchors need to be built with support for this as well
 
+config DECK_LOCO_TDOA3_HYBRID_MODE
+    bool "Support TDoA3 hybrid mode"
+    depends on DECK_LOCO
+    default n
+    help
+        In the standard TDoA3 mode anchors are transmitting while the Crazyflies only receive. In hybrid mode the
+        Crazyflies can also transmit and thus do TWR, either for positioning them selfs or to act as an anchor for
+        other Crazyflies.
+
 config DECK_LOCO_TDMA
     bool "Use Time Division Multiple Access"
     depends on DECK_LOCO_ALGORITHM_TWR