Hi,
I am currently working on a project where I use three anchors and one tag to measure the distance between the tag and the anchors with an accuracy of approximately ±10 cm. However, I have encountered a challenge:
When a person or a device (e.g., a laptop or mobile phone) is present near either the tag or the anchors, the accuracy of the distance measurements deteriorates significantly, with deviations increasing to around ±30 to ±40 cm. I have already performed antenna calibration, but the issue persists.
Could you please suggest any methods or techniques to maintain the accuracy within ±10 to ±20 cm even in the presence of such disturbances?
Additionally, I am calculating the coordinates of the tag using three anchors and the trilateration algorithm. While this approach works under ideal conditions, I am not achieving the desired level of accuracy in real-world scenarios. If there is a better algorithm or methodology to improve the coordinate calculation, I would appreciate your guidance.
Below is the code I am using for coordinate calculation based on the trilateration algorithm:
#include “driver/spi_master.h”
#include <bits/stdc++.h>
#include “driver/gpio.h”
#include “DW1000Ranging.hpp”
#include <stdio.h>
#include <float.h>
#include “esp_log.h”
#include “driver/i2c.h”
#include <string.h>
#include “sdkconfig.h”
#include <stdlib.h>
#include “freertos/FreeRTOS.h”
#include “freertos/task.h”
#include “i2c_oled.h”
#include “mqtt_services.h”
#include “nvs_flash.h”
#include “local_time.h”
using namespace std;
extern void oled_lcd_init();
#define TAG “TAG”
#define TAG_ADDR “7D:00:22:EA:82:60:3B:9B”
#define SPI_SCK GPIO_NUM_18
#define SPI_MISO GPIO_NUM_19
#define SPI_MOSI GPIO_NUM_23
#define UWB_RST GPIO_NUM_27
#define UWB_IRQ GPIO_NUM_34
#define UWB_SS GPIO_NUM_21
#define ANTENA_DELAY 16421
struct Link
{
uint16_t anchor_addr;
float range;
float dbm;
struct Link *next;
};
struct Link *uwb_data;
spi_device_handle_t spi;
esp_err_t SPI_init(void)
{
spi_bus_config_t buscfg = {0}; // Default-initialize all fields to zero
buscfg.mosi_io_num = SPI_MOSI;
buscfg.miso_io_num = SPI_MISO;
buscfg.sclk_io_num = SPI_SCK;
buscfg.quadwp_io_num = -1;
buscfg.quadhd_io_num = -1;
buscfg.max_transfer_sz = 32;
esp_err_t ret = spi_bus_initialize(SPI2_HOST, &buscfg, SPI_DMA_CH_AUTO);
if (ret != ESP_OK)
{
return ret;
}
spi_device_interface_config_t devcfg = {0};
// configure device_structure
devcfg.clock_speed_hz = 5 * 1000 * 1000; // Clock out at 1 MHz
devcfg.mode = 0; // SPI mode 0: CPOL:-0 and CPHA:-0
devcfg.spics_io_num = UWB_SS; // This field is used to specify the GPIO pin that is to be used as CS’
devcfg.queue_size = 7; // We want to be able to queue 7 transactions at a time
devcfg.flags = SPI_DEVICE_NO_DUMMY;
ret = spi_bus_add_device(SPI2_HOST, &devcfg, &spi);
return ret;
}
// Data Link
struct Link *init_link()
{
#ifdef DEBUG
printf(“init_link\n”);
#endif
struct Link *p = (struct Link *)malloc(sizeof(struct Link));
p->next = NULL;
p->anchor_addr = 0;
p->range = 0.0;
return p;
}
struct Link *find_link(struct Link *p, uint16_t addr)
{
#ifdef DEBUG
printf(“find_link\n”);
#endif
if (addr == 0)
{
printf(“find_link:Input addr is 0\n”);
return NULL;
}
if (p->next == NULL)
{
printf("find_link:Link is empty\n");
return NULL;
}
struct Link *temp = p;
// Find target struct Link or struct Link end
while (temp->next != NULL)
{
temp = temp->next;
if (temp->anchor_addr == addr)
{
// Serial.println("find_link:Find addr");
return temp;
}
}
printf("find_link:Can't find addr\n");
return NULL;
}
void add_link(struct Link *p, uint16_t addr)
{
#ifdef DEBUG
printf(“add_link\n”);
#endif
struct Link *temp = p;
// Find struct Link end
while (temp->next != NULL)
{
temp = temp->next;
}
printf("add_link:find struct Link end\n");
// Create a anchor
struct Link *a = (struct Link *)malloc(sizeof(struct Link));
a->anchor_addr = addr;
a->range = 0.0;
a->dbm = 0.0;
a->next = NULL;
// Add anchor to end of struct Link
temp->next = a;
return;
}
void fresh_link(struct Link *p, uint16_t addr, float range, float dbm)
{
#ifdef DEBUG
printf(“fresh_link\n”);
#endif
struct Link *temp = find_link(p, addr);
if (temp != NULL)
{
temp->range = range;
temp->dbm = dbm;
return;
}
else
{
printf("fresh_link:Fresh fail\n");
return;
}
}
void print_link(struct Link *p)
{
#ifdef DEBUG
printf(“print_link\n”);
#endif
struct Link *temp = p;
while (temp->next != NULL)
{
printf("%d\n", temp->next->anchor_addr);
printf("%f\n", temp->next->range);
printf("%f\n", temp->next->dbm);
temp = temp->next;
}
return;
}
void delete_link(struct Link *p, uint16_t addr)
{
#ifdef DEBUG
printf(“delete_link\n”);
#endif
if (addr == 0)
return;
struct Link *temp = p;
while (temp->next != NULL)
{
if (temp->next->anchor_addr == addr)
{
struct Link *del = temp->next;
temp->next = del->next;
free(del);
return;
}
temp = temp->next;
}
return;
}
#define MEDIAN_WINDOW 7
typedef struct
{
float x;
float y;
} Point3D;
static set s1;
static set s2;
static set s3;
float r1 = -1, r2 = -1, r3 = -1;
bool a1 = false, a2 = false, a3 = false;
static bool flag_a1 = false, flag_a2 = false, flag_a3 = false;
Point3D calculatePosition(
Point3D anchor1, float r1,
Point3D anchor2, float r2,
Point3D anchor3, float r3)
{
s1.insert(r1);
s2.insert(r2);
s3.insert(r3);
cout << "r1: " << r1 << " r2: " << r2 << " r3: " << r3 << endl;
if (s1.size() == MEDIAN_WINDOW)
{
auto it1 = s1.begin();
advance(it1, s1.size() / 2); // Advance the iterator to the middle
r1 = *it1;
auto it2 = s2.begin();
advance(it2, s2.size() / 2); // Advance the iterator to the middle
r2 = *it2;
auto it3 = s3.begin();
advance(it3, s3.size() / 2); // Advance the iterator to the middle
r3 = *it3;
s1.clear();
s2.clear();
s3.clear();
Point3D result = {0, 0};
// Convert to 2D problem by setting all Z coordinates to 0
float x1 = anchor1.x, y1 = anchor1.y;
float x2 = anchor2.x, y2 = anchor2.y;
float x3 = anchor3.x, y3 = anchor3.y;
// Calculate intermediate values
float A = 2 * (x2 - x1);
float B = 2 * (y2 - y1);
float C = r1 * r1 - r2 * r2 - x1 * x1 + x2 * x2 - y1 * y1 + y2 * y2;
float D = 2 * (x3 - x2);
float E = 2 * (y3 - y2);
float F = r2 * r2 - r3 * r3 - x2 * x2 + x3 * x3 - y2 * y2 + y3 * y3;
// Calculate position
result.x = (C * E - F * B) / (E * A - B * D);
result.y = (C * D - A * F) / (B * D - A * E);
cout << "final r1: " << r1 << " r2: " << r2 << " r3: " << r3 << endl;
return result;
}
return {-2, -2}; // return invalid position
}
void newRange()
{
uint16_t anchor_add = DW1000Ranging.getDistantDevice()->getShortAddress();
if (anchor_add == 0x0300)
a3 = 1;
if (anchor_add == 0x0200)
a2 = 1;
if (anchor_add == 0x0100)
a1 = 1;
if (a1 == 1 && a2 == 1 && a3 == 1)
{
Point3D anchor1 = {0.0, 0.0}; // First anchor at origin
Point3D anchor2 = {2.00, 0.0}; // Second anchor 5m along x-axis
Point3D anchor3 = {1.00, 1.73}; // Third anchor forming triangle
if (anchor_add == 0x0300)
{
r3 = DW1000Ranging.getDistantDevice()->getRange();
flag_a3 = true; // flag for anchor 3
}
if (anchor_add == 0x0200)
{
r2 = DW1000Ranging.getDistantDevice()->getRange();
flag_a2 = true; // flag for anchor 2
}
if (anchor_add == 0x0100)
{
r1 = DW1000Ranging.getDistantDevice()->getRange();
flag_a1 = true; // flag for anchor 1
}
if (flag_a1 == true && flag_a2 == true && flag_a3 == true)
{
Point3D tagPosition = calculatePosition(anchor1, r1, anchor2, r2, anchor3, r3);
if (tagPosition.x != -2 && tagPosition.y != -2)
{
printf("Tag Position: x=%0.2f, y=%0.2f\n", tagPosition.x, tagPosition.y);
char buffer[100];
sprintf(buffer, "x: %0.2f y: %0.2f", tagPosition.x, tagPosition.y);
oled_write(2, "Tag ");
oled_write(4, buffer);
}
flag_a1 = false;
flag_a2 = false;
flag_a3 = false;
}
}
}
void inactiveDevice(DW1000Device *device)
{
uint16_t addr = device->getShortAddress();
ESP_LOGI(TAG, “delete inactive device: %X”, addr);
// measurement_count = 0;
switch (addr)
{
case 0x0100:
a1 = false;
break;
case 0x0200:
a2 = false;
break;
case 0x0300:
a3 = false;
break;
default:
break;
}
oled_write(2, "Please Add Anchor ");
oled_write(4, " ");
if (!s1.empty())
s1.clear();
if (!s2.empty())
s2.clear();
if (!s3.empty())
s3.clear();
}
void newDevice(DW1000Device *device)
{
printf(“ranging init; 1 device added ! → short: %X\n”, device->getShortAddress());
add_link(uwb_data, device->getShortAddress());
}
void dw1000_loop_task(void *pvParameters)
{
while (true)
{
DW1000Ranging.loop();
vTaskDelay(pdMS_TO_TICKS(10));
}
vTaskDelete(NULL);
}
//--------------------------------------------------------------
#define I2C_MASTER_TX_BUF_DISABLE 0 /!< I2C master doesn’t need buffer /
#define I2C_MASTER_RX_BUF_DISABLE 0 /!< I2C master doesn’t need buffer /
#define WRITE_BIT I2C_MASTER_WRITE /!< I2C master write /
#define READ_BIT I2C_MASTER_READ /!< I2C master read /
#define ACK_CHECK_EN 0x1 /!< I2C master will check ack from slave/
#define ACK_CHECK_DIS 0x0 /*!< I2C master will not check ack from slave /
#define ACK_VAL 0x0 /!< I2C ack value */
#define NACK_VAL 0x1
#define I2C_FREQ 100000
#define I2C_PORT I2C_NUM_0
#define I2C_SDA GPIO_NUM_4
#define I2C_SCL GPIO_NUM_5
esp_err_t i2c_master_driver_initialize(void)
{
i2c_config_t conf = {
mode : I2C_MODE_MASTER,
sda_io_num : I2C_SDA,
scl_io_num : I2C_SCL,
sda_pullup_en : GPIO_PULLUP_ENABLE,
scl_pullup_en : GPIO_PULLUP_ENABLE,
master : {
clk_speed : I2C_FREQ
},
clk_flags : 0
};
i2c_param_config(I2C_PORT, &conf);
return i2c_driver_install(I2C_PORT, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}
//--------------------------------------------------------------
extern “C” void app_main(void)
{
esp_err_t err = nvs_flash_init();
if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND)
{
ESP_ERROR_CHECK(nvs_flash_erase());
err = nvs_flash_init();
}
ESP_ERROR_CHECK(err);
err = SPI_init();
if (err != ESP_OK)
{
ESP_LOGE(TAG, “spi_bus_initialize failed”);
return;
}
ESP_LOGI(TAG, “spi_bus_initialized successfully”);
err = i2c_master_driver_initialize();
if (err != ESP_OK)
{
ESP_LOGE(TAG, “i2c_master_driver_initialize failed”);
return;
}
ESP_LOGI(TAG, “i2c_master_driver_initialized successfully”);
oled_lcd_init();
oled_write(2, "Please Add Anchor ");
vTaskDelay(pdMS_TO_TICKS(50));
DW1000Ranging.initCommunication(UWB_RST, UWB_SS, UWB_IRQ); // Reset, CS, IRQ pin
DW1000.setAntennaDelay(ANTENA_DELAY);
DW1000Ranging.attachNewRange(newRange);
DW1000Ranging.attachNewDevice(newDevice);
DW1000Ranging.attachInactiveDevice(inactiveDevice);
DW1000Ranging.startAsTag(TAG_ADDR, DW1000.MODE_LONGDATA_FAST_ACCURACY, false);
uwb_data = init_link();
xTaskCreate(dw1000_loop_task, "dw1000_loop_task", 4096, NULL, configMAX_PRIORITIES - 1, NULL);
}