Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

TDoA3 hybrid mode #1330

Merged
merged 13 commits into from
Nov 29, 2023
3 changes: 3 additions & 0 deletions configs/all.config
Original file line number Diff line number Diff line change
Expand Up @@ -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
69 changes: 1 addition & 68 deletions docs/functional-areas/loco-positioning-system/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -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 %}
Original file line number Diff line number Diff line change
@@ -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.
121 changes: 121 additions & 0 deletions docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md
Original file line number Diff line number Diff line change
@@ -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 |
9 changes: 9 additions & 0 deletions src/deck/drivers/src/Kconfig
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading