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 | -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | #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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |
3 | # Direction-of-Arrival-DoA-Estimation-Algorithm
4 |
5 | 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.
6 |
7 | ### Useful Navigine Links
8 |
9 | - [Navigine Official Website](https://navigine.com/) – Learn more about our technology, products, and services.
10 | - [User Manual](http://docs.navigine.com/) – Full product usage guidelines and SDK documentation.
11 | - [Get Started with Navigine](http://locations.navigine.com/login) – Create your account and begin building indoor navigation solutions.
12 | - [Discuss the project](https://community.navigine.com/t/direction-of-arrival-doa-estimation-algorithm/174) – Join the community thread for questions, feedback, and collaboration specific to this repository.
13 | - [Contact Navigine](https://navigine.com/contacts/) – Reach out for business or technical support.
14 |
15 | ## Getting started
16 |
17 | To start the project, you need to submit raw IQ samples to the algorithm input:
18 |
19 | ```
20 | cd src/
21 | cat ../data/static.json | python3 aoa_demo.py
22 | ```
23 |
24 | At the output, we get the calculated angles and coordinates:
25 |
26 | ```
27 | x_beacon:9.232542418733246, y_beacon:5.490652286174913
28 | azimuth_angle:43.0, elevation_angle:-17.0
29 | x_beacon:9.25956555622026, y_beacon:5.517626370851988
30 | azimuth_angle:42.0, elevation_angle:-19.0
31 | x_beacon:9.089259754493328, y_beacon:5.665581413837454
32 | azimuth_angle:46.0, elevation_angle:-15.0
33 | x_beacon:9.533823456356725, y_beacon:5.396157120924992
34 | azimuth_angle:46.0, elevation_angle:-14.0
35 | x_beacon:9.653014740958174, y_beacon:5.272770867998707
36 | azimuth_angle:44.0, elevation_angle:-16.0
37 | x_beacon:9.374720054161077, y_beacon:5.44537787620972
38 | azimuth_angle:45.0, elevation_angle:-16.0
39 | x_beacon:9.402449692489892, y_beacon:5.47492352570238
40 | azimuth_angle:45.0, elevation_angle:-16.0
41 | x_beacon:9.402449692489892, y_beacon:5.47492352570238
42 | azimuth_angle:42.0, elevation_angle:-18.0
43 | x_beacon:9.156849906593791, y_beacon:5.5825730485361875
44 | azimuth_angle:46.0, elevation_angle:-14.0
45 | x_beacon:9.653014740958174, y_beacon:5.272770867998707
46 | azimuth_angle:44.0, elevation_angle:-15.0
47 | x_beacon:9.475271358924696, y_beacon:5.333235601052133
48 | ...
49 | ```
50 |
51 | The beacon was located relative to the locator as follows:
52 |
53 | 
54 |
55 | Locator settings:
56 |
57 | 
58 |
59 | 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.
60 |
61 | ## Problems in calculating the AoA
62 |
63 | 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.
64 |
65 | In support of my words, I attach a video of the experiment:
66 |
67 |
68 | https://user-images.githubusercontent.com/30315050/143232495-941c00ad-b561-448f-9a1e-35b51a915432.MOV
69 |
70 |
71 | And a figure of the change in azimuth phases:
72 |
73 | 
74 |
75 | The input data is located in the file: `dynamic.json`
76 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------