Custom location engine with the Gauss-Newton method

Greetings appreciable community of decaforum

I have seen some questions about how to implement our own location engine with the PANs R2 firmware, however, there is no official documentation on how to achieve it, for this reason I share the result of my research. Based on the “dwm-simple” project, we can follow the following steps:

  1. In the dwm.h file add the line
    extern signed int trilat_solve (const double * an_pos, double * meas, unsigned int meas_cnt, double * prev_pos_est);

The “trilat_solve” function is called by the location engine automatically when 3 or 4 anchors are detected, returns -1 in case of finding no solution and 0 in case of obtaining a solution, we declare it as an extern to overwrite it.

“const double * an_pos” is a pointer of the positions of the anchors in the order x0, y0, z0, x1, y1, z1, …, x3, y3, z3
“double * meas” is a pointer of the distances between the tag and each anchor d0, d1, …, d3
“unsigned int meas_cnt” indicates the number of anchors in list 3 or 4
“double * prev_pos_est” pointer with estimated position x, y, z

  1. Add a new file to the flxTrilat.h project with the following content:
#ifndef FLFTRILAT_H_
#define FLFTRILAT_H_

typedef struct Matrixx {
	int nrows;
	int ncols;
	double **data;
}MATRIX, *Matrix;

void freeMatrix(Matrix m);
Matrix createMatrix(unsigned int uiRows, unsigned int uiCols);
signed int optimise(Matrix x, double *y, double *b, int rows);

#endif /* FLFTRILAT_H_ */

The “Matrixx” structure represents the matrices that will be used to solve the multirateration problem by the Gauss-Newton method.

CreateMatrix” returns the pointer with the memory space reserved for the structure.
FreeMatrix” frees the array memory
Optimize” executes the Gauss-Newton optimization method, receives a matrix with the positions of the anchors, a pointer with the distances, a pointer with the initial values ​​and in the same the result, the number of equations to solve.

  1. Add a new file to the flxTrilat.c project with the following content, in this file the logic of the Gauss-Newton algorithm for multirateration is implemented:
#include "flxTrilat.h"
#include<stdio.h>
#include<stdlib.h>
#include <math.h>

void freeMatrix(Matrix m) {
  int i = 0;
  for (i = 0; i < m->nrows; i++) free(m->data[i]);
  free(m->data);
  free(m);
}
Matrix createMatrix(unsigned int uiRows, unsigned int uiCols){
  Matrix lpRet = (Matrix)malloc(sizeof(MATRIX));
  lpRet->ncols = uiCols;
  lpRet->nrows = uiRows;
  lpRet->data = (double**)malloc(sizeof(double*) * lpRet->nrows);
  for (int i = 0; i < lpRet->nrows; i++) {
    lpRet->data[i] = (double*)malloc(sizeof(double) * lpRet->ncols);
  }
  return lpRet;
}
void transpose(Matrix matrix, Matrix transposedMatrix) {
  for (int i = 0; i < matrix->nrows; i++) 
    for (int j = 0; j < matrix->ncols; j++) 
      transposedMatrix->data[j][i] = matrix->data[i][j];
}
void multiplyW(Matrix matrix1, Matrix matrix2, Matrix multipliedMatrix,double *w) {
  for (int i = 0; i < matrix1->nrows; i++) 
    for (int j = 0; j < matrix2->ncols; j++) {
      double sum = 0.0f;
      for (int k = 0; k < matrix1->ncols; k++) 
        sum += matrix1->data[i][k] * matrix2->data[k][j]*w[k];
      multipliedMatrix->data[i][j] = sum;
    }
}


