m b / Mbed 2 deprecated AoA_estimator

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed dsp

Phase_Finder.h@14:8865ec38bdc8, 2016-04-28 (annotated)

Committer:
mikeb
Date:
Thu Apr 28 17:20:41 2016 +0000
Revision:
14:8865ec38bdc8
Parent:
10:cd3f7010da48
Child:
15:29805fab7655
Documented Version

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 14:8865ec38bdc8 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 14:8865ec38bdc8 34 */
mikeb 0:adae25491b93 35 float estimate(float samples[], int leng);
mikeb 0:adae25491b93 36
mikeb 0:adae25491b93 37
mikeb 0:adae25491b93 38 private:
mikeb 10:cd3f7010da48 39 float reference[SAMPLE_LENGTH+56];
mikeb 0:adae25491b93 40 float est_Phase();
mikeb 0:adae25491b93 41 void est_Max(float samples[]);
mikeb 0:adae25491b93 42 float wavelength;
mikeb 0:adae25491b93 43 float frequency;
mikeb 0:adae25491b93 44 int sampleRate;
mikeb 0:adae25491b93 45 int indices1[PEAKS];
mikeb 0:adae25491b93 46 int length;
mikeb 0:adae25491b93 47 int peaks;
mikeb 0:adae25491b93 48 float phase;
mikeb 0:adae25491b93 49
mikeb 0:adae25491b93 50 };
mikeb 10:cd3f7010da48 51 Phase_Finder::Phase_Finder(int nsampleRate, float freq, float ref[]) : sampleRate(nsampleRate), frequency(freq)
mikeb 0:adae25491b93 52 {
mikeb 10:cd3f7010da48 53 for (int i = 0; i < SAMPLE_LENGTH + 56; i++) {
mikeb 10:cd3f7010da48 54 reference[i] = ref[i];
mikeb 10:cd3f7010da48 55 }
mikeb 0:adae25491b93 56 }
mikeb 0:adae25491b93 57
mikeb 0:adae25491b93 58 void Phase_Finder::est_Max(float samples1[]) {
mikeb 8:aaf5cde0aa0a 59 float change = 0;
mikeb 14:8865ec38bdc8 60 //Remove random high amplitude distortion
mikeb 8:aaf5cde0aa0a 61 for (int i = 2; i < length - 1; i++) {
mikeb 8:aaf5cde0aa0a 62 change = abs(samples1[i - 2] - samples1[i - 1]);
mikeb 8:aaf5cde0aa0a 63 if (abs(samples1[i] - samples1[i-1]) > change*4.5)
mikeb 8:aaf5cde0aa0a 64 samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
mikeb 8:aaf5cde0aa0a 65 }
mikeb 14:8865ec38bdc8 66 //Cross correlation
mikeb 10:cd3f7010da48 67 float corr[56] = {};
mikeb 10:cd3f7010da48 68 for (int j = 0; j < 56; j++) {
mikeb 10:cd3f7010da48 69 for (int i = 0; i <= length; i++) {
mikeb 10:cd3f7010da48 70 corr[j] += reference[i-j+55] * samples1[i];
mikeb 10:cd3f7010da48 71 }
mikeb 10:cd3f7010da48 72 }
mikeb 14:8865ec38bdc8 73 //Find maximum correlation
mikeb 10:cd3f7010da48 74 float max = 0;
mikeb 10:cd3f7010da48 75 for (int i = 0; i < 56; i++) {
mikeb 10:cd3f7010da48 76 if (max < corr[i]) {
mikeb 10:cd3f7010da48 77 max = corr[i];
mikeb 10:cd3f7010da48 78 indices1[0] = i;
mikeb 10:cd3f7010da48 79 }
mikeb 10:cd3f7010da48 80 }
mikeb 14:8865ec38bdc8 81 //Peak finder method. Inferior to correlation
mikeb 10:cd3f7010da48 82 /*for (int j = 0; j<peaks; j++) {
mikeb 0:adae25491b93 83 float max = 0;
mikeb 0:adae25491b93 84
mikeb 0:adae25491b93 85 for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
mikeb 0:adae25491b93 86 if (max < samples1[i]) {
mikeb 0:adae25491b93 87 max = samples1[i];
mikeb 0:adae25491b93 88 indices1[j] = i;
mikeb 0:adae25491b93 89 }
mikeb 0:adae25491b93 90 }
mikeb 10:cd3f7010da48 91 if (indices1[j] - round(sampleRate / frequency)/2 >= 0) {
mikeb 10:cd3f7010da48 92 for (int i = -round(sampleRate / frequency)/2; i< round(sampleRate/frequency) / 2; i++) {
mikeb 0:adae25491b93 93 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 94 }
mikeb 0:adae25491b93 95 }
mikeb 0:adae25491b93 96 else {
mikeb 10:cd3f7010da48 97 for (int i = 0; i< indices1[j] + round(sampleRate / frequency) / 2; i++) {
mikeb 0:adae25491b93 98 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 99 }
mikeb 0:adae25491b93 100 }
mikeb 0:adae25491b93 101
mikeb 10:cd3f7010da48 102 }*/
mikeb 0:adae25491b93 103 }
mikeb 0:adae25491b93 104
mikeb 0:adae25491b93 105
mikeb 0:adae25491b93 106 float Phase_Finder::est_Phase() {
mikeb 10:cd3f7010da48 107 //float avgDist = 0;
mikeb 0:adae25491b93 108 float ph;
mikeb 14:8865ec38bdc8 109 //Used for peak finding averaging. Not necessary.
mikeb 10:cd3f7010da48 110 //for (int i = 0; i<peaks - 1; i++){
mikeb 10:cd3f7010da48 111 // avgDist += indices1[i] - round(sampleRate/frequency*i);
mikeb 10:cd3f7010da48 112 //}
mikeb 10:cd3f7010da48 113 //avgDist = avgDist / (peaks- 1);
mikeb 10:cd3f7010da48 114 ph = indices1[0] / float(sampleRate) * float(frequency) *float(360);
mikeb 0:adae25491b93 115 return ph;
mikeb 0:adae25491b93 116 }
mikeb 0:adae25491b93 117
mikeb 0:adae25491b93 118 float Phase_Finder::estimate(float sampl[], int leng) {
mikeb 0:adae25491b93 119 length = leng;
mikeb 0:adae25491b93 120 peaks = floor(frequency / sampleRate*length);
mikeb 0:adae25491b93 121 est_Max(sampl);
mikeb 0:adae25491b93 122 return est_Phase();
mikeb 0:adae25491b93 123
mikeb 0:adae25491b93 124 }