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:
- 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
- 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.
- 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;
}
- 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
Thanks in advance.