void calculateResiduals(Matrix x, double *y, double *b, int rows, Matrix res) {
  for (int i = 0; i < rows; i++){
    double deltaX=b[0] - x->data[i][0];
    double deltaY=b[1] - x->data[i][1];
    double deltaZ=b[2] - x->data[i][2];
    res->data[i][0]=(deltaX*deltaX)+(deltaY*deltaY)+(deltaZ*deltaZ)-(y[i]*y[i]);
  }
}
void jacob(double* b, Matrix x, int rows, Matrix jc) {
  for (int i = 0; i < rows; i++) {
    jc->data[i][0] = 2 * b[0] - 2 * x->data[i][0];
    jc->data[i][1] = 2 * b[1] - 2 * x->data[i][1];
    jc->data[i][2] = 2 * b[2] - 2 * x->data[i][2];
  }
}
double calculateErrorChi(Matrix res,double *w) {
  double sum = 0;
  for (int i = 0; i < res->nrows; i++) {
    sum += (res->data[i][0] * res->data[i][0]*w[i]);
  }
  return sqrt(sum);
}
//3x4
void gauss34(Matrix m) {
  double x = -m->data[0][0];
  double y = m->data[1][0];
  m->data[1][0] = m->data[1][0] / y * x + m->data[0][0];
  m->data[1][1] = m->data[1][1] / y * x + m->data[0][1];
  m->data[1][2] = m->data[1][2] / y * x + m->data[0][2];
  m->data[1][3] = m->data[1][3] / y * x + m->data[0][3];
  x = -m->data[0][0];
  y = m->data[2][0];
  m->data[2][0] = m->data[2][0] / y * x + m->data[0][0];
  m->data[2][1] = m->data[2][1] / y * x + m->data[0][1];
  m->data[2][2] = m->data[2][2] / y * x + m->data[0][2];
  m->data[2][3] = m->data[2][3] / y * x + m->data[0][3];    
  x = -m->data[1][1];
  y = m->data[0][1];
  m->data[0][0] = m->data[0][0] / y * x + m->data[1][0];
  m->data[0][1] = m->data[0][1] / y * x + m->data[1][1];
  m->data[0][2] = m->data[0][2] / y * x + m->data[1][2];
  m->data[0][3] = m->data[0][3] / y * x + m->data[1][3];    
  x = -m->data[1][1];
  y = m->data[2][1];
  m->data[2][0] = m->data[2][0] / y * x + m->data[1][0];
  m->data[2][1] = m->data[2][1] / y * x + m->data[1][1];
  m->data[2][2] = m->data[2][2] / y * x + m->data[1][2];
  m->data[2][3] = m->data[2][3] / y * x + m->data[1][3];    
  x = -m->data[2][2];
  y = m->data[0][2];
  m->data[0][0] = m->data[0][0] / y * x + m->data[2][0];
  m->data[0][1] = m->data[0][1] / y * x + m->data[2][1];
  m->data[0][2] = m->data[0][2] / y * x + m->data[2][2];
  m->data[0][3] = m->data[0][3] / y * x + m->data[2][3];    
  x = -m->data[2][2];
  y = m->data[1][2];
  m->data[1][0] = m->data[1][0] / y * x + m->data[2][0];
  m->data[1][1] = m->data[1][1] / y * x + m->data[2][1];
  m->data[1][2] = m->data[1][2] / y * x + m->data[2][2];
  m->data[1][3] = m->data[1][3] / y * x + m->data[2][3];    
}
signed int optimise(Matrix x, double *y, double *b2, int rows) {
  int maxIteration = 100;
  double precision = 0.5;
  double *w = (double*)malloc(rows*sizeof(double));
  signed int response=-1;
  for (int i = 0; i < rows; i++) {
    w[i] = 1/pow(y[i],2);
  }

  Matrix Wr = createMatrix(rows, 1);
  Matrix J = createMatrix(rows, 3);
  Matrix JT = createMatrix(3, rows);
  Matrix JTWJ = createMatrix(3, 4);
  Matrix JTWr = createMatrix(3, 1);

  for (int i = 0; i < maxIteration; i++) {
    calculateResiduals(x, y, b2, rows, Wr);
    double error = calculateErrorChi(Wr,w);

    //printf("Iteration : %d , Error-diff: %f , b = %f, %f, %f\n", i, (fabs( error)), b2[0], b2[1], b2[2]);
    if (fabs( error) <= precision) {
      response=i;
      break;
    }
    jacob(b2, x, rows, J);
    
    transpose(J, JT); // JT(3x4)
    JTWJ->ncols = 3;
    multiplyW(JT, J, JTWJ, w); // (JT(3x4) * WJ(4x3))(3x3)
    JTWJ->ncols = 4;
    multiplyW(JT, Wr, JTWr,w); // JTWr(3x1)
    JTWJ->data[0][3] = JTWr->data[0][0];
    JTWJ->data[1][3] = JTWr->data[1][0];
    JTWJ->data[2][3] = JTWr->data[2][0];

    gauss34(JTWJ);
    b2[0] = b2[0] - JTWJ->data[0][3]/ JTWJ->data[0][0];
    b2[1] = b2[1] - JTWJ->data[1][3]/ JTWJ->data[1][1];
    b2[2] = b2[2] - JTWJ->data[2][3]/ JTWJ->data[2][2];
		
  }
  freeMatrix(J);
  freeMatrix(JT);
  freeMatrix(JTWJ);
  freeMatrix(Wr);
  freeMatrix(JTWr);
  free(w);
  return response;         
}
  1. In the dwm-simple.c file implement the trilat_solve function:
