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?