diff --git a/configs/all.config b/configs/all.config
index 63bccc37d3..abfe7fcf74 100644
--- a/configs/all.config
+++ b/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
diff --git a/docs/functional-areas/loco-positioning-system/index.md b/docs/functional-areas/loco-positioning-system/index.md
index f7ffdeeaea..68c11d78fd 100644
--- a/docs/functional-areas/loco-positioning-system/index.md
+++ b/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 %}
diff --git a/docs/functional-areas/loco-positioning-system/kalman_measurement_models.md b/docs/functional-areas/loco-positioning-system/kalman_measurement_models.md
new file mode 100644
index 0000000000..df81af9668
--- /dev/null
+++ b/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.
diff --git a/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md b/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md
new file mode 100644
index 0000000000..0fc533e3a7
--- /dev/null
+++ b/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 |
diff --git a/src/deck/drivers/src/Kconfig b/src/deck/drivers/src/Kconfig
index 7a40ee3565..1db2aa01b4 100644
--- a/src/deck/drivers/src/Kconfig
+++ b/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
diff --git a/src/deck/drivers/src/lpsTdoa2Tag.c b/src/deck/drivers/src/lpsTdoa2Tag.c
index 4f914b61c5..8f739413c3 100644
--- a/src/deck/drivers/src/lpsTdoa2Tag.c
+++ b/src/deck/drivers/src/lpsTdoa2Tag.c
@@ -116,7 +116,7 @@ static void updateRemoteData(tdoaAnchorContext_t* anchorCtx, const rangePacket2_
       if (hasDistance) {
         int64_t tof = packet->distances[i];
         if (isValidTimeStamp(tof)) {
-          tdoaStorageSetTimeOfFlight(anchorCtx, remoteId, tof);
+          tdoaStorageSetRemoteTimeOfFlight(anchorCtx, remoteId, tof);
 
           if (isConsecutiveIds(previousAnchor, anchorId)) {
             logAnchorDistance[anchorId] = packet->distances[previousAnchor];
diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 18e488a1de..64e865069e 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -7,7 +7,7 @@
  *
  * Crazyflie firmware.
  *
- * Copyright 2018, Bitcraze AB
+ * Copyright 2018-2023, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU Lesser General Public License as published by
@@ -40,6 +40,14 @@ The implementation must handle
 3. Dynamically changing visibility of anchors over time
 4. Random TX times from anchors with possible packet collisions and packet loss
 
+
+Hybrid Mode
+
+In Hybrid Mode, the tag is not only passively listening for packets, but is
+also transmitting, which enables Two Way Ranging with peers in the network.
+The default behaviour is to send the acquired distance data to the estimator
+for improved position estimation.
+
 */
 
 #include <string.h>
@@ -57,6 +65,12 @@ The implementation must handle
 #include "mac.h"
 
 #include "param.h"
+#include "autoconf.h"
+#include "cf_math.h"
+
+#include "physicalConstants.h"
+#include "statsCnt.h"
+#include "log.h"
 
 #define DEBUG_MODULE "TDOA3"
 #include "debug.h"
@@ -68,7 +82,48 @@ The implementation must handle
 
 #define PACKET_TYPE_TDOA3 0x30
 
-#define TDOA3_RECEIVE_TIMEOUT 10000
+#define TDOA3_RECEIVE_TIMEOUT 50000
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+// The delay required for the radio to be ready to transmit
+#define TX_DELAY_TIME_S ( 500e-6 )
+#define TX_DELAY_TIME (uint64_t)( TX_DELAY_TIME_S * LOCODECK_TS_FREQ )
+
+#define LPP_HEADER 0
+#define LPP_TYPE (LPP_HEADER + 1)
+#define LPP_PAYLOAD (LPP_HEADER + 2)
+
+#define MAX_NR_OF_ANCHORS_IN_TX 8
+
+#define STATS_INTERVAL 500
+
+#define SYSTEM_TX_FREQ 400.0f
+#define ANCHOR_MAX_TX_FREQ 50.0f
+// We need a lower limit of minimum tx rate. The TX timestamp in the protocol is
+// only 32 bits (equal to 67 ms) and we want to avoid double wraps of the TX counter.
+// To have some margin set the lowest tx frequency to 20 Hz (= 50 ms)
+#define ANCHOR_MIN_TX_FREQ 20.0f
+#define ID_COUNT 256
+
+// Short LPP packets for position
+#define SHORT_LPP 0xF0
+#define LPP_SHORT_ANCHOR_POSITION 0x01
+
+// Log ids for estimated position
+static logVarId_t estimatedPosLogX;
+static logVarId_t estimatedPosLogY;
+static logVarId_t estimatedPosLogZ;
+
+static const locoAddress_t base_address = 0xcfbc;
+
+static void processTwoWayRanging(tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T);
+
+typedef enum {
+  txOwnPosNot = 0,
+  txOwnPosEstimated,
+  txOwnPosLocked
+} TxOwnPosition;
+#endif
 
 typedef struct {
   uint8_t type;
@@ -95,12 +150,57 @@ typedef struct {
   uint8_t remoteAnchorData;
 } __attribute__((packed)) rangePacket3_t;
 
+static struct {
+  float tdoaStdDev;
+  bool isReceivingPackets;
+
+  // Outgoing LPP packet
+  lpsLppShortPacket_t lppPacket;
+
+  bool isTdoaActive;
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
 
-// Outgoing LPP packet
-static lpsLppShortPacket_t lppPacket;
+  // Hybrid mode transmission information
+  int anchorId;
+  uint8_t txSeqNr;
+  uint32_t txT_in_cl_T; // In UWB clock ticks
+  uint32_t latestTransmissionTime_ms; // System clock
+  uint32_t nextTxTick;  // in system clock ticks
 
-static bool rangingOk;
-static float stdDev = TDOA_ENGINE_MEASUREMENT_NOISE_STD;
+  uint32_t averageTxDelay;
+  uint32_t nextTxDelayEvaluationTime_ms;
+  uint32_t rxCount[ID_COUNT];
+
+  // Transmit packets if true
+  bool isTwrActive;
+
+  // Indicates if position information should be transmitted when sending packets.
+  // If no position information is transmitted, the CF can use TWR for positioning itself only while other CFs can not
+  // use the transmitted packets for positioning.
+  // If position is transmitted other CFs can use the packets for positioning.
+  TxOwnPosition twrSendPosition;
+  struct lppShortAnchorPos_s twrTxPos;
+
+  // Std dev for TWR samples sent to the estimator
+  float twrStdDev;
+
+  // If TWR data should be used for position estimation
+  bool useTwrForPositionEstimation;
+
+  // Maximum acceptable age of time of flight information to be used in transmissions. The ToF information is used by
+  // other CFs for TDoA positioning. A longer time is acceptable if the transmitting CF is not moving.
+  uint32_t maxAgeOfTof_ms;
+
+  statsCntRateLogger_t cntPacketsTransmitted;
+  statsCntRateLogger_t cntTwrSeqNrOk;
+  statsCntRateLogger_t cntTwrToEstimator;
+
+  // Logging of hybrid mode distances
+  uint8_t logDistAnchorId;
+  float logDistance;
+#endif
+} ctx;
 
 static bool isValidTimeStamp(const int64_t anchorRxTime) {
   return anchorRxTime != 0;
@@ -124,7 +224,7 @@ static int updateRemoteData(tdoaAnchorContext_t* anchorCtx, const void* payload)
     if (hasDistance) {
       int64_t tof = anchorData->distance;
       if (isValidTimeStamp(tof)) {
-        tdoaStorageSetTimeOfFlight(anchorCtx, remoteId, tof);
+        tdoaStorageSetRemoteTimeOfFlight(anchorCtx, remoteId, tof);
 
         uint8_t anchorId = tdoaStorageGetId(anchorCtx);
         tdoaStats_t* stats = &tdoaEngineState.stats;
@@ -176,25 +276,46 @@ static void rxcallback(dwDevice_t *dev) {
   dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
   const uint8_t anchorId = rxPacket.sourceAddress & 0xff;
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+  ctx.rxCount[anchorId]++;
+#endif
+
   dwTime_t arrival = {.full = 0};
   dwGetReceiveTimestamp(dev, &arrival);
   const int64_t rxAn_by_T_in_cl_T = arrival.full;
 
   const rangePacket3_t* packet = (rangePacket3_t*)rxPacket.payload;
   if (packet->header.type == PACKET_TYPE_TDOA3) {
-    const int64_t txAn_in_cl_An = packet->header.txTimeStamp;;
-    const uint8_t seqNr = packet->header.seq & 0x7f;;
+    const int64_t txAn_in_cl_An = packet->header.txTimeStamp;
+    const uint8_t seqNr = packet->header.seq & 0x7f;
 
-    tdoaAnchorContext_t anchorCtx;
     uint32_t now_ms = T2M(xTaskGetTickCount());
-
+    tdoaAnchorContext_t anchorCtx;
     tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchorId, now_ms, &anchorCtx);
     int rangeDataLength = updateRemoteData(&anchorCtx, packet);
-    tdoaEngineProcessPacket(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+    const bool doExcludeId = ctx.isTwrActive;
+    const uint8_t excludedId = ctx.anchorId;
+    const bool timeIsGood = tdoaEngineProcessPacketFiltered(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, doExcludeId, excludedId);
+#else
+    const bool doExcludeId = false;
+    const uint8_t excludedId = 0;
+    tdoaEngineProcessPacketFiltered(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, doExcludeId, excludedId);
+#endif
+
     tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
     handleLppPacket(dataLength, rangeDataLength, &rxPacket, &anchorCtx);
 
-    rangingOk = true;
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+    if (ctx.isTwrActive) {
+      if (timeIsGood) {
+        processTwoWayRanging(&anchorCtx, now_ms, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+      }
+    }
+#endif
+
+    ctx.isReceivingPackets = true;
   }
 }
 
@@ -204,8 +325,212 @@ static void setRadioInReceiveMode(dwDevice_t *dev) {
   dwStartReceive(dev);
 }
 
-static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+
+static void processTwoWayRanging(tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T) {
+  // We assume updateRemoteData() has been called before this function
+  // and that the remote data from the current packet is in the storage, as we read data from the storage.
+
+  const uint32_t MAX_TIME_SINCE_TRANSMISSION_MS = 100;
+  const bool isLatestTransmisionTimeClose = ((now_ms - ctx.latestTransmissionTime_ms) < MAX_TIME_SINCE_TRANSMISSION_MS);
+  if (isLatestTransmisionTimeClose) {
+    uint8_t latestSeqReceivedByRemote = 255;
+    int64_t rxT_by_An_in_cl_An = 0;
+    if (tdoaStorageGetRemoteRxTimeSeqNr(anchorCtx, ctx.anchorId, &rxT_by_An_in_cl_An, &latestSeqReceivedByRemote)) {
+      STATS_CNT_RATE_EVENT(&ctx.cntTwrSeqNrOk);
+      const bool isLatestTxPacketReturnedFromRemote = (ctx.txSeqNr == latestSeqReceivedByRemote);
+      if (isLatestTxPacketReturnedFromRemote) {
+        const double clockCorrection = tdoaStorageGetClockCorrection(anchorCtx);
+        int64_t t_in_anchor_T = (int64_t)(clockCorrection * tdoaEngineTruncateToAnchorTimeStamp(txAn_in_cl_An - rxT_by_An_in_cl_An));
+        int64_t t_since_tx_T = tdoaEngineTruncateToAnchorTimeStamp(rxAn_by_T_in_cl_T - ctx.txT_in_cl_T);
+        int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
+
+        float distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ;
+        tdoaStorageSetTimeOfFlight(anchorCtx, tof_T, now_ms);
+
+        if (tdoaStorageGetId(anchorCtx) == ctx.logDistAnchorId) {
+          ctx.logDistance = distance;
+        }
+
+        point_t position;
+        if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
+          distanceMeasurement_t measurement = {
+            .distance = distance,
+            .stdDev = ctx.twrStdDev,
+            .x = position.x,
+            .y = position.y,
+            .z = position.z,
+          };
+
+          if (ctx.useTwrForPositionEstimation) {
+            estimatorEnqueueDistance(&measurement);
+            STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
+          }
+        }
+      }
+    }
+  }
+}
+
+static int countSeenAnchorsAndClearCounters() {
+  int anchorsCount = 0;
+
+  for (int i = 0; i < ID_COUNT; i++) {
+    if (ctx.rxCount[i] != 0) {
+      anchorsCount++;
+      ctx.rxCount[i] = 0;
+    }
+  }
+
+  return anchorsCount;
+}
+
+static uint32_t updateAverageTxDelay(const uint32_t now_ms) {
+  if (now_ms > ctx.nextTxDelayEvaluationTime_ms) {
+    const uint32_t evalutationPeriod_ms = 500;
+    int anchorsCount = countSeenAnchorsAndClearCounters();
+
+    // Set the TX rate based on the number of transmitting anchors around us
+    // Aim for 400 messages/s. Up to 8 anchors: 50 Hz / anchor
+    float freq = SYSTEM_TX_FREQ / (anchorsCount + 1);
+    if (freq > ANCHOR_MAX_TX_FREQ) {
+      freq = ANCHOR_MAX_TX_FREQ;
+    }
+    if (freq < ANCHOR_MIN_TX_FREQ) {
+      freq = ANCHOR_MIN_TX_FREQ;
+    }
+    ctx.averageTxDelay = 1000.0f / freq;
+
+    ctx.nextTxDelayEvaluationTime_ms = now_ms + evalutationPeriod_ms;
+  }
+
+  return ctx.averageTxDelay;
+}
+
+static uint32_t randomizeDelayToNextTx(const uint32_t now_ms) {
+  const uint32_t interval = 10;
+
+  uint32_t averageTxDelay = updateAverageTxDelay(now_ms);
+
+  uint32_t r = rand();
+  uint32_t delay = averageTxDelay + r % interval - interval / 2;
+
+  return delay;
+}
+
+static dwTime_t findTransmitTimeAsSoonAsPossible(dwDevice_t *dev) {
+  dwTime_t transmitTime = { .full = 0 };
+  dwGetSystemTimestamp(dev, &transmitTime);
+
+  // Add some extra time to make sure the radio is ready to transmit, taking into account that this task may
+  // be halted for a while. This time also includes time for the preamble (128 * 1017.63e-9 s).
+  transmitTime.full += TX_DELAY_TIME;
+  return transmitTime;
+}
+
+static int populateTxData(rangePacket3_t *rangePacket, const uint32_t now_ms) {
+  // rangePacket->header.type already populated
+  rangePacket->header.seq = ctx.txSeqNr;
+  rangePacket->header.txTimeStamp = ctx.txT_in_cl_T;
+
+  uint8_t remoteAnchorCount = 0;
+  uint8_t* anchorDataPtr = &rangePacket->remoteAnchorData;
+
+  // Consider a more clever selection of which anchors to include as remote data.
+  // This implementation will give a somewhat randomized set but can probably be improved
+  uint8_t ids[MAX_NR_OF_ANCHORS_IN_TX];
+  uint8_t anchorCount = tdoaStorageGetListOfActiveAnchorIds(tdoaEngineState.anchorInfoArray, ids, MAX_NR_OF_ANCHORS_IN_TX, now_ms);
+
+  for (uint8_t i = 0; i < anchorCount; i++) {
+    remoteAnchorDataFull_t* anchorData = (remoteAnchorDataFull_t*) anchorDataPtr;
+
+    uint32_t now_ms = T2M(xTaskGetTickCount());
+
+    uint8_t id = ids[i];
+    tdoaAnchorContext_t anchorCtx;
+    tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, id, now_ms, &anchorCtx);
+
+    anchorData->id = id;
+    anchorData->seq = tdoaStorageGetSeqNr(&anchorCtx);
+    anchorData->rxTimeStamp = tdoaStorageGetRxTime(&anchorCtx);
+
+    uint64_t tof = tdoaStorageGetTimeOfFlight(&anchorCtx, now_ms - ctx.maxAgeOfTof_ms);
+    if (tof > 0 && tof <= 0xfffful) {
+      anchorData->distance = tof;
+      anchorDataPtr += sizeof(remoteAnchorDataFull_t);
+      anchorData->seq |= 0x80;  // Set bit to indicate tof is included
+    } else {
+      anchorDataPtr += sizeof(remoteAnchorDataShort_t);
+    }
+
+    remoteAnchorCount++;
+  }
+  rangePacket->header.remoteCount = remoteAnchorCount;
+
+  return (uint8_t*)anchorDataPtr - (uint8_t*)rangePacket;
+}
+
+// Set TX data in the radio TX buffer
+static void setTxData(dwDevice_t *dev, const uint32_t now_ms)
 {
+  static packet_t txPacket;
+  static bool firstEntry = true;
+  static int lppLength = 0;
+
+  if (firstEntry) {
+    MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+    txPacket.destAddress = (base_address & 0xffffffffffffff00) | 0xff;
+    txPacket.payload[0] = PACKET_TYPE_TDOA3;
+
+    firstEntry = false;
+  }
+
+  txPacket.sourceAddress = (base_address & 0xffffffffffffff00) | ctx.anchorId;
+
+  int rangePacketSize = populateTxData((rangePacket3_t *)txPacket.payload, now_ms);
+
+  if (ctx.twrSendPosition != txOwnPosLocked) {
+      // Store current estimated position
+      ctx.twrTxPos.x = logGetFloat(estimatedPosLogX);
+      ctx.twrTxPos.y = logGetFloat(estimatedPosLogY);
+      ctx.twrTxPos.z = logGetFloat(estimatedPosLogZ);
+  }
+
+  if (ctx.twrSendPosition != txOwnPosNot) {
+    txPacket.payload[rangePacketSize + LPP_HEADER] = SHORT_LPP;
+    txPacket.payload[rangePacketSize + LPP_TYPE] = LPP_SHORT_ANCHOR_POSITION;
+
+    struct lppShortAnchorPos_s *pos = (struct lppShortAnchorPos_s*) &txPacket.payload[rangePacketSize + LPP_PAYLOAD];
+    *pos = ctx.twrTxPos;
+
+    lppLength = 2 + sizeof(struct lppShortAnchorPos_s);
+  } else {
+    lppLength = 0;
+  }
+
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH + rangePacketSize + lppLength);
+}
+
+
+// Setup the radio to send a ranging packet
+static void setupTx(dwDevice_t *dev, const uint32_t now_ms) {
+  dwTime_t txTime = findTransmitTimeAsSoonAsPossible(dev);
+  ctx.txT_in_cl_T = txTime.low32;
+  ctx.txSeqNr = (ctx.txSeqNr + 1) & 0x7f;
+
+  dwIdle(dev);
+
+  setTxData(dev, now_ms);
+
+  dwNewTransmit(dev);
+  dwSetDefaults(dev);
+  dwSetTxRxTime(dev, txTime);
+
+  dwStartTransmit(dev);
+}
+#endif
+
+static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet) {
   static packet_t txPacket;
   dwIdle(dev);
 
@@ -226,15 +551,46 @@ static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
 }
 
 static bool sendLpp(dwDevice_t *dev) {
-  bool lppPacketToSend = lpsGetLppShort(&lppPacket);
+  bool lppPacketToSend = lpsGetLppShort(&ctx.lppPacket);
   if (lppPacketToSend) {
-    sendLppShort(dev, &lppPacket);
+    sendLppShort(dev, &ctx.lppPacket);
     return true;
   }
 
   return false;
 }
 
+static uint32_t startNextEvent(dwDevice_t *dev, const uint32_t now) {
+  uint32_t timeout = 500;
+
+  bool isTxPending = sendLpp(dev);
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+  if (ctx.isTwrActive) {
+    if (!isTxPending) {
+      if (ctx.nextTxTick < now) {
+        const uint32_t now_ms = T2M(now);
+        setupTx(dev, now_ms);
+        isTxPending = true;
+        ctx.latestTransmissionTime_ms = now_ms;
+        STATS_CNT_RATE_EVENT(&ctx.cntPacketsTransmitted);
+
+        uint32_t newDelay = randomizeDelayToNextTx(now_ms);
+        ctx.nextTxTick = now + newDelay;
+      }
+    }
+
+    timeout = ctx.nextTxTick - now;
+  }
+#endif
+
+  if (!isTxPending) {
+    setRadioInReceiveMode(dev);
+  }
+
+  return timeout;
+}
+
 static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
   switch(event) {
     case eventPacketReceived:
@@ -247,35 +603,33 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
     case eventReceiveFailed:
       break;
     case eventPacketSent:
-      // Service packet sent, the radio is back to receive automatically
       break;
     default:
       ASSERT_FAILED();
   }
 
-  if(!sendLpp(dev)) {
-    setRadioInReceiveMode(dev);
-  }
-
-  uint32_t now_ms = T2M(xTaskGetTickCount());
-  tdoaStatsUpdate(&tdoaEngineState.stats, now_ms);
+  uint32_t now = xTaskGetTickCount();
+  tdoaStatsUpdate(&tdoaEngineState.stats, T2M(now));
 
-  return MAX_TIMEOUT;
+  uint32_t timeout = startNextEvent(dev, now);
+  return timeout;
 }
 
 static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
-  // Override the default standard deviation set by the TDoA engine.
-  tdoaMeasurement->stdDev = stdDev;
-
-  estimatorEnqueueTDOA(tdoaMeasurement);
-
-  #ifdef CONFIG_DECK_LOCO_2D_POSITION
-  heightMeasurement_t heightData;
-  heightData.timestamp = xTaskGetTickCount();
-  heightData.height = DECK_LOCO_2D_POSITION_HEIGHT;
-  heightData.stdDev = 0.0001;
-  estimatorEnqueueAbsoluteHeight(&heightData);
-  #endif
+  if (ctx.isTdoaActive) {
+    // Override the default standard deviation set by the TDoA engine.
+    tdoaMeasurement->stdDev = ctx.tdoaStdDev;
+
+    estimatorEnqueueTDOA(tdoaMeasurement);
+
+    #ifdef CONFIG_DECK_LOCO_2D_POSITION
+    heightMeasurement_t heightData;
+    heightData.timestamp = xTaskGetTickCount();
+    heightData.height = DECK_LOCO_2D_POSITION_HEIGHT;
+    heightData.stdDev = 0.0001;
+    estimatorEnqueueAbsoluteHeight(&heightData);
+    #endif
+  }
 }
 
 static bool getAnchorPosition(const uint8_t anchorId, point_t* position) {
@@ -312,12 +666,37 @@ static void Initialize(dwDevice_t *dev) {
 
   dwCommitConfiguration(dev);
 
-  rangingOk = false;
+  ctx.tdoaStdDev = TDOA_ENGINE_MEASUREMENT_NOISE_STD;
+  ctx.isTdoaActive = true;
+  ctx.isReceivingPackets = false;
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+  ctx.anchorId = 254;
+  ctx.txSeqNr = 0;
+  ctx.latestTransmissionTime_ms = 0;
+  ctx.nextTxTick = 0;
+  ctx.isTwrActive = false;
+  ctx.twrSendPosition = txOwnPosNot;
+  ctx.twrStdDev = 0.25;
+  ctx.useTwrForPositionEstimation = false;
+  ctx.maxAgeOfTof_ms = 200;
+
+  ctx.averageTxDelay = 1000.0f / ANCHOR_MAX_TX_FREQ;
+  ctx.nextTxDelayEvaluationTime_ms = 0;
+
+  // Get log ids to acquire the current estimated position
+  estimatedPosLogX = logGetVarId("stateEstimate", "x");
+  estimatedPosLogY = logGetVarId("stateEstimate", "y");
+  estimatedPosLogZ = logGetVarId("stateEstimate", "z");
+
+  STATS_CNT_RATE_INIT(&ctx.cntPacketsTransmitted, STATS_INTERVAL);
+  STATS_CNT_RATE_INIT(&ctx.cntTwrSeqNrOk, STATS_INTERVAL);
+  STATS_CNT_RATE_INIT(&ctx.cntTwrToEstimator, STATS_INTERVAL);
+#endif
 }
 
-static bool isRangingOk()
-{
-  return rangingOk;
+static bool isRangingOk() {
+  return ctx.isReceivingPackets;
 }
 
 uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
@@ -329,10 +708,80 @@ uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
   .getActiveAnchorIdList = getActiveAnchorIdList,
 };
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+LOG_GROUP_START(tdoa3)
+  /**
+   * @brief Transmission rate of TWR packets in hybrid mode [packets/s]
+   */
+  STATS_CNT_RATE_LOG_ADD(hmTx, &ctx.cntPacketsTransmitted)
+
+  /**
+   * @brief Rate of received TWR packets with matching seq nr in hybrid mode [packets/s]
+   */
+  STATS_CNT_RATE_LOG_ADD(hmSeqOk, &ctx.cntTwrSeqNrOk)
+
+  /**
+   * @brief Rate of TWR measurements that are sent to the estimator in hybrid mode [samples/s]
+   */
+  STATS_CNT_RATE_LOG_ADD(hmEst, &ctx.cntTwrToEstimator)
+
+  /**
+   * @brief Measured distance to the anchor selected by the tdoa3.hmAnchLog parameter in hybrid mode [m]
+   */
+  LOG_ADD(LOG_FLOAT, hmDist, &ctx.logDistance)
+LOG_GROUP_STOP(tdoa3)
+#endif
+
 PARAM_GROUP_START(tdoa3)
 /**
  * @brief The measurement noise to use when sending TDoA measurements to the estimator.
  */
-PARAM_ADD(PARAM_FLOAT, stddev, &stdDev)
-
+PARAM_ADD(PARAM_FLOAT, stddev, &ctx.tdoaStdDev)
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+  /**
+   * @brief Anchor id used when transmitting packets in hybrid mode. Don't use 255 as this means broadcast and is
+   * received by all nodes.
+   */
+  PARAM_ADD(PARAM_UINT8, hmId, &ctx.anchorId)
+
+  /**
+   * @brief If non-zero use TDoA for position estimation in hybrid mode
+   */
+  PARAM_ADD(PARAM_UINT8, hmTdoa, &ctx.isTdoaActive)
+
+  /**
+   * @brief If non-zero transmit TWR packets in hybrid mode
+   */
+  PARAM_ADD(PARAM_UINT8, hmTwr, &ctx.isTwrActive)
+
+  /**
+   * @brief Include position information in transmitted TWR packets for other CFs to use for positioning
+   * 0 = do not send TWR packets
+   * 1 = use the current estimated position
+   * 2 = use the estimated position that was sampled when this parameter was set to 2
+   */
+  PARAM_ADD(PARAM_UINT8, hmTwrTXPos, &ctx.twrSendPosition)
+
+  /**
+   * @brief If non-zero use data from the TWR process for position estimation in hybrid mode. Requires hmTwr to be enabled.
+   */
+  PARAM_ADD(PARAM_UINT8, hmTwrEstPos, &ctx.useTwrForPositionEstimation)
+
+  /**
+   * @brief Max age of tof to include in transmitted packets.
+   */
+  PARAM_ADD(PARAM_UINT32, hmTofAge, &ctx.maxAgeOfTof_ms)
+
+  /**
+   * @brief Select an anchor id to use for logging distance to a specific anchor. The distance is available in the
+   * tdoa3.hmDist log variable. Used in hybrid mode.
+   */
+  PARAM_ADD(PARAM_UINT8, hmAnchLog, &ctx.logDistAnchorId)
+
+  /**
+   * @brief The measurement noise to use when sending TWR measurements to the estimator in hybrid mode
+   */
+  PARAM_ADD(PARAM_FLOAT, twrStd, &ctx.twrStdDev)
+#endif
 PARAM_GROUP_STOP(tdoa3)
diff --git a/src/utils/interface/tdoa/tdoaEngine.h b/src/utils/interface/tdoa/tdoaEngine.h
index ec8b06295f..022a03e3eb 100644
--- a/src/utils/interface/tdoa/tdoaEngine.h
+++ b/src/utils/interface/tdoa/tdoaEngine.h
@@ -41,7 +41,7 @@ void tdoaEngineInit(tdoaEngineState_t* state, const uint32_t now_ms, tdoaEngineS
 
 void tdoaEngineGetAnchorCtxForPacketProcessing(tdoaEngineState_t* engineState, const uint8_t anchorId, const uint32_t currentTime_ms, tdoaAnchorContext_t* anchorCtx);
 void tdoaEngineProcessPacket(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T);
-void tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const bool doExcludeId, const uint8_t excludedId);
+bool tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const bool doExcludeId, const uint8_t excludedId);
 
 #define TDOA_ENGINE_TRUNCATE_TO_ANCHOR_TS_BITMAP 0x00FFFFFFFF
 static inline uint64_t tdoaEngineTruncateToAnchorTimeStamp(uint64_t fullTimeStamp) {
diff --git a/src/utils/interface/tdoa/tdoaStorage.h b/src/utils/interface/tdoa/tdoaStorage.h
index 5f5f0c26fc..85aadd4a09 100644
--- a/src/utils/interface/tdoa/tdoaStorage.h
+++ b/src/utils/interface/tdoa/tdoaStorage.h
@@ -3,11 +3,16 @@
 
 #include "stabilizer_types.h"
 #include "clockCorrectionEngine.h"
+#include "autoconf.h"
 
 #define ANCHOR_STORAGE_COUNT 16
 #define REMOTE_ANCHOR_DATA_COUNT 16
 #define TOF_PER_ANCHOR_COUNT 16
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+#define TWR_HISTORY_LENGTH 32
+#define TWR_OUTLIER_TH 4
+#endif
 
 typedef struct {
   uint8_t id; // Id of remote remote anchor
@@ -35,8 +40,13 @@ typedef struct {
 
   point_t position; // The coordinates of the anchor
 
-  tdoaTimeOfFlight_t tof[TOF_PER_ANCHOR_COUNT];
+  tdoaTimeOfFlight_t remoteTof[TOF_PER_ANCHOR_COUNT];
   tdoaRemoteAnchorData_t remoteAnchorData[REMOTE_ANCHOR_DATA_COUNT];
+
+  #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+  uint64_t tof;
+  uint32_t tofTime_ms;
+  #endif
 } tdoaAnchorInfo_t;
 
 typedef tdoaAnchorInfo_t tdaoAnchorInfoArray_t[ANCHOR_STORAGE_COUNT];
@@ -72,8 +82,13 @@ int64_t tdoaStorageGetRemoteRxTime(const tdoaAnchorContext_t* anchorCtx, const u
 bool tdoaStorageGetRemoteRxTimeSeqNr(const tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, int64_t* rxTime, uint8_t* seqNr);
 void tdoaStorageSetRemoteRxTime(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t remoteRxTime, const uint8_t remoteSeqNr);
 void tdoaStorageGetRemoteSeqNrList(const tdoaAnchorContext_t* anchorCtx, int* remoteCount, uint8_t seqNr[], uint8_t id[]);
-int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint8_t otherAnchor);
-void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof);
+int64_t tdoaStorageGetRemoteTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint8_t otherAnchor);
+void tdoaStorageSetRemoteTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof);
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint32_t oldestAcceptableTime_ms);
+void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const int64_t tof, const uint32_t currentTime_ms);
+#endif
 
 // Mainly for test
 bool tdoaStorageIsAnchorInStorage(tdoaAnchorInfo_t anchorStorage[], const uint8_t anchor);