extern signed int trilat_solve(const double *an_pos, double *meas, unsigned int meas_cnt, double *prev_pos_est){
  signed int i,j;
  int off;
  Matrix points = createMatrix(meas_cnt, 3);
  prev_pos_est[0] = 0;
  prev_pos_est[1] = 0;
  prev_pos_est[2] = 0;
  for(i=0;i<meas_cnt;i++){
    off=i*3;
    points->data[i][0] = an_pos[off];
    points->data[i][1] = an_pos[off+1];
    points->data[i][2] = an_pos[off+2];

    prev_pos_est[0]+=points->data[i][0];
    prev_pos_est[1]+=points->data[i][1];
    prev_pos_est[2]+=points->data[i][2];
  }

  prev_pos_est[0] /= meas_cnt;
  prev_pos_est[1] /= meas_cnt;
  prev_pos_est[2] /= meas_cnt;
	
  i=optimise(points, meas, prev_pos_est, meas_cnt);
  freeMatrix(points);
  return i;
}

This will allow us to add filters or whatever we want in the future. The result is 100% compatible with the PANs R2 firmware, the laboratory results show a significant improvement in the Z axis, however there are still field tests.

Note:
This is not official decawave information so the “trilat_solve” function may receive more parameters not described in this entry.

I hope this information is helpful to the community.

best regards

13 Likes

Greetings to Fdiaz.
Big respect to you for the interesting work done.
I experimented with your location mechanism. And I had a couple of questions requiring your clarification:
When building a system of 4 anchors, 1 bridge and 1 tag with your location mechanism, on the bridge with the “les” command, I observe the coordinates of the tag calculated by your location mechanism or from the built-in TWR?
Why is the quality factor always equal to 0 in the data received in the bridge from the tag?

[004335.970 INF] loc_data: 1
0) 1CA5[nan,nan,nan,0,x05]

[004336.980 INF] loc_data: 1
0) 1CA5[0.45,0.09,0.01,0,x05]

[004337.980 INF] loc_data: 1
0) 1CA5[nan,nan,nan,0,x05]

Thanks.
With respect

Hello, I am very pleased to see interest in my research.
I will gladly clarify your doubts.

trilat_solve is an built-in function, when setting it as external obliges the compiler to overwrite it, so the function used is the one we declare. (This is not official information, it is the product of my investigation)

I did not implement the quality factor, so it always turns out 0. :sweat_smile:

in the optimise method you can adjust the variables maxIteration and/or precision in addition to uncomment line printf ("Iteration:% d, … to observe the behavior of the method. I personally suggest values greater than 1 for precision, this impacts the time required to find solution.

I have more information about the operation of the system PANs, however I do not publish everything because it can be confusing. if you have any other questions don’t have the confidence to ask.

regards

Hello Fdiaz.
Thank you for clarification, they are very useful for me.
As for the quality factor, I do not use it in the analysis. For me, more important is the user information about the battery charge or from the sensor, which I can get from the tag.
In the PANS 2 release, it is possible to transfer user data via IoT, but this is only using Raspberi PI, and I use the STM-based controller to create the bridge. I have been looking for a solution for transferring user data without using Raspberi PI for a long time.
The most optimal solution for me is to replace the quality factor and the TWR slot number with my data in the data packet transmitted by the tag, but I did not find a way to solve this.

[004336.980 INF] loc_data: 1
0) 1CA5[0.45,0.09,0.01, 0 ,x05]

Perhaps you can help me with this. I would be grateful for any information.

With respect

Hi
yes it is possible to use the quality factor to send the battery status, only the data is an 8-bit unsigned integer pointer type, so I would recommend sending a percentage.

the TWR slot is not a data that we can manipulate.

To make use of the quality factor you must declare the trilat_solve function in the following way:

extern signed int trilat_solve (const double * an_pos , double * meas , unsigned int meas_cnt , double * prev_pos_est , uint8_t *quality );

where quality is the pointer of a uint8_t

I hope this information helps you.

regards

2 Likes

Hello .
Thank you so much for the info. I planned to transfer battery charge data as a percentage. I will implement. I will be happy to help you as part of my competence. Thanks a lot again.
With respect

Hello Fdiaz.
I declared the function extern signed int trilat_solve (const double * an_pos, double * meas, unsigned int meas_cnt, double * prev_pos_est, uint8_t * quality);
in the dwm.h file, in the dwm-simple.c file in this function I tried to transfer a fixed quality value in the uint8_t format, but the quality value in the data packet remains 0.
Tell me what could be my error, which parameter should i return.
Thanks.
With respect

hello sorry the delay
Can you share with me how are you doing the assignment?

extern signed int trilat_solve(const double *an_pos, double *meas, unsigned int meas_cnt, double *prev_pos_est, uint8_t *quality){
*quality=100;
.
.
.
return 0;
}

Hello Fdiaz.
Thank you so much for your help.
Due to inattention, I missed the * character in the code.

extern signed int trilat_solve(const double *an_pos, double *meas, unsigned int meas_cnt, double *prev_pos_est, uint8_t *quality ){
signed int i,j;
int off;
.
.
.
freeMatrix(points);
quality = 33;
return i;
}

Thank you, now the value of the quality factor is changing.

