Localization Using 3 DWM1000 anchors and a tag

Hello everyone, need some serious help here. I am currently working on my project which is human following robot and for that I am using DW1000 UWB technology. What I have done up till now is that I have calibrated each of three anchors antenna delay with the tag and I ca see all three distances from each anchor to the tag when opening serial monitor of tag. Then I send all these three distances from tag to the third anchor’ microcontroller using ESP NOW. In my case each anchor has its own esp32 and specifically talking about third anchor’s microcontroller I thought it will be a good idea to use esp32 of third anchor as a main controller board of my robot. So, this controller will have a third anchor and basic functioning of robot.

What I want is that now as all three distances are available on single microcontroller I want to do localization to know the exact position of tag. I tried using various AI generated codes for localization but failed to do so. So anyone please help me so that I can do this localization. Any code, recourse will be appreciated. The code of anchor3 and tag is shown below. Thanks in advance. Waiting for someone’s help…

Anchor3 Code:
/**************************************************************

  • Anchor #3 + Main Controller
    • Acts as a DW1000 anchor (ID = 85).
    • Receives 3 distances from the Tag (via ESP-NOW).
    • In the future, you can add robot motor control,
  • obstacle avoidance, etc. in this code.
    **************************************************************/

#include <SPI.h>
#include “DW1000Ranging.h”
#include “DW1000.h”
#include <WiFi.h>
#include <esp_now.h>

// ---------------------
// DW1000 Setup
// ---------------------
char anchor_addr = “85:00:5B:D5:A9:9A:E2:9C”;
uint16_t Adelay = 16608;

#define SPI_SCK 18
#define SPI_MISO 19
#define SPI_MOSI 23
#define DW_CS 5

const uint8_t PIN_RST = 16;
const uint8_t PIN_IRQ = 17;
const uint8_t PIN_SS = 5;

// If you still want to see the local anchor #3 distance on this board:
void newRange() {
float dist = DW1000Ranging.getDistantDevice()->getRange();
Serial.print("Anchor #3 local distance to tag: ");
Serial.println(dist);
}
void newDevice(DW1000Device *device) {
Serial.print("Anchor #3 new device: ");
Serial.println(device->getShortAddress(), HEX);
}
void inactiveDevice(DW1000Device *device) {
Serial.print("Anchor #3 inactive device: ");
Serial.println(device->getShortAddress(), HEX);
}

// ---------------------
// ESP-NOW Setup
// ---------------------
// A struct for receiving all 3 distances in one packet.
typedef struct {
float distAnchor83;
float distAnchor84;
float distAnchor85;
} DistancesPacket;

// We’ll store the received distances here.
float distance83 = 0.0;
float distance84 = 0.0;
float distance85 = 0.0;

// Callback: runs whenever the Tag sends the updated distances.
void onDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
DistancesPacket packet;
memcpy(&packet, incomingData, sizeof(packet));

distance83 = packet.distAnchor83;
distance84 = packet.distAnchor84;
distance85 = packet.distAnchor85;

Serial.println(“=== Received 3 distances from Tag ===”);
Serial.print("Anchor 83: ");
Serial.println(distance83);
Serial.print("Anchor 84: ");
Serial.println(distance84);
Serial.print("Anchor 85: ");
Serial.println(distance85);

// TODO: Here you can add your localization/trilateration,
// motor control, obstacle avoidance, etc.
}

