Navigine Open Source Code of Conduct
2 | 3 | This code of conduct provides guidance on participation in Navigine-managed open source communities and denotes the process for reporting unacceptable behavior. We are committed to providing a welcoming and inspiring community for all. People violating this code of conduct may be banned from the community. 4 | 5 | ## Our Standards 6 | 7 | Examples of behavior that contributes to a positive environment for our community include: 8 | - Demonstrating empathy and kindness toward other people 9 | - Being respectful of differing opinions, viewpoints, and experiences 10 | - Giving and gracefully accepting constructive feedback 11 | - Accepting responsibility and apologizing to those affected by our mistakes, and learning from the experience 12 | - Focusing on what is best not just for us as individuals, but for the overall community 13 | 14 | Examples of unacceptable behavior include: 15 | - The use of sexualized language or imagery, and sexual attention or advances of any kind 16 | - Trolling, insulting or derogatory comments, and personal or political attacks 17 | - Public or private harassment 18 | - Publishing others’ private information, such as a physical or email address, without their explicit permission 19 | - Other conduct which could reasonably be considered inappropriate in a professional setting 20 | - Advocating for or encouraging any of the above behaviors 21 | 22 | ## Enforcement Responsibilities 23 | 24 | Community leaders are responsible for clarifying and enforcing our standards of acceptable behavior and will take appropriate and fair corrective action in response to any behavior that they deem inappropriate, threatening, offensive, or harmful. 25 | Community leaders have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, and will communicate reasons for moderation decisions when appropriate. 26 | 27 | ## Scope 28 | 29 | This Code of Conduct applies within all community spaces, and also applies when an individual is officially representing the community in public spaces. Examples of representing our community include using an official e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event. 30 | 31 | ## Enforcement 32 | 33 | Instances of abusive, harassing, or otherwise unacceptable behavior may be reported to the community leaders responsible for enforcement at info@navigine.com. All complaints will be reviewed and investigated promptly and fairly. 34 | All community leaders are obligated to respect the privacy and security of the reporter of any incident. 35 | 36 | ## Attribution 37 | 38 | This Code of Conduct is adapted from the Contributor Covenant, version 2.1, available at https://www.contributor-covenant.org/version/2/1/code_of_conduct.html. 39 | For answers to common questions about this code of conduct, see the FAQ at https://www.contributor-covenant.org/faq. Translations are available at https://www.contributor-covenant.org/translations. 40 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | #Contributing to Navigine Routing Library
2 | 3 | The easiest way to help out is to submit bug reports and feature requests on our [issues](https://github.com/Navigine/Indoor-Routing-Library/issues) page. 4 | 5 | When submitting a bug report, please include: 6 | 7 | - The device and operating system version that produced the bug 8 | - The version or commit of Routing Library that produced the bug 9 | - Steps required to recreate the issue 10 | - What happened 11 | - What you expected to happen 12 | 13 | Thanks! 14 | 15 | ## Development 16 | 17 | To develop for the library, it is usually easiest to build and test your changes using either the Mac OS X or Ubuntu desktop targets. 18 | 19 | ## Code Style 20 | 21 | In general, code changes should follow the style of the surrounding code. 22 | 23 | When in doubt, you can use the provided clang-format style file for automatic styling. 24 | 25 | Install clang-format (available through brew or apt-get): 26 | ``` 27 | brew install clang-format 28 | ``` 29 | or 30 | ``` 31 | sudo apt-get install clang-format 32 | ``` 33 | 34 | Run clang-format with specified style (use -i to modify the contents of the specified file): 35 | ``` 36 | clang-format -i -style=file [file] 37 | ``` 38 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Navigine 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This demo project shows how using raw IQ samples obtained from the Minew Aoa Locator, you can get the azimuth and elevation angles at which the beacon is located. Using these angles, we also get the position of the beacon in XY coordinates (knowing the location of the locator in the same coordinate system). This project is based on our [AoA article](https://navigine.com/blog/using-angle-of-arrival-for-direction-finding-with-bluetooth/) in which we determined the azimuth angle using 2 adjacent elements of the linear antenna array of the locator. 2 | 3 | ## Getting started 4 | 5 | To start the project, you need to submit raw IQ samples to the algorithm input: 6 | 7 | ``` 8 | cd src/ 9 | cat ../data/static.json | python3 aoa_demo.py 10 | ``` 11 | 12 | At the output, we get the calculated angles and coordinates: 13 | 14 | ``` 15 | x_beacon:9.232542418733246, y_beacon:5.490652286174913 16 | azimuth_angle:43.0, elevation_angle:-17.0 17 | x_beacon:9.25956555622026, y_beacon:5.517626370851988 18 | azimuth_angle:42.0, elevation_angle:-19.0 19 | x_beacon:9.089259754493328, y_beacon:5.665581413837454 20 | azimuth_angle:46.0, elevation_angle:-15.0 21 | x_beacon:9.533823456356725, y_beacon:5.396157120924992 22 | azimuth_angle:46.0, elevation_angle:-14.0 23 | x_beacon:9.653014740958174, y_beacon:5.272770867998707 24 | azimuth_angle:44.0, elevation_angle:-16.0 25 | x_beacon:9.374720054161077, y_beacon:5.44537787620972 26 | azimuth_angle:45.0, elevation_angle:-16.0 27 | x_beacon:9.402449692489892, y_beacon:5.47492352570238 28 | azimuth_angle:45.0, elevation_angle:-16.0 29 | x_beacon:9.402449692489892, y_beacon:5.47492352570238 30 | azimuth_angle:42.0, elevation_angle:-18.0 31 | x_beacon:9.156849906593791, y_beacon:5.5825730485361875 32 | azimuth_angle:46.0, elevation_angle:-14.0 33 | x_beacon:9.653014740958174, y_beacon:5.272770867998707 34 | azimuth_angle:44.0, elevation_angle:-15.0 35 | x_beacon:9.475271358924696, y_beacon:5.333235601052133 36 | ... 37 | ``` 38 | 39 | The beacon was located relative to the locator as follows: 40 | 41 |
42 |
43 | Locator settings:
44 |
45 | 
46 |
47 | The accuracy of the angles obtained corresponds to the accuracy stated in the article, where measurements were taken from many different points. In other words, it is no more than 6 degrees.
48 |
49 | ## Problems in calculating the AoA
50 |
51 | In the stationary test, everything is fine, but if you carry out the following experiment: first the beacon lies in place, then it moves a little from side to side, and at the end - it lies in the same place again, then a strange effect is observed: during this small movement, a constant offset by some angle. This movement is expected to be close to stationary and therefore the angle should be approximately the same, but this is not the case.
52 |
53 | In support of my words, I attach a video of the experiment:
54 |
55 |
56 | https://user-images.githubusercontent.com/30315050/143232495-941c00ad-b561-448f-9a1e-35b51a915432.MOV
57 |
58 |
59 | And a figure of the change in azimuth phases:
60 |
61 | 
62 |
63 | The input data is located in the file: `dynamic.json`
64 |
--------------------------------------------------------------------------------
/img/dynamic_azimuth_phases.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm/e5fcd88dbbc7af3659cc48fe95573b52ee7a29c1/img/dynamic_azimuth_phases.jpg
--------------------------------------------------------------------------------
/img/location.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm/e5fcd88dbbc7af3659cc48fe95573b52ee7a29c1/img/location.jpg
--------------------------------------------------------------------------------
/img/locator_settings.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm/e5fcd88dbbc7af3659cc48fe95573b52ee7a29c1/img/locator_settings.jpg
--------------------------------------------------------------------------------
/src/aoa_demo.py:
--------------------------------------------------------------------------------
1 | import math
2 | import sys
3 | import json
4 | from pyargus.directionEstimation import *
5 | from scipy.spatial import distance
6 | from collections import deque
7 | import numpy as np
8 |
9 | N_SAMPLES_OF_REF_PERIOD = 8
10 | NUMBER_MESSAGES = 1 # data accumulation window
11 | SIGMA_FILTER_WINDOW = 5
12 |
13 | # map parameters
14 | width_map = 14.7
15 | height_map = 12.4
16 | kx_locator = 0.532471
17 | ky_locator = 0.558504
18 | x_locator = kx_locator * width_map
19 | y_locator = ky_locator * height_map
20 | z_locator = 0.2
21 | z_beacon = 0.814
22 |
23 | # antenna array parameters
24 | frequency = 2480
25 | wavelength = 0.12
26 | d = 0.05 # inter element spacing
27 | M = 2 # number of antenna elements in the antenna system
28 |
29 |
30 | # 68–95–99.7 rule for identify and discard positional outliers
31 | class SigmaFilter:
32 | def __init__(self, maxlen = SIGMA_FILTER_WINDOW):
33 | self.deque = deque(maxlen = maxlen)
34 |
35 | def isValid(self, v):
36 | isValid = False
37 | if len(self.deque) == SIGMA_FILTER_WINDOW:
38 | mean_v, std_v = np.array(self.deque).mean(axis=0), np.array(self.deque).std(axis=0)
39 | if distance.euclidean(v, mean_v) < distance.euclidean(mean_v + 3 * std_v, mean_v):
40 | isValid = True
41 | else:
42 | print('outlier:', v)
43 |
44 | self.deque.append(v)
45 | return isValid
46 |
47 |
48 | def to_plus_minus_pi(angle):
49 | while angle >= 180:
50 | angle -= 2 * 180
51 | while angle < -180:
52 | angle += 2 * 180
53 | return angle
54 |
55 |
56 | def get_angle(X):
57 | # Estimating the spatial correlation matrix
58 | R = corr_matrix_estimate(X.T, imp="fast")
59 |
60 | array_alignment = np.arange(0, M, 1) * d
61 | incident_angles = np.arange(-90, 91, 1)
62 | scanning_vectors = np.zeros((M, np.size(incident_angles)), dtype=complex)
63 | for i in range(np.size(incident_angles)):
64 | scanning_vectors[:, i] = np.exp(
65 | array_alignment * 1j * 2 * np.pi * np.sin(np.radians(incident_angles[i])) / wavelength) # scanning vector
66 |
67 | ula_scanning_vectors = scanning_vectors
68 |
69 | # Estimate DOA
70 | MUSIC = DOA_MUSIC(R, ula_scanning_vectors, signal_dimension=1)
71 | norm_data = np.divide(np.abs(MUSIC), np.max(np.abs(MUSIC)))
72 | return float(incident_angles[np.where(norm_data == 1)[0]][0])
73 |
74 |
75 | def get_coordinate(azimuth, elevation, height, receiver_coords):
76 | nx = np.cos(np.deg2rad(90.0 - azimuth))
77 | nz = np.cos(np.deg2rad(90.0 - abs(elevation)))
78 | if math.isclose(nx, 0.0, abs_tol=1e-16) or math.isclose(nz, 0.0, abs_tol=1e-16):
79 | return [float("nan"), float("nan")]
80 | else:
81 | ny = np.sqrt(1 - nx ** 2 - nz ** 2)
82 | t = (height - receiver_coords[2]) / nz
83 | x = receiver_coords[0] + t * nx
84 | y = receiver_coords[1] - t * ny
85 | return [x, y]
86 |
87 |
88 | if __name__ == '__main__':
89 | velSigmaFilter = SigmaFilter(SIGMA_FILTER_WINDOW)
90 | messages = []
91 | for line in sys.stdin:
92 | data = json.loads(line)
93 | try:
94 | aoa_data = json.loads(data['message'])
95 | except BaseException as e:
96 | # print(e)
97 | continue
98 |
99 | try:
100 | for i in aoa_data[1:]:
101 | if i['aoa']['frequency'] == frequency:
102 | messages.append(i)
103 | except BaseException as e:
104 | # print(e)
105 | pass
106 |
107 | if len(messages) < NUMBER_MESSAGES:
108 | continue
109 |
110 | x_00, azimuth_x_12, elevation_x_12 = [], [], []
111 | azimuth_phases, elevation_phases = [], []
112 | for i in messages:
113 | ref_phases = []
114 | iq_samples = [i['aoa']['iq'][n:n + 2] for n in range(0, len(i['aoa']['iq']), 2)]
115 |
116 | for iq_idx in range(N_SAMPLES_OF_REF_PERIOD - 1):
117 | iq_next = complex(iq_samples[iq_idx + 1][0], iq_samples[iq_idx + 1][1])
118 | iq_cur = complex(iq_samples[iq_idx][0], iq_samples[iq_idx][1])
119 | phase_next = np.rad2deg(np.arctan2(iq_next.imag, iq_next.real))
120 | phase_cur = np.rad2deg(np.arctan2(iq_cur.imag, iq_cur.real))
121 | ref_phases.append((to_plus_minus_pi(phase_next - phase_cur)))
122 | phase_ref = np.mean(ref_phases)
123 |
124 | iq_2ant_batches = [iq_samples[n:n + 2] for n in range(N_SAMPLES_OF_REF_PERIOD, len(iq_samples), 2)]
125 | for iq_batch_idx, iq_batch in enumerate(iq_2ant_batches[:-1]):
126 | iq_next = complex(iq_batch[1][0], iq_batch[1][1])
127 | iq_cur = complex(iq_batch[0][0], iq_batch[0][1])
128 | phase_next = np.rad2deg(np.arctan2(iq_next.imag, iq_next.real))
129 | phase_cur = np.rad2deg(np.arctan2(iq_cur.imag, iq_cur.real))
130 | diff_phase = to_plus_minus_pi((phase_next - phase_cur) - 2 * phase_ref)
131 | if iq_batch_idx % 2 != 0:
132 | elevation_phases.append(diff_phase)
133 | else:
134 | x_00.append(1)
135 | azimuth_phases.append(diff_phase)
136 | messages.clear()
137 | # MUSIC algo
138 | X = np.zeros((M, np.size(x_00)), dtype=complex)
139 | X[0, :] = x_00
140 | for i in azimuth_phases:
141 | azimuth_x_12.append(np.exp(1j * np.deg2rad(i)))
142 | X[1, :] = azimuth_x_12
143 | azimuth_angle = get_angle(X)
144 |
145 | for i in elevation_phases:
146 | elevation_x_12.append(np.exp(1j * np.deg2rad(i)))
147 | X[1, :] = elevation_x_12
148 | elevation_angle = get_angle(X)
149 | print(f'azimuth_angle:{azimuth_angle}, elevation_angle:{elevation_angle}')
150 |
151 | xy = get_coordinate(azimuth_angle, elevation_angle, z_beacon, [x_locator, y_locator, z_locator])
152 | if not math.isnan(xy[0]) and not math.isnan(xy[1]):
153 | if velSigmaFilter.isValid(xy):
154 | print(f'x_beacon:{xy[0]}, y_beacon:{xy[1]}')
155 |
--------------------------------------------------------------------------------