m b / Mbed 2 deprecated AoA_estimator

Part of a program that estimates the direction of arrival of a signal

Dependencies:   mbed dsp

AoA_Est.h@15:29805fab7655, 2016-04-28 (annotated)

Committer:
mikeb
Date:
Thu Apr 28 17:25:34 2016 +0000
Revision:
15:29805fab7655
Parent:
14:8865ec38bdc8 
Fixed Documentation;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikeb 1:4fceb43e2dd3 1 #pragma once
mikeb 0:adae25491b93 2 #include "mbed.h"
mikeb 0:adae25491b93 3 #include <cmath>
mikeb 14:8865ec38bdc8 4 const int MAX_SENSORS = 10; //Maxmimum number of sensors
mikeb 8:aaf5cde0aa0a 5 /** AoA_Est.h
mikeb 8:aaf5cde0aa0a 6 * Computes the angle of arrival of an audio signal
mikeb 8:aaf5cde0aa0a 7 * EXAMPLE:
mikeb 8:aaf5cde0aa0a 8 @code
mikeb 8:aaf5cde0aa0a 9 //Find the angle of arrival of a 900Hz audio signal using 3 MBEDS equipped with microphones.
mikeb 8:aaf5cde0aa0a 10 //Requires an array of phases, found using Phase_Finder and communication between MBEDs.
mikeb 8:aaf5cde0aa0a 11 //This code begins after phases have been found and communicated to master MBED
mikeb 8:aaf5cde0aa0a 12 //Sensors are located at (0,0), (150,0), and (150,90). Maximum sensors is 10 including master. Can be changed with parameter MAX_SENSORS
mikeb 8:aaf5cde0aa0a 13 float angle = 0;
mikeb 8:aaf5cde0aa0a 14 int x[2] = {150, 150); //Distances from master MBED microphone in milimeters. All distances must be for microphones, not MBED units
mikeb 8:aaf5cde0aa0a 15 int y[2] = {0, 90); //Do not include master MBED, its microphone is assumed to be 0,0.
mikeb 8:aaf5cde0aa0a 16 AoA_Est AoA(3, x, y, 900);
mikeb 8:aaf5cde0aa0a 17 angle = AoA.estimate(phases, phases);
mikeb 0:adae25491b93 18 * }
mikeb 0:adae25491b93 19 * @endcode
mikeb 0:adae25491b93 20 */
mikeb 0:adae25491b93 21
mikeb 14:8865ec38bdc8 22
mikeb 8:aaf5cde0aa0a 23
mikeb 8:aaf5cde0aa0a 24
mikeb 8:aaf5cde0aa0a 25
mikeb 0:adae25491b93 26 class AoA_Est {
mikeb 0:adae25491b93 27
mikeb 0:adae25491b93 28 public:
mikeb 8:aaf5cde0aa0a 29 /** Create an Angle of Arrival Estimator Object
mikeb 8:aaf5cde0aa0a 30 *
mikeb 8:aaf5cde0aa0a 31 * @param numOfSensors Number of sensors including the master
mikeb 8:aaf5cde0aa0a 32 * @param xPassed[] X coordinates of every sensor EXCEPT the master. Master is assumed (0,0).
mikeb 8:aaf5cde0aa0a 33 * @param yPassed[] Y coordinates of every sensor EXCEPT the master
mikeb 8:aaf5cde0aa0a 34 * @param freq frequency of interest in Hz
mikeb 8:aaf5cde0aa0a 35 * @note Maximum number of supported sensors is 10
mikeb 8:aaf5cde0aa0a 36 */
mikeb 8:aaf5cde0aa0a 37 AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq);
mikeb 8:aaf5cde0aa0a 38 /** Estimate the angle to the sound source
mikeb 8:aaf5cde0aa0a 39 *
mikeb 15:29805fab7655 40 * @param phases[] Array of phases, first phase is the master. Must correspond to order of X and Y coordinates. All sensors must be within 1/2 wavelength of eachother. Will not work otherwise.
mikeb 8:aaf5cde0aa0a 41 * @param amp[] Amplitudes. Not currently used. Repeat the phase array here to prevent errors.
mikeb 8:aaf5cde0aa0a 42 * @param yPassed[] Y coordinates of every sensor EXCEPT the master
mikeb 15:29805fab7655 43 * @return Angle relative to X axis
mikeb 8:aaf5cde0aa0a 44 */
mikeb 8:aaf5cde0aa0a 45 float estimate(float phases[], float amp[]);
mikeb 8:aaf5cde0aa0a 46 /** Not Used
mikeb 0:adae25491b93 47 */
mikeb 0:adae25491b93 48 bool calibrate();
mikeb 8:aaf5cde0aa0a 49 /** Confidence level of the output
mikeb 8:aaf5cde0aa0a 50 *
mikeb 15:29805fab7655 51 * @return Float corresponding to the number of angle determinations used in the average
mikeb 8:aaf5cde0aa0a 52 */
mikeb 0:adae25491b93 53 int confidence;
mikeb 0:adae25491b93 54
mikeb 0:adae25491b93 55 private:
mikeb 8:aaf5cde0aa0a 56 float estimate_Theoretical(float phases[], float amp[]);
mikeb 8:aaf5cde0aa0a 57 float estimate_Calibrated(float phases[], float amp[]); //Unused
mikeb 8:aaf5cde0aa0a 58 void comparative_Phases(float phas[]); //Returns the phases relative to the master
mikeb 0:adae25491b93 59 float angle_Resolver();
mikeb 0:adae25491b93 60 float distanceFinder(float phase);
mikeb 0:adae25491b93 61 int sensors;
mikeb 8:aaf5cde0aa0a 62 int ITERATIONS;
mikeb 0:adae25491b93 63 int x[MAX_SENSORS];
mikeb 0:adae25491b93 64 int y[MAX_SENSORS];
mikeb 0:adae25491b93 65 float phases[MAX_SENSORS - 1];
mikeb 0:adae25491b93 66 float sensorSep[MAX_SENSORS];
mikeb 0:adae25491b93 67 float sensorAngles[MAX_SENSORS];
mikeb 0:adae25491b93 68 float amplitudes[MAX_SENSORS];
mikeb 0:adae25491b93 69 int z[2];
mikeb 14:8865ec38bdc8 70 float ambigAngles[2][MAX_SENSORS];
mikeb 0:adae25491b93 71 float wavelength;
mikeb 0:adae25491b93 72
mikeb 0:adae25491b93 73 };
mikeb 0:adae25491b93 74 AoA_Est::AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq) : sensors(numOfSensors)
mikeb 0:adae25491b93 75 {
mikeb 8:aaf5cde0aa0a 76 int j = sensors - 1;
mikeb 8:aaf5cde0aa0a 77 while (j > 0){
mikeb 8:aaf5cde0aa0a 78 ITERATIONS += j;
mikeb 8:aaf5cde0aa0a 79 j--;
mikeb 8:aaf5cde0aa0a 80 }
mikeb 0:adae25491b93 81 wavelength = (338.4 / freq)*1000;
mikeb 0:adae25491b93 82 for (short i = 0; i < sensors-1; i++) {
mikeb 0:adae25491b93 83 x[i] = xPassed[i];
mikeb 0:adae25491b93 84 y[i] = yPassed[i];
mikeb 0:adae25491b93 85 sensorSep[i] = sqrt(float(xPassed[i] * x[i]) + float(yPassed[i] * y[i]));
mikeb 0:adae25491b93 86 sensorAngles[i] = atan2(float(yPassed[i]), float(xPassed[i]))*180/3.1415926535;
mikeb 0:adae25491b93 87 }
mikeb 0:adae25491b93 88 }
mikeb 0:adae25491b93 89
mikeb 0:adae25491b93 90 float AoA_Est::distanceFinder(float phase) {
mikeb 0:adae25491b93 91 return phase / 360 * wavelength;
mikeb 0:adae25491b93 92 }
mikeb 0:adae25491b93 93
mikeb 0:adae25491b93 94 float AoA_Est::estimate_Theoretical(float phases[], float amp[]) {
mikeb 0:adae25491b93 95 float distance = 0;
mikeb 0:adae25491b93 96 float angle = 0;
mikeb 0:adae25491b93 97
mikeb 0:adae25491b93 98 for (int i = 0; i < sensors-1; i++) {
mikeb 0:adae25491b93 99 distance = distanceFinder(phases[i]);
mikeb 13:d221d9ef6338 100 distance = (distance > sensorSep[i]) ? distance - 338400/50000 : distance;
mikeb 0:adae25491b93 101 angle = acos(distance / sensorSep[i])*180/3.1415923535;
mikeb 0:adae25491b93 102 ambigAngles[0][i] = sensorAngles[i] - angle; //Potentially swap +/-
mikeb 0:adae25491b93 103 ambigAngles[1][i] = sensorAngles[i] + angle;
mikeb 11:8574bb5b92b0 104
mikeb 0:adae25491b93 105 ambigAngles[0][i] = (ambigAngles[0][i] < 0) ? ambigAngles[0][i] + 360 : ambigAngles[0][i];
mikeb 0:adae25491b93 106 ambigAngles[1][i] = (ambigAngles[1][i] < 0) ? ambigAngles[1][i] + 360 : ambigAngles[1][i];
mikeb 0:adae25491b93 107 }
mikeb 1:4fceb43e2dd3 108
mikeb 1:4fceb43e2dd3 109 float phas_diff = 0;
mikeb 1:4fceb43e2dd3 110 float relative_angle = 0;
mikeb 1:4fceb43e2dd3 111 float relative_dist = 0;
mikeb 14:8865ec38bdc8 112 //Accuracy improvement
mikeb 8:aaf5cde0aa0a 113 /*for (int i = 1; i < sensors - 1; i++) { //for(int i = 1; i<ITERATIONS - sensors+1; i++)
mikeb 1:4fceb43e2dd3 114 while (i < sensors - 1) {
mikeb 1:4fceb43e2dd3 115 phas_diff = phases[i - 1] - phases[i];
mikeb 1:4fceb43e2dd3 116 if (abs(phas_diff) > 180) {
mikeb 1:4fceb43e2dd3 117 phas_diff = (phas_diff < 0) ? phas_diff + 360 : phas_diff - 360;
mikeb 1:4fceb43e2dd3 118 }
mikeb 1:4fceb43e2dd3 119 distance = distanceFinder(phas_diff);
mikeb 1:4fceb43e2dd3 120 relative_angle = atan2(float(y[i - 1] - y[i]), float((x[i - 1] - x[i]))) * 180 / 3.1415926535;
mikeb 1:4fceb43e2dd3 121 relative_dist = sqrt(float((x[i - 1] - x[i]) *(x[i - 1] - x[i])) + float((y[i - 1] - y[i]) *(y[i - 1] - y[i])));
mikeb 1:4fceb43e2dd3 122 angle = acos(distance / relative_dist) * 180 / 3.1415923535;
mikeb 1:4fceb43e2dd3 123 ambigAngles[0][sensors - 2 + i] = relative_angle - angle;
mikeb 1:4fceb43e2dd3 124 ambigAngles[1][sensors - 2 + i] = relative_angle + angle;
mikeb 1:4fceb43e2dd3 125
mikeb 1:4fceb43e2dd3 126 ambigAngles[0][sensors - 2 + i] = (ambigAngles[0][sensors - 2 + i] < 0) ? ambigAngles[0][sensors - 2 + i] + 360 : ambigAngles[0][sensors - 2 + i];
mikeb 1:4fceb43e2dd3 127 ambigAngles[1][sensors - 2 + i] = (ambigAngles[1][sensors - 2 + i] < 0) ? ambigAngles[1][sensors - 2 + i] + 360 : ambigAngles[1][sensors - 2 + i];
mikeb 1:4fceb43e2dd3 128 i++;
mikeb 1:4fceb43e2dd3 129 }
mikeb 6:697b75e941a7 130 }*/
mikeb 0:adae25491b93 131 angle = angle_Resolver();
mikeb 0:adae25491b93 132 return angle;
mikeb 0:adae25491b93 133 }
mikeb 0:adae25491b93 134
mikeb 0:adae25491b93 135 float AoA_Est::angle_Resolver() {
mikeb 0:adae25491b93 136 float angle = ambigAngles[0][0];
mikeb 0:adae25491b93 137 float avg = angle;
mikeb 0:adae25491b93 138 bool flag = false;
mikeb 0:adae25491b93 139 confidence = 0;
mikeb 6:697b75e941a7 140 for (short i = 1; i <= sensors-1; i++) {
mikeb 7:25dacf35f4c7 141 if (abs(angle - ambigAngles[0][i]) < abs(angle-ambigAngles[1][i]) && abs(angle-ambigAngles[0][i]) < 40) {
mikeb 0:adae25491b93 142 angle = ambigAngles[0][i];
mikeb 0:adae25491b93 143 avg += ambigAngles[0][i];
mikeb 0:adae25491b93 144 confidence++;
mikeb 0:adae25491b93 145 }
mikeb 7:25dacf35f4c7 146 else if (abs(angle - ambigAngles[1][i]) < abs(angle-ambigAngles[0][i]) && abs(angle - ambigAngles[1][i]) < 40) {
mikeb 0:adae25491b93 147 angle = ambigAngles[1][i];
mikeb 0:adae25491b93 148 avg += ambigAngles[1][i];
mikeb 0:adae25491b93 149 confidence++;
mikeb 0:adae25491b93 150 }
mikeb 0:adae25491b93 151 else if (i == 1 && flag == false) {
mikeb 0:adae25491b93 152 angle = ambigAngles[1][0];
mikeb 0:adae25491b93 153 avg = angle;
mikeb 0:adae25491b93 154 i = 0;
mikeb 0:adae25491b93 155 flag = true;
mikeb 0:adae25491b93 156 }
mikeb 0:adae25491b93 157
mikeb 0:adae25491b93 158 }
mikeb 0:adae25491b93 159
mikeb 7:25dacf35f4c7 160 return avg / (confidence+1);//change when compute the other way
mikeb 0:adae25491b93 161 }
mikeb 0:adae25491b93 162
mikeb 0:adae25491b93 163 void AoA_Est :: comparative_Phases(float phas[]) {
mikeb 0:adae25491b93 164 for (int i = 0; i < sensors - 1; i++) {
mikeb 0:adae25491b93 165 phases[i] = phas[0] - phas[i + 1];
mikeb 1:4fceb43e2dd3 166 if (abs(phases[i]) > 180) {
mikeb 1:4fceb43e2dd3 167 phases[i] = (phases[i] < 0) ? phases[i] + 360 : phases[i] - 360;
mikeb 1:4fceb43e2dd3 168 }
mikeb 0:adae25491b93 169
mikeb 0:adae25491b93 170 }
mikeb 0:adae25491b93 171
mikeb 0:adae25491b93 172 }
mikeb 0:adae25491b93 173
mikeb 0:adae25491b93 174 float AoA_Est::estimate(float phas[], float amplitudes[]) {
mikeb 0:adae25491b93 175 float angle;
mikeb 0:adae25491b93 176 comparative_Phases(phas);
mikeb 0:adae25491b93 177 angle = estimate_Theoretical(phases, amplitudes);
mikeb 0:adae25491b93 178
mikeb 0:adae25491b93 179 return angle;
mikeb 0:adae25491b93 180
mikeb 0:adae25491b93 181 }