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@7:25dacf35f4c7, 2016-04-27 (annotated)

Committer:
mikeb
Date:
Wed Apr 27 17:11:16 2016 +0000
Revision:
7:25dacf35f4c7
Parent:
6:697b75e941a7
Child:
8:aaf5cde0aa0a
Removed accuracy imporvement

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 0:adae25491b93 4
mikeb 0:adae25491b93 5 const int MAX_SENSORS = 10;
mikeb 1:4fceb43e2dd3 6 const int ITERATIONS = 3;
mikeb 0:adae25491b93 7 /** A
mikeb 0:adae25491b93 8 * }
mikeb 0:adae25491b93 9 * }
mikeb 0:adae25491b93 10 * @endcode
mikeb 0:adae25491b93 11 */
mikeb 0:adae25491b93 12
mikeb 0:adae25491b93 13 class AoA_Est {
mikeb 0:adae25491b93 14
mikeb 0:adae25491b93 15 public:
mikeb 0:adae25491b93 16 /** Create a
mikeb 0:adae25491b93 17 *
mikeb 0:adae25491b93 18 * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
mikeb 0:adae25491b93 19 *
mikeb 0:adae25491b93 20 * @note Supported types of sensors:
mikeb 0:adae25491b93 21 */
mikeb 0:adae25491b93 22 AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq);
mikeb 0:adae25491b93 23 float estimate(float phases[], float amp[]);
mikeb 0:adae25491b93 24 bool calibrate();
mikeb 0:adae25491b93 25
mikeb 0:adae25491b93 26
mikeb 0:adae25491b93 27 int confidence;
mikeb 0:adae25491b93 28
mikeb 0:adae25491b93 29 private:
mikeb 0:adae25491b93 30 float estimate_Theoretical(float phases[], float amp[]);
mikeb 0:adae25491b93 31 float estimate_Calibrated(float phases[], float amp[]);
mikeb 0:adae25491b93 32 void comparative_Phases(float phas[]);
mikeb 0:adae25491b93 33 float angle_Resolver();
mikeb 0:adae25491b93 34 float distanceFinder(float phase);
mikeb 0:adae25491b93 35 int sensors;
mikeb 0:adae25491b93 36
mikeb 0:adae25491b93 37 int x[MAX_SENSORS];
mikeb 0:adae25491b93 38 int y[MAX_SENSORS];
mikeb 0:adae25491b93 39 float phases[MAX_SENSORS - 1];
mikeb 0:adae25491b93 40 float sensorSep[MAX_SENSORS];
mikeb 0:adae25491b93 41 float sensorAngles[MAX_SENSORS];
mikeb 0:adae25491b93 42 float amplitudes[MAX_SENSORS];
mikeb 0:adae25491b93 43 int z[2];
mikeb 1:4fceb43e2dd3 44 float ambigAngles[2][MAX_SENSORS+ITERATIONS];
mikeb 0:adae25491b93 45 float wavelength;
mikeb 0:adae25491b93 46
mikeb 0:adae25491b93 47 };
mikeb 0:adae25491b93 48 AoA_Est::AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq) : sensors(numOfSensors)
mikeb 0:adae25491b93 49 {
mikeb 0:adae25491b93 50 wavelength = (338.4 / freq)*1000;
mikeb 0:adae25491b93 51 for (short i = 0; i < sensors-1; i++) {
mikeb 0:adae25491b93 52 x[i] = xPassed[i];
mikeb 0:adae25491b93 53 y[i] = yPassed[i];
mikeb 0:adae25491b93 54 sensorSep[i] = sqrt(float(xPassed[i] * x[i]) + float(yPassed[i] * y[i]));
mikeb 0:adae25491b93 55 sensorAngles[i] = atan2(float(yPassed[i]), float(xPassed[i]))*180/3.1415926535;
mikeb 0:adae25491b93 56 }
mikeb 0:adae25491b93 57 }
mikeb 0:adae25491b93 58
mikeb 0:adae25491b93 59 float AoA_Est::distanceFinder(float phase) {
mikeb 0:adae25491b93 60 return phase / 360 * wavelength;
mikeb 0:adae25491b93 61 }
mikeb 0:adae25491b93 62
mikeb 0:adae25491b93 63 float AoA_Est::estimate_Theoretical(float phases[], float amp[]) {
mikeb 0:adae25491b93 64 float distance = 0;
mikeb 0:adae25491b93 65 float angle = 0;
mikeb 0:adae25491b93 66
mikeb 0:adae25491b93 67 for (int i = 0; i < sensors-1; i++) {
mikeb 0:adae25491b93 68 distance = distanceFinder(phases[i]);
mikeb 0:adae25491b93 69 angle = acos(distance / sensorSep[i])*180/3.1415923535;
mikeb 0:adae25491b93 70 ambigAngles[0][i] = sensorAngles[i] - angle; //Potentially swap +/-
mikeb 0:adae25491b93 71 ambigAngles[1][i] = sensorAngles[i] + angle;
mikeb 0:adae25491b93 72 // if (distance > 0) {
mikeb 0:adae25491b93 73 //ambigAngles[0][i] = (int(ambigAngles[0][i]) + 180) % 360; //Check
mikeb 0:adae25491b93 74 //ambigAngles[1][i] = (int(ambigAngles[1][i]) + 180) % 360; //Not sure
mikeb 0:adae25491b93 75 // ambigAngles[0][i] = angle - sensorAngles[i];
mikeb 0:adae25491b93 76 // ambigAngles[1][i] = sensorAngles[i] + angle;
mikeb 0:adae25491b93 77 // }
mikeb 0:adae25491b93 78 ambigAngles[0][i] = (ambigAngles[0][i] < 0) ? ambigAngles[0][i] + 360 : ambigAngles[0][i];
mikeb 0:adae25491b93 79 ambigAngles[1][i] = (ambigAngles[1][i] < 0) ? ambigAngles[1][i] + 360 : ambigAngles[1][i];
mikeb 0:adae25491b93 80 }
mikeb 1:4fceb43e2dd3 81
mikeb 1:4fceb43e2dd3 82 float phas_diff = 0;
mikeb 1:4fceb43e2dd3 83 float relative_angle = 0;
mikeb 1:4fceb43e2dd3 84 float relative_dist = 0;
mikeb 6:697b75e941a7 85 /*for (int i = 1; i < sensors - 1; i++) {
mikeb 1:4fceb43e2dd3 86 while (i < sensors - 1) {
mikeb 1:4fceb43e2dd3 87 phas_diff = phases[i - 1] - phases[i];
mikeb 1:4fceb43e2dd3 88 if (abs(phas_diff) > 180) {
mikeb 1:4fceb43e2dd3 89 phas_diff = (phas_diff < 0) ? phas_diff + 360 : phas_diff - 360;
mikeb 1:4fceb43e2dd3 90 }
mikeb 1:4fceb43e2dd3 91 distance = distanceFinder(phas_diff);
mikeb 1:4fceb43e2dd3 92 relative_angle = atan2(float(y[i - 1] - y[i]), float((x[i - 1] - x[i]))) * 180 / 3.1415926535;
mikeb 1:4fceb43e2dd3 93 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 94 angle = acos(distance / relative_dist) * 180 / 3.1415923535;
mikeb 1:4fceb43e2dd3 95 ambigAngles[0][sensors - 2 + i] = relative_angle - angle;
mikeb 1:4fceb43e2dd3 96 ambigAngles[1][sensors - 2 + i] = relative_angle + angle;
mikeb 1:4fceb43e2dd3 97
mikeb 1:4fceb43e2dd3 98 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 99 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 100 i++;
mikeb 1:4fceb43e2dd3 101 }
mikeb 6:697b75e941a7 102 }*/
mikeb 0:adae25491b93 103 angle = angle_Resolver();
mikeb 0:adae25491b93 104 return angle;
mikeb 0:adae25491b93 105 }
mikeb 0:adae25491b93 106
mikeb 0:adae25491b93 107 float AoA_Est::angle_Resolver() {
mikeb 0:adae25491b93 108 float angle = ambigAngles[0][0];
mikeb 0:adae25491b93 109 float avg = angle;
mikeb 0:adae25491b93 110 bool flag = false;
mikeb 0:adae25491b93 111 confidence = 0;
mikeb 6:697b75e941a7 112 for (short i = 1; i <= sensors-1; i++) {
mikeb 7:25dacf35f4c7 113 if (abs(angle - ambigAngles[0][i]) < abs(angle-ambigAngles[1][i]) && abs(angle-ambigAngles[0][i]) < 40) {
mikeb 0:adae25491b93 114 angle = ambigAngles[0][i];
mikeb 0:adae25491b93 115 avg += ambigAngles[0][i];
mikeb 0:adae25491b93 116 confidence++;
mikeb 0:adae25491b93 117 }
mikeb 7:25dacf35f4c7 118 else if (abs(angle - ambigAngles[1][i]) < abs(angle-ambigAngles[0][i]) && abs(angle - ambigAngles[1][i]) < 40) {
mikeb 0:adae25491b93 119 angle = ambigAngles[1][i];
mikeb 0:adae25491b93 120 avg += ambigAngles[1][i];
mikeb 0:adae25491b93 121 confidence++;
mikeb 0:adae25491b93 122 }
mikeb 0:adae25491b93 123 else if (i == 1 && flag == false) {
mikeb 0:adae25491b93 124 angle = ambigAngles[1][0];
mikeb 0:adae25491b93 125 avg = angle;
mikeb 0:adae25491b93 126 i = 0;
mikeb 0:adae25491b93 127 flag = true;
mikeb 0:adae25491b93 128 }
mikeb 0:adae25491b93 129
mikeb 0:adae25491b93 130 }
mikeb 0:adae25491b93 131
mikeb 7:25dacf35f4c7 132 return avg / (confidence+1);//change when compute the other way
mikeb 0:adae25491b93 133 }
mikeb 0:adae25491b93 134
mikeb 0:adae25491b93 135 void AoA_Est :: comparative_Phases(float phas[]) {
mikeb 0:adae25491b93 136 for (int i = 0; i < sensors - 1; i++) {
mikeb 0:adae25491b93 137 phases[i] = phas[0] - phas[i + 1];
mikeb 1:4fceb43e2dd3 138 if (abs(phases[i]) > 180) {
mikeb 1:4fceb43e2dd3 139 phases[i] = (phases[i] < 0) ? phases[i] + 360 : phases[i] - 360;
mikeb 1:4fceb43e2dd3 140 }
mikeb 0:adae25491b93 141
mikeb 0:adae25491b93 142 }
mikeb 0:adae25491b93 143
mikeb 0:adae25491b93 144 }
mikeb 0:adae25491b93 145
mikeb 0:adae25491b93 146 float AoA_Est::estimate(float phas[], float amplitudes[]) {
mikeb 0:adae25491b93 147 float angle;
mikeb 0:adae25491b93 148 comparative_Phases(phas);
mikeb 0:adae25491b93 149 angle = estimate_Theoretical(phases, amplitudes);
mikeb 0:adae25491b93 150
mikeb 0:adae25491b93 151 return angle;
mikeb 0:adae25491b93 152
mikeb 0:adae25491b93 153 }