Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
trilateration.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
27 #include "trilateration.h"
29 
30 // base original locations
31 static float P[3][3];
32 // base unit vector on x, y and z axis
33 static float Ex[3], Ey[3], Ez[3];
34 // base inter distances
35 static float D, I, J;
36 
38 
39 int trilateration_init(struct Anchor *anchors)
40 {
41  float tmp[3], n;
42  init_failed = false;
43 
44  // store original points
45  for (int i = 0; i < 3; i++) {
46  P[i][0] = anchors[i].pos.x;
47  P[i][1] = anchors[i].pos.y;
48  P[i][2] = anchors[i].pos.z;
49  }
50  // Ex = (P1 - P0) / ||P1 - P0||
51  float_vect_diff(tmp, P[1], P[0], 3);
52  n = float_vect_norm(tmp, 3);
53  if (n > 0.f) {
54  float_vect_sdiv(Ex, tmp, n, 3);
55  } else {
56  init_failed = true;
57  return -1;
58  }
59  // I = Ex . (P2 - P0)
60  float_vect_diff(tmp, P[2], P[0], 3);
61  I = float_vect_dot_product(Ex, tmp, 3);
62  // Ey = (P2 - P0 - I.Ex) / ||P2 - P0 - I.Ex||
63  float_vect_smul(tmp, Ex, -I, 3);
64  float_vect_diff(tmp, tmp, P[0], 3);
65  float_vect_add(tmp, P[2], 3);
66  n = float_vect_norm(tmp, 3);
67  if (n > 0.f) {
68  float_vect_sdiv(Ey, tmp, n, 3);
69  } else {
70  init_failed = true;
71  return -1;
72  }
73  // Ez = Ex x Ey
74  Ez[0] = Ex[1]*Ey[2] - Ex[2]*Ey[1];
75  Ez[1] = Ex[2]*Ey[0] - Ex[0]*Ey[2];
76  Ez[2] = Ex[0]*Ey[1] - Ex[1]*Ey[0];
77  // D = ||P1 - P0||
78  float_vect_diff(tmp, P[1], P[0], 3);
79  D = float_vect_norm(tmp, 3);
80  // J = Ey . (P2 - P0)
81  float_vect_diff(tmp, P[2], P[0], 3);
82  J = float_vect_dot_product(Ey, tmp, 3);
83 
84  return 0;
85 }
86 
87 int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
88 {
89  if (init_failed) {
90  return -1;
91  }
92  const float r02 = anchors[0].distance * anchors[0].distance;
93  const float r12 = anchors[1].distance * anchors[1].distance;
94  const float r22 = anchors[2].distance * anchors[2].distance;
95  const float d2 = D * D;
96  const float i2 = I * I;
97  const float j2 = J * J;
98  float tmp[3];
99  tmp[0] = (r02 - r12 + d2) / (2.f * D);
100  tmp[1] = (r02 - r22 + i2 + j2) / (2.f * J) - ((I * tmp[0]) / J);
101  const float d0 = r02 - (tmp[0] * tmp[0]) - (tmp[1] * tmp[1]);
102  if (d0 < 0.f) {
103  // impossible solution
104  // might happen if position of the anchors are not correct
105  // or if reported distances are completely wrong
106  return -1;
107  }
108  tmp[2] = sqrtf(d0); // only keep positive value
109  // restore original frame
110  pos->x = P[0][0] + tmp[0] * Ex[0] + tmp[1] * Ey[0] + tmp[2] * Ez[0];
111  pos->y = P[0][1] + tmp[0] * Ex[1] + tmp[1] * Ey[1] + tmp[2] * Ez[1];
112  pos->z = P[0][2] + tmp[0] * Ex[2] + tmp[1] * Ey[2] + tmp[2] * Ez[2];
113  pos->z = fabsf(pos->z); // in case the base is not matching, keep positive z
114  return 0;
115 }
116 
static uint16_t d2
Definition: baro_MS5534A.c:202
static float float_vect_norm(const float *a, const int n)
||a||
int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
Compute trilateration based on the latest measurments.
Definition: trilateration.c:87
static void float_vect_add(float *a, const float *b, const int n)
a += b
vector in East North Up coordinates Units: meters
static float Ey[3]
Definition: trilateration.c:33
static float P[3][3]
Definition: trilateration.c:31
static float I
Definition: trilateration.c:35
int trilateration_init(struct Anchor *anchors)
Init internal trilateration structures.
Definition: trilateration.c:39
static void float_vect_sdiv(float *o, const float *a, const float s, const int n)
o = a / s
static float D
Definition: trilateration.c:35
static float J
Definition: trilateration.c:35
Paparazzi floating point algebra.
float x
in meters
static float Ez[3]
Definition: trilateration.c:33
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
static float Ex[3]
Definition: trilateration.c:33
float distance
last measured distance
Definition: trilateration.h:35
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
Anchor structure.
Definition: trilateration.h:34
float z
in meters
struct EnuCoor_f pos
position of the anchor
Definition: trilateration.h:37
float y
in meters
static float float_vect_dot_product(const float *a, const float *b, const int n)
a.b
bool init_failed
Definition: trilateration.c:37