void setup() {
Serial.begin(115200);
delay(1000);

Serial.println(“=== Anchor #3 (Main Controller) ===”);

// 1) Initialize DW1000 (Anchor #3)
SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
DW1000Ranging.initCommunication(PIN_RST, PIN_SS, PIN_IRQ);
DW1000.setAntennaDelay(Adelay);
DW1000Ranging.attachNewRange(newRange);
DW1000Ranging.attachNewDevice(newDevice);
DW1000Ranging.attachInactiveDevice(inactiveDevice);
DW1000Ranging.startAsAnchor(anchor_addr, DW1000.MODE_LONGDATA_RANGE_LOWPOWER, false);

// 2) Initialize ESP-NOW to receive distances from the Tag
WiFi.mode(WIFI_STA);
if (esp_now_init() != ESP_OK) {
Serial.println(“Error initializing ESP-NOW”);
return;
}
esp_now_register_recv_cb(onDataRecv);
}

void loop() {
// Keep measuring local distance if you want (anchor #3 → tag).
DW1000Ranging.loop();

// For demonstration, print all 3 distances once per second
static unsigned long lastPrint = 0;
if (millis() - lastPrint > 1000) {
lastPrint = millis();
Serial.print(“[Periodic] A83=”);
Serial.print(distance83);
Serial.print(" m, A84=“);
Serial.print(distance84);
Serial.print(” m, A85=“);
Serial.print(distance85);
Serial.println(” m");
}
}

Tag Code:
/**************************************************************

  • TAG Code
    • Measures distance to each of the 3 anchors (83,84,85).
    • Collects all 3 distances in a struct.
    • Sends them (via ESP-NOW) to Anchor #3’s MAC address.
      **************************************************************/

#include <SPI.h>
#include “DW1000Ranging.h”
#include “DW1000.h”
#include <WiFi.h>
#include <esp_now.h>

// ---------------------
// DW1000 Setup
// ---------------------
#define SPI_SCK 18
#define SPI_MISO 19
#define SPI_MOSI 23
#define DW_CS 5

const uint8_t PIN_RST = 16;
const uint8_t PIN_IRQ = 17;
const uint8_t PIN_SS = 5;

// Tag short address
char tag_addr = “02:00:00:00:00:00:10:01”;

// Per-Anchor Moving Average Configuration
#define MAX_ANCHORS 3 // Maximum number of anchors
#define WINDOW_SIZE 15 // Adjust as needed
#define MEDIAN_WINDOW 5
float distanceBuffer[WINDOW_SIZE]; // Buffer to hold historical data

// We’ll store the distances for anchors 83, 84, 85
float distAnchor83 = 0.0;
float distAnchor84 = 0.0;
float distAnchor85 = 0.0;

typedef struct {
uint16_t shortAddress;
float buffer[WINDOW_SIZE];
int index;
bool filled;
float medianHist[MEDIAN_WINDOW];
int medIndex;
} AnchorFilter;

AnchorFilter anchorFilters[MAX_ANCHORS];
int numAnchors = 0;

// ---------------------
// ESP-NOW Setup
// ---------------------
// A struct to send all three distances in one packet.
typedef struct {
float distAnchor83;
float distAnchor84;
float distAnchor85;
} DistancesPacket;

DistancesPacket packetToSend;

// >>> REPLACE WITH ANCHOR #3’s ACTUAL MAC ADDRESS <<<
uint8_t anchor3MAC = {0xCC, 0x7B, 0x5C, 0xF1, 0xF5, 0xBC}; // Example placeholder

// Optional: Callback to see if the packet was sent successfully
void onDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
// You can uncomment for debugging:
// Serial.print("ESP-NOW Send Status: ");
// Serial.println(status == ESP_NOW_SEND_SUCCESS ? “Success” : “Fail”);
}

void setup()
{
Serial.begin(115200);
delay(1000);

// 1) Initialize DW1000 for Tag
SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
DW1000Ranging.initCommunication(PIN_RST, PIN_SS, PIN_IRQ);
DW1000Ranging.attachNewRange(newRange);
DW1000Ranging.attachNewDevice(newDevice);
DW1000Ranging.attachInactiveDevice(inactiveDevice);

// Start as tag
DW1000Ranging.startAsTag(tag_addr, DW1000.MODE_LONGDATA_RANGE_LOWPOWER, false);

DW1000.setChannel(2);

// DW1000.setPulseFrequency(DW1000.PULSE_FREQ_16MHZ);

// Initialize buffer with zeros
for (int i = 0; i < WINDOW_SIZE; i++) {
distanceBuffer[i] = 0.0;
}

// 2) Initialize ESP-NOW for sending data to Anchor #3
WiFi.mode(WIFI_STA); // Station mode
if (esp_now_init() != ESP_OK) {
Serial.println(“Error initializing ESP-NOW”);
return;
}
esp_now_register_send_cb(onDataSent);

// Add Anchor #3 as a peer
esp_now_peer_info_t peerInfo;
memcpy(peerInfo.peer_addr, anchor3MAC, 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
if (esp_now_add_peer(&peerInfo) != ESP_OK) {
Serial.println(“Failed to add peer (Anchor #3)”);
return;
}
}

float getMedian(float* arr, int size) {
float temp[size];
memcpy(temp, arr, sizeof(temp));
std::sort(temp, temp + size);
return temp[size/2];
}

void loop()
{
// Continuously process DW1000 ranging
DW1000Ranging.loop();
}

// Called whenever a new distance measurement is ready
void newRange()
{
// Identify which anchor this measurement is from
DW1000Device* dev = DW1000Ranging.getDistantDevice();
uint16_t shortAddr = dev->getShortAddress();
float rawDistance = dev->getRange();

// // Print for debugging
// Serial.print(shortAddr, HEX);
// Serial.print(", ");
// Serial.println(distance);

// Median filtering
static float medianHistory[MAX_ANCHORS][MEDIAN_WINDOW];
static int medIndex[MAX_ANCHORS] = {0};

int anchorIdx = -1;
for(int i=0; i<numAnchors; i++) {
if(anchorFilters[i].shortAddress == shortAddr) {
anchorIdx = i;
break;
}
}

if(anchorIdx != -1) {
medianHistory[anchorIdx][medIndex[anchorIdx]] = rawDistance;
medIndex[anchorIdx] = (medIndex[anchorIdx] + 1) % MEDIAN_WINDOW;
rawDistance = getMedian(medianHistory[anchorIdx], MEDIAN_WINDOW);
}

// Find or create filter for this anchor
int filterIndex = -1;
for(int i=0; i<numAnchors; i++) {
if(anchorFilters[i].shortAddress == shortAddr) {
filterIndex = i;
break;
}
}

if(filterIndex == -1 && numAnchors < MAX_ANCHORS) {
filterIndex = numAnchors++;
anchorFilters[filterIndex].shortAddress = shortAddr;
memset(anchorFilters[filterIndex].buffer, 0, sizeof(float)*WINDOW_SIZE);
}

if(filterIndex != -1) {
// Update buffer for this anchor
AnchorFilter* filter = &anchorFilters[filterIndex];
filter->buffer[filter->index] = rawDistance;
filter->index = (filter->index + 1) % WINDOW_SIZE;

// Calculate moving average
float avg = 0.0;
int count = filter->filled ? WINDOW_SIZE : filter->index;
for(int i=0; i<count; i++) {
  avg += filter->buffer[i];
}
avg /= count;

if(!filter->filled && filter->index == 0) {
  filter->filled = true;
}

// Print results
Serial.print(shortAddr, HEX);
Serial.print(",");
Serial.print(avg);
Serial.println(" m");

if (shortAddr == 0x83) {
distAnchor83 = avg;

} else if (shortAddr == 0x84) {
distAnchor84 = avg;
} else if (shortAddr == 0x85) {
distAnchor85 = avg;
}
}

// Store this distance based on anchor ID
// (In your case, anchor short addresses are: 0x835B, 0x845B, 0x855B, etc.
// But let’s do a partial match for the last two digits or a known pattern.)
// The short address you see might be truncated from your “83:00:5B:D5:…” etc.
// Adjust the if-conditions to match your actual short addresses.

// Prepare the struct with all three distances
packetToSend.distAnchor83 = distAnchor83;
packetToSend.distAnchor84 = distAnchor84;
packetToSend.distAnchor85 = distAnchor85;

// Send the entire struct to Anchor #3
esp_now_send(anchor3MAC, (uint8_t*)&packetToSend, sizeof(packetToSend));
}

void newDevice(DW1000Device *device)
{
Serial.print("Tag sees new device: ");
Serial.println(device->getShortAddress(), HEX);
}

void inactiveDevice(DW1000Device *device)
{
Serial.print("Tag sees inactive device: ");
Serial.println(device->getShortAddress(), HEX);
}

So you want to calculate the XY location based on 3 measured ranges.
This is a fairly standard trilateration problem. coordinate systems - Find X location using 3 known (X,Y) location using trilateration - Mathematics Stack Exchange gives you the maths you need to implement.

Thanks a lot it helped me a lot and I was successful to done with localization but again I am in a serious issue when I manually put distances values then I get x and y coordinates of tag exactly same where I put the tag but when these distances values are taken which each anchors measure from tag then I was unable to get the same tag position.

Actually here I observe that after calculating antenna delay of each anchor when one anchor is on then I get the same and accurate distance between anchor and tag which I can see on serial monitor but when all three anchors are on then when opening serial monitor of tag all three distances becomes inaccurate and gives accuracy of some times above +50cm or even -50cm for example if distance between A1 and tag is 1m then after all anchors are on this distance becomes 1.5cm and even more with more and more fluctuations and same the case with other anchors which is the major cause why I am not getting the exact coordinated of tag where it is placed.

I don’t know what the issue I am stuck here. I am operating anchors and tag in RANGE_LOWPOWER mode as:
DW1000Ranging.startAsTag(tag_addr, DW1000.MODE_LONGDATA_RANGE_LOWPOWER, false);

As I am making human following robot so I need more range if I use accuracy then after approx 2.5m communication fails between anchors and tag. ASo please @AndyA or anyone kindly guide me what’s the issue is why I am not getting same distance when I switch on multiple anchors. However I saw articles it is implementable to make human following robot using DWM1000 uwb but Here I am stuck with localization. So kindly anyone guide me so that I can do accurate localization of tag.

If each tag - anchor range is accurate when no other anchors are on and inaccurate when other anchors are switched on then you probably have a firmware bug.
This is fairly common to see when someone has taken the standard example code and tried to scale it to multiple anchors, it’s caused by all of the anchors responding to the range initiation message from the tag.
With UWB you need to ensure that only one device is ever talking at a time.
When the tag starts a range request to an anchor it needs to include the anchors ID number in that request. You then need to ensure that each anchor ignores any requests that aren’t addressed to its ID and only replies when it is requested.

This has been covered on this forum multiple times, you should be able to find a lot more details by searching past posts.

You said that I need to ensure that only one anchor is talking at a time. Isn’t it will be an issue for me. As I am making human following robot using DWM1000 UWB which is implementable as people had already implemented it and for that all three anchors should be communicating to the tag at the same time means all anchors should be on and if I on all anchors then all anchors communicate with tag but all three distances becomes inaccurate. What should I do now?

I need another advice from you that when tag is in (0.2, 0.94) x and y coordinates respectively and due to distance fluctuations when all anchors are on I get distance on serial monitor (0.18, 1.74). Now as you can see y coordinate is very much inaccurate due to distance fluctuations will robot able to reach towards the tag? According to me its a serious issue because y coordinate which code shows is +80cm the real location of tag. That’s why I stuck and thinking how can I get accurate distances even all anchors are on. And its a logic as peoples have implemented following through DWM1000 UWB but I am stuck here and need some help or guidance. So please anyone guide me how can I get the accurate distances even all anchors are on. I can provide the code here if anyone demanded.

No one has done this using 3 anchors talking at the same time. They have done it using 3 anchors talking at very slightly different times.

You can measure a range in a couple of ms (or less). You measure to just anchor 1 with nothing else responding. Then you measure to anchor 2 with nothing else responding. Then you measure to anchor 3 with nothing else responding. If you do all of that quickly enough then you can assume that nothing has moved significantly and so you can calculate a position as if all the ranges were measured simultaneously.
If you assume your range has an error of 5 cm then for someone moving at walking speeds (around 2 m/s) as long as all your measurements are done in around 25 ms the errors due to them moving will be smaller than the range errors in your measurements and so the overall accuracy won’t suffer much.
The faster things are moving the trickier this becomes but walking speed is fairly easy.

You mean that I should add a small milli second delay on working time of each anchor so that each anchor can communicate to tag when other anchors are in sleep mode. For example all anchors are on and I add a delay in the code that first communicate with A1 while A2 and A3 are on but tag not communicating with them after that I add a small delay of let’s say 100ms and after that A2 becomes active to communicate with tag while other two goes to sleep mode. Is this strategy you are talking about?
Through this will I be able two get all three distances from three anchors when all the anchors are on at the same time but just communicating with tag after a small delay of milli seconds so that one anchor communicate with tag at a time?

No. I mean you should do exactly what I said in the previous post.

Include an anchor ID in the range initiation message from the tag. Ensure anchors only reply to messages that are addressed to their ID. The tag can then pick which anchor to range to and can range to each one in turn.

Ok I understand your point to include each of three anchor’s id in different range in tag code so that tag should communicate with only one anchor at a time and this range will be many small in milli seconds. Its ok but how to implement it in the code. I discussed it with chat gpt its saying to modify the library and I tried myself but still confuse how to compel the tag to send specific anchor ID of each anchor in its range. I am using jremington DWM1000 library. So please guide me so that I can get the desired output.

That’s a fairly involved library that I’ve never used, I’m not going to dig through the code to work out how to use it, that’s your job.
However there looks to be examples of how do do exactly what you want, calculate a location based on 3 anchors. Are you using that?
In the anchor code example the very first lines are a comment telling you to make sure you set the ID do a different number for each anchor. Did you do that?

Yes I want to calculate location of tag using three anchors and that was done by me when you gave me link for all mathematical calculations for localization but the issue was when I on all 3 anchors all 3 distances become inaccurate however when using only one anchor distance value comes to be accurate as I mentioned in my previous posts.

Which examples are you talking about to calculate a location based 3 anchors?

Yes, I assign each anchor a different ID as mentioned:
A1 = 83:00:5B:D5:A9:9A:E2:9C
A2 = 84:00:5B:D5:A9:9A:E2:9C
A3 = 85:00:5B:D5:A9:9A:E2:9C

Each tag has different unique id and tag has also different unique id as: 02:00:00:00:00:00:10:01.
Actually I am confused how can I implement that what you mentioned previously to implement one anchor and tag communication at a time and then moves toward other anchors. Whether to add a delay in tag or anchor code just need for some guidance. Thanks you already helped me alot for localization just want some more help to get rid of this problem also.