diff --git a/src/utils/src/tdoa/tdoaEngine.c b/src/utils/src/tdoa/tdoaEngine.c
index 4c7140d415..46fd2fc350 100644
--- a/src/utils/src/tdoa/tdoaEngine.c
+++ b/src/utils/src/tdoa/tdoaEngine.c
@@ -113,7 +113,7 @@ static bool updateClockCorrection(tdoaAnchorContext_t* anchorCtx, const int64_t
 static int64_t calcTDoA(const tdoaAnchorContext_t* otherAnchorCtx, const tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T) {
   const uint8_t otherAnchorId = tdoaStorageGetId(otherAnchorCtx);
 
-  const int64_t tof_Ar_to_An_in_cl_An = tdoaStorageGetTimeOfFlight(anchorCtx, otherAnchorId);
+  const int64_t tof_Ar_to_An_in_cl_An = tdoaStorageGetRemoteTimeOfFlight(anchorCtx, otherAnchorId);
   const int64_t rxAr_by_An_in_cl_An = tdoaStorageGetRemoteRxTime(anchorCtx, otherAnchorId);
   const double clockCorrection = tdoaStorageGetClockCorrection(anchorCtx);
 
@@ -145,7 +145,7 @@ static bool matchRandomAnchor(tdoaEngineState_t* engineState, tdoaAnchorContext_
     const uint8_t candidateAnchorId = engineState->matching.id[index];
     if (!doExcludeId || (excludedId != candidateAnchorId)) {
       if (tdoaStorageGetCreateAnchorCtx(engineState->anchorInfoArray, candidateAnchorId, now_ms, otherAnchorCtx)) {
-        if (engineState->matching.seqNr[index] == tdoaStorageGetSeqNr(otherAnchorCtx) && tdoaStorageGetTimeOfFlight(anchorCtx, candidateAnchorId)) {
+        if (engineState->matching.seqNr[index] == tdoaStorageGetSeqNr(otherAnchorCtx) && tdoaStorageGetRemoteTimeOfFlight(anchorCtx, candidateAnchorId)) {
           return true;
         }
       }
@@ -167,9 +167,9 @@ static bool matchYoungestAnchor(tdoaEngineState_t* engineState, tdoaAnchorContex
     for (int index = 0; index < remoteCount; index++) {
       const uint8_t candidateAnchorId = engineState->matching.id[index];
       if (!doExcludeId || (excludedId != candidateAnchorId)) {
-        if (tdoaStorageGetTimeOfFlight(anchorCtx, candidateAnchorId)) {
+        if (tdoaStorageGetRemoteTimeOfFlight(anchorCtx, candidateAnchorId)) {
           if (tdoaStorageGetCreateAnchorCtx(engineState->anchorInfoArray, candidateAnchorId, now_ms, otherAnchorCtx)) {
-            uint32_t updateTime = otherAnchorCtx->anchorInfo->lastUpdateTime;
+            uint32_t updateTime = tdoaStorageGetLastUpdateTime(otherAnchorCtx);
             if (updateTime > youmgestUpdateTime) {
               if (engineState->matching.seqNr[index] == tdoaStorageGetSeqNr(otherAnchorCtx)) {
                 youmgestUpdateTime = updateTime;
@@ -224,7 +224,7 @@ void tdoaEngineProcessPacket(tdoaEngineState_t* engineState, tdoaAnchorContext_t
   tdoaEngineProcessPacketFiltered(engineState, anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, false, 0);
 }
 
-void tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const bool doExcludeId, const uint8_t excludedId) {
+bool tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const bool doExcludeId, const uint8_t excludedId) {
   bool timeIsGood = updateClockCorrection(anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, &engineState->stats);
   if (timeIsGood) {
     STATS_CNT_RATE_EVENT(&engineState->stats.timeIsGood);
@@ -236,4 +236,5 @@ void tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorC
       enqueueTDOA(&otherAnchorCtx, anchorCtx, tdoaDistDiff, engineState);
     }
   }
+  return timeIsGood;
 }
diff --git a/src/utils/src/tdoa/tdoaStorage.c b/src/utils/src/tdoa/tdoaStorage.c
index 9fa7d785ec..cb6e5a4cbf 100644
--- a/src/utils/src/tdoa/tdoaStorage.c
+++ b/src/utils/src/tdoa/tdoaStorage.c
@@ -187,6 +187,22 @@ void tdoaStorageSetRxTxData(tdoaAnchorContext_t* anchorCtx, int64_t rxTime, int6
   anchorInfo->lastUpdateTime = now;
 }
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint32_t oldestAcceptableTime_ms) {
+  if (anchorCtx->anchorInfo->tofTime_ms < oldestAcceptableTime_ms) {
+    return 0;
+  }
+
+  return anchorCtx->anchorInfo->tof;
+}
+
+void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const int64_t tof, const uint32_t currentTime_ms) {
+  anchorCtx->anchorInfo->tof = tof;
+  anchorCtx->anchorInfo->tofTime_ms = currentTime_ms;
+}
+
+#endif
+
 double tdoaStorageGetClockCorrection(const tdoaAnchorContext_t* anchorCtx) {
   return clockCorrectionEngineGet(&anchorCtx->anchorInfo->clockCorrectionStorage);
 }
@@ -264,14 +280,14 @@ void tdoaStorageGetRemoteSeqNrList(const tdoaAnchorContext_t* anchorCtx, int* re
   *remoteCount = count;
 }
 
-int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint8_t otherAnchor) {
+int64_t tdoaStorageGetRemoteTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint8_t otherAnchor) {
   const tdoaAnchorInfo_t* anchorInfo = anchorCtx->anchorInfo;
 
   for (int i = 0; i < TOF_PER_ANCHOR_COUNT; i++) {
-    if (otherAnchor == anchorInfo->tof[i].id) {
+    if (otherAnchor == anchorInfo->remoteTof[i].id) {
       uint32_t now = anchorCtx->currentTime_ms;
-      if (anchorInfo->tof[i].endOfLife > now) {
-        return anchorInfo->tof[i].tof;
+      if (anchorInfo->remoteTof[i].endOfLife > now) {
+        return anchorInfo->remoteTof[i].tof;
       }
       break;
     }
@@ -280,7 +296,7 @@ int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const u
   return 0;
 }
 
-void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof) {
+void tdoaStorageSetRemoteTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof) {
   tdoaAnchorInfo_t* anchorInfo = anchorCtx->anchorInfo;
 
   int indexToUpdate = 0;
@@ -288,20 +304,20 @@ void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t re
   uint32_t oldestTime = 0xFFFFFFFF;
 
   for (int i = 0; i < TOF_PER_ANCHOR_COUNT; i++) {
-    if (remoteAnchor == anchorInfo->tof[i].id) {
+    if (remoteAnchor == anchorInfo->remoteTof[i].id) {
       indexToUpdate = i;
       break;
     }
 
-    if (anchorInfo->tof[i].endOfLife < oldestTime) {
-      oldestTime = anchorInfo->tof[i].endOfLife;
+    if (anchorInfo->remoteTof[i].endOfLife < oldestTime) {
+      oldestTime = anchorInfo->remoteTof[i].endOfLife;
       indexToUpdate = i;
     }
   }
 
-  anchorInfo->tof[indexToUpdate].id = remoteAnchor;
-  anchorInfo->tof[indexToUpdate].tof = tof;
-  anchorInfo->tof[indexToUpdate].endOfLife = now + TOF_VALIDITY_PERIOD;
+  anchorInfo->remoteTof[indexToUpdate].id = remoteAnchor;
+  anchorInfo->remoteTof[indexToUpdate].tof = tof;
+  anchorInfo->remoteTof[indexToUpdate].endOfLife = now + TOF_VALIDITY_PERIOD;
 }
 
 bool tdoaStorageIsAnchorInStorage(tdoaAnchorInfo_t anchorStorage[], const uint8_t anchor) {
diff --git a/test/utils/src/tdoa/test_tdoa_storage.c b/test/utils/src/tdoa/test_tdoa_storage.c
index bcfd605801..9f3e9ea41c 100644
--- a/test/utils/src/tdoa/test_tdoa_storage.c
+++ b/test/utils/src/tdoa/test_tdoa_storage.c
@@ -599,7 +599,7 @@ void testThatNoTimeOfFlightIsReturnedWhenRemoteAnchorIsNotInStorage() {
   tdoaStorageGetCreateAnchorCtx(storage, anchor, storageTime, &context);
 
   // Test
-  int64_t actual = tdoaStorageGetTimeOfFlight(&context, remoteAnchor);
+  int64_t actual = tdoaStorageGetRemoteTimeOfFlight(&context, remoteAnchor);
 
   // Assert
   TEST_ASSERT_EQUAL_INT64(expected, actual);
@@ -617,7 +617,7 @@ void testThatTimeOfFlightIsReturnedWhenSet() {
   fixtureSetTof(&context, anchor, storageTime, remoteAnchor, expected);
 
   // Test
-  int64_t actual = tdoaStorageGetTimeOfFlight(&context, remoteAnchor);
+  int64_t actual = tdoaStorageGetRemoteTimeOfFlight(&context, remoteAnchor);
 
   // Assert
   TEST_ASSERT_EQUAL_INT64(expected, actual);
@@ -638,7 +638,7 @@ void testThatTimeOfFlightIsReturnedWhenSetASecondTime() {
   fixtureSetTof(&context, anchor, storageTime, remoteAnchor, expected);
 
   // Test
-  int64_t actual = tdoaStorageGetTimeOfFlight(&context, remoteAnchor);
+  int64_t actual = tdoaStorageGetRemoteTimeOfFlight(&context, remoteAnchor);
 
   // Assert
   TEST_ASSERT_EQUAL_INT64(expected, actual);
@@ -675,13 +675,33 @@ void testThatTofReplacesTheOldestEntryWhenStorageIsFull() {
   fixtureSetTof(&context, anchor, verificationStorageTime, verificationRemoteAnchor, verificationTof);
 
   // Assert
-  const int64_t actualVerification = tdoaStorageGetTimeOfFlight(&context, verificationRemoteAnchor);
+  const int64_t actualVerification = tdoaStorageGetRemoteTimeOfFlight(&context, verificationRemoteAnchor);
   TEST_ASSERT_EQUAL_INT64(verificationTof, actualVerification);
 
-  const int64_t actualReplaced = tdoaStorageGetTimeOfFlight(&context, oldestRemoteAnchor);
+  const int64_t actualReplaced = tdoaStorageGetRemoteTimeOfFlight(&context, oldestRemoteAnchor);
   TEST_ASSERT_EQUAL_INT64(0, actualReplaced);
 }
 
+// Not possible to put the ifdef outside the test function due to the way the test framework is implemented
+void testThatTimeOfFlightIsSet() {
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+  // Fixture
+  uint32_t storageTime_ms = 1234;
+  int64_t expectedToF = 4747474747;
+
+  tdoaAnchorContext_t context;
+  tdoaStorageGetCreateAnchorCtx(storage, 0, 0, &context);
+
+  // Test
+  tdoaStorageSetTimeOfFlight(&context, expectedToF, storageTime_ms);
+
+  // Assert
+  TEST_ASSERT_EQUAL_UINT64(expectedToF, tdoaStorageGetTimeOfFlight(&context, storageTime_ms - 1));
+  TEST_ASSERT_EQUAL_UINT64(expectedToF, tdoaStorageGetTimeOfFlight(&context, storageTime_ms));
+  TEST_ASSERT_EQUAL_UINT64(0, tdoaStorageGetTimeOfFlight(&context, storageTime_ms + 1));
+#endif
+}
+
 
 // Helpers ///////////////
 
@@ -692,5 +712,5 @@ static void fixtureSetRemoteRxTime(tdoaAnchorContext_t* context, const uint8_t a
 
 static void fixtureSetTof(tdoaAnchorContext_t* context, const uint8_t anchor, const uint32_t storageTime, const uint8_t remoteAnchor, const uint64_t tof) {
   tdoaStorageGetCreateAnchorCtx(storage, anchor, storageTime, context);
-  tdoaStorageSetTimeOfFlight(context, remoteAnchor, tof);
+  tdoaStorageSetRemoteTimeOfFlight(context, remoteAnchor, tof);
 }