Integrating TDOA location system

Development Team
1

Hi, I’m developing a location system based on TDOA meant to be used in a UAVs swarm…
TDOA is a multilateration algorithm which uses beacons and tags( the UAVs).

The final purpose is to provide the GCS with a 3D coordinate of the UAV’s position in a local reference system.

As hardware we’re using DWM1000 fro the UWB transmissions.
The beacons (or anchors) transmit in broadcast and listen to each other.
They integrate the info they get and send this packets:

typedef struct packet_s {
    union {
      uint16_t fcf;
      struct {
        uint16_t type:3;
        uint16_t security:1;
        uint16_t framePending:1;
        uint16_t ack:1;
        uint16_t ipan:1;
        uint16_t reserved:3;
        uint16_t destAddrMode:2;
        uint16_t version:2;
        uint16_t srcAddrMode:2;
      } fcf_s;
    };
    uint8_t seq;
    uint16_t pan;
    locoAddress_t destAddress;
    locoAddress_t sourceAddress;
    uint8_t payload[64];
} __attribute__((packed)) packet_t;

Where payload contains the timestamps of the other beacons:

typedef struct rangePacket_s {
  uint8_t type;
  uint8_t txMaster[5];
  uint8_t timestamps[LOCODECK_NR_OF_ANCHORS][5];
} __attribute__((packed)) rangePacket_t;

The beacons (up to 8) transmit one at a time every 2ms, in the orther of their id (from 0 to 7), and then restart.

The UAVs are only using the DWM1000 to receive these packets.
Since they don’t transmit anything this system is much more advantageus for UAVs swarm compared to TOA.

The UAVs use Pixracer boards and the DWM1000 is connected via SPI.
I wrote a driver which accomplishes successful setup and communication with the DWM1000.

(The data I have is 40bit precise so for most the variables I need double)

Now I’m dealing with the final part. I see two possible ways to proceed:

  • use the TDoA of every packet to update the EKF
  • calculate the position autonomously and pass it to the EKF

The Bitcraze’s Crazyflie project utilizes the first method. They implemented an EKF which can integrate tdoa.
This also has the advantage to make use of single packets in “cicles” with a lot of packets lost.

Does anyone know how could I do something similar with Ardupilot’s EKF?

About other approach, what I tried to do is put the data I got into a linear system, “solve” it with a least-squares, and optimize the result (I followed this paper ).
I wrote the code in a rush so I still have to see but the main problem is that I can’t find a reliable way to compute the least-squares.
The library I tried gives very bad result. I tried Eigen in Visual studio and it’s just not comparable, the problem is that it seems incompatible with ardupilot’s compiler.
Also the optimization step seems a bit heavy (lot of matrix operations), but I’m not yet sure about that.

Any suggestion about this method?

1 Like
2

About the point of integrating data from the single packets in the EKF.

To explain better… In the crazyflie they based the EKF on these two papers, but to me this seems to organic for it to just be copied in ardupilot’s EKF, am I wrong?

What I was asking for is: is there a way to make the EKF use the TDOA’s data in a similar fashion as what it does with Pozyx data?

(Sorry, I can’t edit my post)

3

The ardupilot EKF3 already has a method to utilise range measurements to known locations. This has been tested on the Pozyx system: http://ardupilot.org/copter/docs/common-pozyx.html

Data is read here: https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp#L715

Fusion is managed here:
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp

4

@priseborough Thank you for answering, but I don’t get it; doesn’t Pozyx use distances from beacons?
My data are differences of distances from the UAV to couples of beacons (multilateration).
Does EKF3 already accept this kind of data? I can’t see it in the code.

5

Sorry, I missed that part. That would require adding an additional EKF state to resolve the time ambiguity, or solving for it before the data is sent to the EKF.

6

If it’s not too much could you give me some guidelines about how that should be done?

7

Adding another state to the ArduPilot EKF is a complex undertaking due to the number of places in code that have to change and will also increase memory and CPU utilisation for all users due to the larger memory allocation, which is not something the lead developers would be keen on at the moment.

I am anticipating developments at the operating system level that should improve available memory, but I don’t have a time frame on that. My recommendation in the short term is to develop a reduced order cartesian EKF that uses the NED frame accelerations from the main filter. Do you have any previous experience with estimator development?

The derivation for the EKF3 equations are are here: https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m

There is a matlab implementation of the derivation used by EKF3 that can be used to prototype algorithm changes:

8

Unfortunately I don’t, but I’m willing to make an effort.

Thanks, I’ll start to look into it.

9

hello,have you done this job now?

10

@wuhaixiang No, I’m still dealing with some isues. If all goes well it’s possible I won’t have to make the changes to the EKF.

11

hello,have you used the POZYX module to test indoor location?

12

No, I’m using anchors/beacons with the bitcraze firmware and I ported the Crazyflie driver (more or less) in ardupilot.

Anyway I’m using TDoA (Time Difference of Arrival) and Pozyx uses TOA (Time of Arrival).

13

I have the new beacon system working, but I’m stuck on making the EKF accept the estimated position (exactly like a GPS). The telemetry position is always 0,0.

The algorithm is TDoA which scales a lot better than others because the vehicles don’t send any signal, the position is calculated onboard!

Unlike other systems, I don’t know the distances from the beacons, so I can’t feed them into the EKF2.

May I ask some help from @rmackay9, @tridge or @priseborough that worked on EKF and Beacons? I think I just need a hint to integrate it and to be able to make a pull request.

More details:

BCN_LATITUDE = 44,40241   //so at least I expect pos to be 44,4 8,9
BCN_LONGITUDE = 8,960767  //instead of 0,0
BCN_ALT = 200
BCN_TYPE = 3  // I've implemented the type 3
EK2_ENABLE = 1
EK2_GPS_TYPE = 3
GPS_TYPE = 0

readyToUseRangeBeacon() is always false

Does PV_AidingMode have to be AID_ABSOLUTE?

I’m setting the 3D position into veh_pos_ned, something like:

void AP_Beacon_TDoA::update(void)
{
    set_vehicle_position( <my-estimated-position> , <my-accuracy>);
    last_update_ms = AP_HAL::millis();
}
14

Did you set the home position ? (with mission planner or another GCS)

15

@khancyr Do you mean with this parameters?

BCN_LATITUDE = 44,40241   
BCN_LONGITUDE = 8,960767 
BCN_ALT = 200
16

No, I mean like on opticalflow and visualOdom :
" Set the home position from the ground station map (from MP’s Flight Data screen, right-mouse-button-click on the map and select “Set Home Here”). The vehicle should appear on the map."

17

@khancyr I just tried but nothing changes.

18

Hello,I am also doing something about it.Have you done this job now?

19

Hi @bodhan, the TDOA localization is almost done, I’ve finished the IDaniele work and I’m tuning it. Have you tried something similar?

20

@guglie could you maybe share your work? How did you implement the TDOA localization?