Do you mean the implementation of battery monitoring?

I have one more question for you: Have you considered expanding the functions of the positioning anchor using BLE as iBeacon so that positioning can be carried out using not only tags, but also on mobile devices?

Thanks again.
With respect

Hi 555dragon

I’m glad to hear that you solved the problem.

I honestly have not had the need to raise this scenario, since my clients are only interested in uwb, however I could do an implementation of iBeacon without problem.

if necessary I could make a firmware for your needs.

regards

Hi Fdiaz
I made an account just to thank you for sharing your work. We are currently in the middle of developing an RTLS system for on-stage application. (automatic followspots, generative visuals and much more). If any of you are interested we will most likely make some parts of the software Open Source in this year. As it stands right now we are building on the Pans library so it should be easy to implement for anyone. I’m looking forward to bringing reality and software closer thus opening a new way of creative content on the stage.

Hey Fdiaz, first thanks so much for sharing your improved LE.

Though, I wonder how do you receive your position data now? For my use case it is necessary to send the location data via Bluetooth. I’ve already set up a stable Bluetooth connection and am able to receive position data on my device with the on-board LE, but I’d like to receive the customized LE data via Bluetooth instead. Is that possible?

I am a little confused as
a) the API Guide states on page 32 in section 3.5.2.3 that “data offload is only possible via IoT data” while
b) the developer can choose either between using the on-board LE or a customized one as for example stated in the API Guide on page 36 in section 4.4.7 “loc_engine_en”.

Thanks for your help,
Max

Hello
I can gladly contribute.
You tell me how I can help you.

regards

hi Max
I am pleased to see interest in my contribution.
The code I shared what it does is replace the default LE with a personalized one. this means that all other functions continue to work according to PANs.
If you load the code, it transmits the location results by BLE and by UWB just as the LE default does, in addition to being compatible with the gateway.

everything works due to callback, that is why it is not clear when it is sent.

I deciphered this after studying the behavior of the entire PANs system (tag, anchor and gateway), for lack of official documentation, however it works well.

regards

Hey Fdiaz,
thanks for this information! It saves my day.

By overriding the default LE does that mean that I should tell the tag to still use its internal LE or do I have to change that value?

And just because I am curious about how your solution works, only by overriding ‘trilat_solve’ we override the internal LE, right? How did you come up with that function name though, since it is nowhere documented? That’s amazing work!

Regards, Max

Thanks for you reply.
Tomorrow i will load your changes to a second tag and try out how it compares to the factorí Location Engine. Right now we are working on the main software that will eventually handle all the needed processing on the data.

is correct, we must keep the value of LE = 1
I had to check the binaries and interpret the functions, it is certainly very difficult because you have to translate from binary to assembler, then identify the functions, analyze the parameters that it receives and returns, the type of data (pointer, structure or primitive).

honestly on occasions I thought about aborting the project, but I ended it for personal challenge, I decided to share it when I saw the lack of information about it by the manufacturer.

I also managed to decipher the operation of the gateway, in fact I made one written in java and with its kernel module for an orange pi H3, in the not too distant future I will share it.

regards

1 Like

You’re a legend! Big ups :clap:t5:

In the meantime i have spoken with my software colleague and we were thinking about something that maybe you could help us with.
We would like to bypass the internal location engine and only pass on unfiltered data from the node.
This way we could filter everything in our own software and potentially find out the weakpoints in the system.
If you have any idea how we could achive this i would like to hear about it :slight_smile: Thanks in advance.

Hi,

I added your mentioned files. In my project, “dwm-simple.c” and “flxTrilat.c” include “flxTrilat.h”. “flxTrilat.c” implements the methods declared in “flxTrilat.h” and “dwm-simple.c” should simply call these methods. Unfortunately, I cannot build the project. While linking “dwm-simple_fw2.elf” errors occur, saying there are undefined references to the functions declared in “flxTrilat.h” (createMatrix, optimise and freeMatrix).

I’ve already manipulated the user include directory in the solution options but this didn’t fix the problem.

However, in my output folder there is “dwm-simple.o” and “flxTrilat.o”. For me it seems that the linker is not linking these two files. Is that correct? How do I change that?

Excuse me, I am very new to C.
Thanks, Max


EDIT:
Instead of including “flxTrilat.h”, I simply include “flxTrilat.c” in “dwm-simple.c”. This let me build the project but now no position can be estimated. The output of “optimise” is always -1. That indicates that in “flxTrilat.c” in line 130 the condition is never true.

If I uncomment the print statement one line above in line 129, only one iteration succeeds, giving me an error of around 5. The parameters included in “b” are always the same (the last one being the z coordinate of my anchors…).
Then, the remaining 99 iterations simply output “nan” for every parameter.
What did I do wrong?

Here’s a screenshof of the print statement: