m b / Mbed 2 deprecated AoA_estimator

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

Dependencies:   mbed dsp

Phase_Finder.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 0:adae25491b93 1 #include <mbed.h>
mikeb 0:adae25491b93 2 using namespace std;
mikeb 0:adae25491b93 3 const int SAMPLE_LENGTH = 251;
mikeb 0:adae25491b93 4 const int PEAKS = 4;
mikeb 0:adae25491b93 5
mikeb 14:8865ec38bdc8 6 /** Phase_Finder.h
mikeb 14:8865ec38bdc8 7 * Computes the phase of a signal using correlation
mikeb 9:d86d73964999 8 * EXAMPLE:
mikeb 9:d86d73964999 9 @code
mikeb 14:8865ec38bdc8 10 //Estimate the phase of a 900Hz input signal.
mikeb 14:8865ec38bdc8 11 //Requires an input cosine reference. Generated in MATLAB and placed into an array not shown.
mikeb 14:8865ec38bdc8 12 float reference[307] = {...};
mikeb 14:8865ec38bdc8 13 float phase = 0;
mikeb 14:8865ec38bdc8 14 Phase_Finder phase(50000, 900, reference);
mikeb 15:29805fab7655 15 phase = phase.estimate(signal, 251);
mikeb 0:adae25491b93 16 * }
mikeb 0:adae25491b93 17 * @endcode
mikeb 0:adae25491b93 18 */
mikeb 10:cd3f7010da48 19
mikeb 0:adae25491b93 20 class Phase_Finder {
mikeb 0:adae25491b93 21
mikeb 0:adae25491b93 22 public:
mikeb 14:8865ec38bdc8 23 /** Create a Phase_Finder object
mikeb 0:adae25491b93 24 *
mikeb 14:8865ec38bdc8 25 * @param sampleRate ADC Sample Rate
mikeb 14:8865ec38bdc8 26 * @param frequency Frequency of Interest in Hz
mikeb 14:8865ec38bdc8 27 * @param reference[] Cosine Reference signal used for correlation. Must be input sample length + ceil(sampleRate/Frequency). Must be same frequency as input frequency
mikeb 0:adae25491b93 28 */
mikeb 10:cd3f7010da48 29 Phase_Finder(int sampleRate, float frequency, float reference[]);
mikeb 14:8865ec38bdc8 30 /** Estimate the phase
mikeb 14:8865ec38bdc8 31 *
mikeb 14:8865ec38bdc8 32 * @param samples[] Input samples
mikeb 14:8865ec38bdc8 33 * @param leng Length of input sample array
mikeb 15:29805fab7655 34 * @return Float of the phase in degrees
mikeb 14:8865ec38bdc8 35 */
mikeb 0:adae25491b93 36 float estimate(float samples[], int leng);
mikeb 0:adae25491b93 37
mikeb 0:adae25491b93 38
mikeb 0:adae25491b93 39 private:
mikeb 10:cd3f7010da48 40 float reference[SAMPLE_LENGTH+56];
mikeb 0:adae25491b93 41 float est_Phase();
mikeb 0:adae25491b93 42 void est_Max(float samples[]);
mikeb 0:adae25491b93 43 float wavelength;
mikeb 0:adae25491b93 44 float frequency;
mikeb 0:adae25491b93 45 int sampleRate;
mikeb 0:adae25491b93 46 int indices1[PEAKS];
mikeb 0:adae25491b93 47 int length;
mikeb 0:adae25491b93 48 int peaks;
mikeb 0:adae25491b93 49 float phase;
mikeb 0:adae25491b93 50
mikeb 0:adae25491b93 51 };
mikeb 10:cd3f7010da48 52 Phase_Finder::Phase_Finder(int nsampleRate, float freq, float ref[]) : sampleRate(nsampleRate), frequency(freq)
mikeb 0:adae25491b93 53 {
mikeb 10:cd3f7010da48 54 for (int i = 0; i < SAMPLE_LENGTH + 56; i++) {
mikeb 10:cd3f7010da48 55 reference[i] = ref[i];
mikeb 10:cd3f7010da48 56 }
mikeb 0:adae25491b93 57 }
mikeb 0:adae25491b93 58
mikeb 0:adae25491b93 59 void Phase_Finder::est_Max(float samples1[]) {
mikeb 8:aaf5cde0aa0a 60 float change = 0;
mikeb 14:8865ec38bdc8 61 //Remove random high amplitude distortion
mikeb 8:aaf5cde0aa0a 62 for (int i = 2; i < length - 1; i++) {
mikeb 8:aaf5cde0aa0a 63 change = abs(samples1[i - 2] - samples1[i - 1]);
mikeb 8:aaf5cde0aa0a 64 if (abs(samples1[i] - samples1[i-1]) > change*4.5)
mikeb 8:aaf5cde0aa0a 65 samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
mikeb 8:aaf5cde0aa0a 66 }
mikeb 14:8865ec38bdc8 67 //Cross correlation
mikeb 10:cd3f7010da48 68 float corr[56] = {};
mikeb 10:cd3f7010da48 69 for (int j = 0; j < 56; j++) {
mikeb 10:cd3f7010da48 70 for (int i = 0; i <= length; i++) {
mikeb 10:cd3f7010da48 71 corr[j] += reference[i-j+55] * samples1[i];
mikeb 10:cd3f7010da48 72 }
mikeb 10:cd3f7010da48 73 }
mikeb 14:8865ec38bdc8 74 //Find maximum correlation
mikeb 10:cd3f7010da48 75 float max = 0;
mikeb 10:cd3f7010da48 76 for (int i = 0; i < 56; i++) {
mikeb 10:cd3f7010da48 77 if (max < corr[i]) {
mikeb 10:cd3f7010da48 78 max = corr[i];
mikeb 10:cd3f7010da48 79 indices1[0] = i;
mikeb 10:cd3f7010da48 80 }
mikeb 10:cd3f7010da48 81 }
mikeb 14:8865ec38bdc8 82 //Peak finder method. Inferior to correlation
mikeb 10:cd3f7010da48 83 /*for (int j = 0; j<peaks; j++) {
mikeb 0:adae25491b93 84 float max = 0;
mikeb 0:adae25491b93 85
mikeb 0:adae25491b93 86 for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
mikeb 0:adae25491b93 87 if (max < samples1[i]) {
mikeb 0:adae25491b93 88 max = samples1[i];
mikeb 0:adae25491b93 89 indices1[j] = i;
mikeb 0:adae25491b93 90 }
mikeb 0:adae25491b93 91 }
mikeb 10:cd3f7010da48 92 if (indices1[j] - round(sampleRate / frequency)/2 >= 0) {
mikeb 10:cd3f7010da48 93 for (int i = -round(sampleRate / frequency)/2; i< round(sampleRate/frequency) / 2; i++) {
mikeb 0:adae25491b93 94 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 95 }
mikeb 0:adae25491b93 96 }
mikeb 0:adae25491b93 97 else {
mikeb 10:cd3f7010da48 98 for (int i = 0; i< indices1[j] + round(sampleRate / frequency) / 2; i++) {
mikeb 0:adae25491b93 99 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 100 }
mikeb 0:adae25491b93 101 }
mikeb 0:adae25491b93 102
mikeb 10:cd3f7010da48 103 }*/
mikeb 0:adae25491b93 104 }
mikeb 0:adae25491b93 105
mikeb 0:adae25491b93 106
mikeb 0:adae25491b93 107 float Phase_Finder::est_Phase() {
mikeb 10:cd3f7010da48 108 //float avgDist = 0;
mikeb 0:adae25491b93 109 float ph;
mikeb 14:8865ec38bdc8 110 //Used for peak finding averaging. Not necessary.
mikeb 10:cd3f7010da48 111 //for (int i = 0; i<peaks - 1; i++){
mikeb 10:cd3f7010da48 112 // avgDist += indices1[i] - round(sampleRate/frequency*i);
mikeb 10:cd3f7010da48 113 //}
mikeb 10:cd3f7010da48 114 //avgDist = avgDist / (peaks- 1);
mikeb 10:cd3f7010da48 115 ph = indices1[0] / float(sampleRate) * float(frequency) *float(360);
mikeb 0:adae25491b93 116 return ph;
mikeb 0:adae25491b93 117 }
mikeb 0:adae25491b93 118
mikeb 0:adae25491b93 119 float Phase_Finder::estimate(float sampl[], int leng) {
mikeb 0:adae25491b93 120 length = leng;
mikeb 0:adae25491b93 121 peaks = floor(frequency / sampleRate*length);
mikeb 0:adae25491b93 122 est_Max(sampl);
mikeb 0:adae25491b93 123 return est_Phase();
mikeb 0:adae25491b93 124
mikeb 0:adae25491b93 125 }