m b
/
AoA_estimator
Part of a program that estimates the direction of arrival of a signal
Revision 0:adae25491b93, committed 2016-04-18
- Comitter:
- mikeb
- Date:
- Mon Apr 18 01:14:36 2016 +0000
- Child:
- 1:4fceb43e2dd3
- Commit message:
- 1st Draft. Some accuracy errors when close to sensor axis. ; Needs a filter and a method to process the 2 sensors. ; Filter is a must.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AoA_Est.h Mon Apr 18 01:14:36 2016 +0000 @@ -0,0 +1,128 @@ + +#include "mbed.h" +#include <cmath> + +const int MAX_SENSORS = 10; +const int ITERATIONS = 2; +/** A +* } +* } +* @endcode +*/ + +class AoA_Est { + +public: + /** Create a + * + * @param _pin mbed AnalogIn pin where the analog output of sensor is connected + * + * @note Supported types of sensors: + */ + AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq); + float estimate(float phases[], float amp[]); + bool calibrate(); + + + int confidence; + +private: + float estimate_Theoretical(float phases[], float amp[]); + float estimate_Calibrated(float phases[], float amp[]); + void comparative_Phases(float phas[]); + float angle_Resolver(); + float distanceFinder(float phase); + int sensors; + + 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]; + float wavelength; + +}; +AoA_Est::AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq) : sensors(numOfSensors) +{ + 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]); + angle = acos(distance / sensorSep[i])*180/3.1415923535; + ambigAngles[0][i] = sensorAngles[i] - angle; //Potentially swap +/- + ambigAngles[1][i] = sensorAngles[i] + angle; + // if (distance > 0) { + //ambigAngles[0][i] = (int(ambigAngles[0][i]) + 180) % 360; //Check + //ambigAngles[1][i] = (int(ambigAngles[1][i]) + 180) % 360; //Not sure + // ambigAngles[0][i] = angle - sensorAngles[i]; + // 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]; + } + 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 <= ITERATIONS; i++) { + if (abs(angle - ambigAngles[0][i]) < 30) { + angle = ambigAngles[0][i]; + avg += ambigAngles[0][i]; + confidence++; + } + else if (abs(angle - ambigAngles[1][i]) < 30) { + 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 / (ITERATIONS);//change when comute 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]; + + } + +} + +float AoA_Est::estimate(float phas[], float amplitudes[]) { + float angle; + comparative_Phases(phas); + angle = estimate_Theoretical(phases, amplitudes); + + return angle; + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Phase_Finder.h Mon Apr 18 01:14:36 2016 +0000 @@ -0,0 +1,86 @@ +#include <mbed.h> +using namespace std; +const int SAMPLE_LENGTH = 251; +const int PEAKS = 4; + +/** A +* } +* } +* @endcode +*/ + +class Phase_Finder { + +public: + /** Create a + * + * @param _pin mbed AnalogIn pin where the analog output of sensor is connected + * + * @note Supported types of sensors: + */ + Phase_Finder(int sampleRate, float frequency); + float estimate(float samples[], int leng); + + +private: + float est_Phase(); + void est_Max(float samples[]); + float wavelength; + float frequency; + int sampleRate; + int indices1[PEAKS]; + int length; + int peaks; + float phase; + +}; +Phase_Finder::Phase_Finder(int nsampleRate, float freq) : sampleRate(nsampleRate), frequency(freq) +{ + //wavelength = (338.4/freq); +} + +void Phase_Finder::est_Max(float samples1[]) { + + + for (int j = 0; j<peaks; j++) { + float max = 0; + + for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) { + if (max < samples1[i]) { + max = samples1[i]; + indices1[j] = i; + } + } + if (indices1[j] - ceil(sampleRate / frequency)/2 >= 0) { + for (int i = -ceil(sampleRate / frequency)/2; i< ceil(sampleRate/frequency) / 2; i++) { + samples1[indices1[j] + i] = 0; + } + } + else { + for (int i = 0; i< indices1[j] + ceil(sampleRate / frequency) / 2; i++) { + samples1[indices1[j] + i] = 0; + } + } + + } +} + + +float Phase_Finder::est_Phase() { + float avgDist = 0; + float ph; + for (int i = 0; i<peaks - 1; i++){ + avgDist += indices1[i] - ceil(sampleRate/frequency*i); + } + avgDist = avgDist / (peaks- 1); + ph = avgDist / float(sampleRate) * float(frequency) *float(360); + return ph; +} + +float Phase_Finder::estimate(float sampl[], int leng) { + length = leng; + peaks = floor(frequency / sampleRate*length); + est_Max(sampl); + return est_Phase(); + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 18 01:14:36 2016 +0000 @@ -0,0 +1,15 @@ +#include "mbed.h" +#include "AoA_Est.h" +#include "Phase_Finder.h" + +int main() { + int x[2] = { 95, -82 }; + int y[2] = { 77, -110 }; + Phase_Finder phase(50000, 900); + Phase_Finder phase2(50000, 900); + Phase_Finder phase3(50000, 900); + AoA_Est AoA(2, x, y, 900); + while(1) { + + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Apr 18 01:14:36 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9 \ No newline at end of file