m b
/
AoA_estimator
Part of a program that estimates the direction of arrival of a signal
AoA_Est.h
- Committer:
- mikeb
- Date:
- 2016-04-28
- Revision:
- 11:8574bb5b92b0
- Parent:
- 8:aaf5cde0aa0a
- Child:
- 13:d221d9ef6338
File content as of revision 11:8574bb5b92b0:
#pragma once #include "mbed.h" #include <cmath> /** AoA_Est.h * Computes the angle of arrival of an audio signal * EXAMPLE: @code //Find the angle of arrival of a 900Hz audio signal using 3 MBEDS equipped with microphones. //Requires an array of phases, found using Phase_Finder and communication between MBEDs. //This code begins after phases have been found and communicated to master MBED //Sensors are located at (0,0), (150,0), and (150,90). Maximum sensors is 10 including master. Can be changed with parameter MAX_SENSORS float angle = 0; int x[2] = {150, 150); //Distances from master MBED microphone in milimeters. All distances must be for microphones, not MBED units int y[2] = {0, 90); //Do not include master MBED, its microphone is assumed to be 0,0. AoA_Est AoA(3, x, y, 900); angle = AoA.estimate(phases, phases); * } * @endcode */ const int MAX_SENSORS = 10; //Maxmimum number of sensors class AoA_Est { public: /** Create an Angle of Arrival Estimator Object * * @param numOfSensors Number of sensors including the master * @param xPassed[] X coordinates of every sensor EXCEPT the master. Master is assumed (0,0). * @param yPassed[] Y coordinates of every sensor EXCEPT the master * @param freq frequency of interest in Hz * @note Maximum number of supported sensors is 10 */ AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq); /** Estimate the angle to the sound source * * @param phases[] Array of phases, first phase is the master. Must correspond to order of X and Y coordinates. * @param amp[] Amplitudes. Not currently used. Repeat the phase array here to prevent errors. * @param yPassed[] Y coordinates of every sensor EXCEPT the master * @return angle relative to X axis */ float estimate(float phases[], float amp[]); /** Not Used */ bool calibrate(); /** Confidence level of the output * * @return number corresponding to the number of angle determinations used in the average */ int confidence; private: float estimate_Theoretical(float phases[], float amp[]); float estimate_Calibrated(float phases[], float amp[]); //Unused void comparative_Phases(float phas[]); //Returns the phases relative to the master float angle_Resolver(); float distanceFinder(float phase); int sensors; int ITERATIONS; int x[MAX_SENSORS]; int y[MAX_SENSORS]; float phases[MAX_SENSORS - 1]; float sensorSep[MAX_SENSORS]; float sensorAngles[MAX_SENSORS]; float amplitudes[MAX_SENSORS]; int z[2]; float ambigAngles[2][MAX_SENSORS+ITERATIONS]; float wavelength; }; AoA_Est::AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq) : sensors(numOfSensors) { int j = sensors - 1; while (j > 0){ ITERATIONS += j; j--; } wavelength = (338.4 / freq)*1000; for (short i = 0; i < sensors-1; i++) { x[i] = xPassed[i]; y[i] = yPassed[i]; sensorSep[i] = sqrt(float(xPassed[i] * x[i]) + float(yPassed[i] * y[i])); sensorAngles[i] = atan2(float(yPassed[i]), float(xPassed[i]))*180/3.1415926535; } } float AoA_Est::distanceFinder(float phase) { return phase / 360 * wavelength; } float AoA_Est::estimate_Theoretical(float phases[], float amp[]) { float distance = 0; float angle = 0; for (int i = 0; i < sensors-1; i++) { distance = distanceFinder(phases[i]); distance = (distance > sensorSep[i]) ? distance - 338400/sampleRate : distance; angle = acos(distance / sensorSep[i])*180/3.1415923535; ambigAngles[0][i] = sensorAngles[i] - angle; //Potentially swap +/- ambigAngles[1][i] = sensorAngles[i] + angle; ambigAngles[0][i] = (ambigAngles[0][i] < 0) ? ambigAngles[0][i] + 360 : ambigAngles[0][i]; ambigAngles[1][i] = (ambigAngles[1][i] < 0) ? ambigAngles[1][i] + 360 : ambigAngles[1][i]; } float phas_diff = 0; float relative_angle = 0; float relative_dist = 0; /*for (int i = 1; i < sensors - 1; i++) { //for(int i = 1; i<ITERATIONS - sensors+1; i++) while (i < sensors - 1) { phas_diff = phases[i - 1] - phases[i]; if (abs(phas_diff) > 180) { phas_diff = (phas_diff < 0) ? phas_diff + 360 : phas_diff - 360; } distance = distanceFinder(phas_diff); relative_angle = atan2(float(y[i - 1] - y[i]), float((x[i - 1] - x[i]))) * 180 / 3.1415926535; 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]))); angle = acos(distance / relative_dist) * 180 / 3.1415923535; ambigAngles[0][sensors - 2 + i] = relative_angle - angle; ambigAngles[1][sensors - 2 + i] = relative_angle + angle; ambigAngles[0][sensors - 2 + i] = (ambigAngles[0][sensors - 2 + i] < 0) ? ambigAngles[0][sensors - 2 + i] + 360 : ambigAngles[0][sensors - 2 + i]; ambigAngles[1][sensors - 2 + i] = (ambigAngles[1][sensors - 2 + i] < 0) ? ambigAngles[1][sensors - 2 + i] + 360 : ambigAngles[1][sensors - 2 + i]; i++; } }*/ angle = angle_Resolver(); return angle; } float AoA_Est::angle_Resolver() { float angle = ambigAngles[0][0]; float avg = angle; bool flag = false; confidence = 0; for (short i = 1; i <= sensors-1; i++) { if (abs(angle - ambigAngles[0][i]) < abs(angle-ambigAngles[1][i]) && abs(angle-ambigAngles[0][i]) < 40) { angle = ambigAngles[0][i]; avg += ambigAngles[0][i]; confidence++; } else if (abs(angle - ambigAngles[1][i]) < abs(angle-ambigAngles[0][i]) && abs(angle - ambigAngles[1][i]) < 40) { angle = ambigAngles[1][i]; avg += ambigAngles[1][i]; confidence++; } else if (i == 1 && flag == false) { angle = ambigAngles[1][0]; avg = angle; i = 0; flag = true; } } return avg / (confidence+1);//change when compute the other way } void AoA_Est :: comparative_Phases(float phas[]) { for (int i = 0; i < sensors - 1; i++) { phases[i] = phas[0] - phas[i + 1]; if (abs(phases[i]) > 180) { phases[i] = (phases[i] < 0) ? phases[i] + 360 : phases[i] - 360; } } } float AoA_Est::estimate(float phas[], float amplitudes[]) { float angle; comparative_Phases(phas); angle = estimate_Theoretical(phases, amplitudes); return angle; }