1 /*
2 *
3 * Copyright (c) 2005 The Regents of the University of California. All
4 * rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 *
10 * - Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 *
13 * - Neither the name of the University nor the names of its
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
19 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
20 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
21 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
22 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
23 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
24 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
25 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 *
29 */
30
31 #include <ar.h>
32
33 /*
34 * ar_orig_doa.c
35 *
36 * original (broken) DOA algorithm
37 */
38
39 #undef REQUIRE_3_MAXES
40 #define MAX_DIFF (M_PI/6.0)
41
42 void ar_spatial_filter(complex *fft_filtered, float theta, float phi, float array[4][3],
43 complex *ffts[], size_t points, float offsets[4])
44 {
45 int i;
46 /* phase shift and add each fft */
47 complex *shift = g_new(complex,points);
48 memset(fft_filtered, 0, sizeof(complex)*points);
49 for (i=0; i<4; i++) {
50 nl_copy_vector(shift, ffts[i], points);
51 nl_phase_shift(shift, points, offsets[i]);
52 nl_accum(fft_filtered, shift, points);
53 }
54 free(shift);
55 }
56
57
58 float ar_test_angle_frac(float theta, float phi, float array[4][3],
59 complex *ffts[], size_t points)
60 {
61 float offsets[4];
62 complex *shifts[4] = {};
63 int i,j;
64 float *tmp = g_new(float,points);
65 float *td = g_new(float,points);
66
67 /* compute the phase offsets for this angle */
68 ar_compute_offsets(theta, phi, array, offsets);
69
70 /* fft -> shift -> ifft */
71 for (i=0; i<4; i++) {
72 shifts[i] = g_new(complex,points);
73 nl_copy_vector(shifts[i], ffts[i], points);
74 nl_phase_shift(shifts[i], points, offsets[i]);
75 nl_ifft(shifts[i], td, points);
76 for (j=0; j<points; j++) {
77 if (i == 0) tmp[j] = td[j];
78 else tmp[j] *= td[j];
79 }
80 }
81
82 /* accumulate */
83 float mac = 0;
84 for (j=0; j<points; j++) {
85 mac += tmp[j];
86 }
87
88 /* free */
89 ar_complex_vector_free(shifts);
90 free(tmp);
91 free(td);
92
93 return mac / (float)(256*points);
94 }
95
96
97 float ar_test_angle(float theta, float phi, float array[4][3],
98 float *tds[], size_t points)
99 {
100 float offsets[4];
101 int i,j;
102
103 /* compute the phase offsets for this angle */
104 ar_compute_offsets(theta, phi, array, offsets);
105
106 /* round the offsets off to nearest sample*/
107 int rounded[4];
108 for (j=0; j<4; j++)
109 rounded[j] = -offsets[j];
110
111 float mac = 0;
112 float *tmp = g_new(float,points);
113
114 /* init to 1's */
115 for (j=0; j<points; j++)
116 tmp[j] = 1;
117
118 for (i=0; i<4; i++) {
119 if (rounded[i]>=0)
120 for (j=0; j<(points-rounded[i]); j++) {
121 tmp[j] *= tds[i][j+rounded[i]];
122 }
123 else
124 for (j=-rounded[i]; j<points; j++) {
125 tmp[j] *= tds[i][j+rounded[i]];
126 }
127 }
128
129 /* accumulate */
130 for (j=0; j<points; j++) {
131 mac += tmp[j];
132 }
133
134 /* free */
135 free(tmp);
136
137 return mac / (float)(256*points);
138 }
139
140
141 /* find maxes and then cross-correlate each pair of correlation
142 * funcs in the time domain */
143 int ar_find_doa(ar_state_t *ar, float array[4][3], float *correls[], complex *fft_input[],
144 complex *fft_ref, float *correl_filtered, size_t chirp_len,
145 float *theta, float *phi, float *SNR,
146 int32_t *max_ind, float *max, float *conf,
147 int32_t *uncombined_range, char **reason)
148 {
149 #define TD_MAX_OFFSET 64
150 #define TD_CORREL_LEN (TD_MAX_OFFSET*2)
151 float *correl_snip[4] = {};
152 complex *ffts[4]={};
153 float maxes[4];
154 int max_indices[4];
155 int channel_near[4];
156 int retval = -1;
157
158 /* find maxes */
159 int i,j;
160 for (i=0; i<4; i++) {
161 ar_find_max(&(max_indices[i]), &(maxes[i]), ALPHA, STARTUP_WINDOW, STD_DEV_FACTOR,
162 correls[i], chirp_len, NULL);
163 elog(LOG_WARNING, "Found peak of %f at %d, channel %d", maxes[i], max_indices[i], i);
164 }
165
166 /* reject maxes that are too far from other maxes */
167 for (i=0; i<4; i++) {
168 channel_near[i] = 0;
169 for (j=0; j<4; j++) {
170 if (abs(max_indices[i] - max_indices[j]) <= TD_MAX_OFFSET)
171 channel_near[i]++;
172 }
173 if (channel_near[i] == 1) {
174 elog(LOG_WARNING, "rejecting max of channel %d (%d/%f)", i,
175 max_indices[i], maxes[i]);
176 }
177 #ifdef REQUIRE_3_MAXES
178 /* reject maxes that are near just one other max */
179 else
180 if (channel_near[i] == 2) {
181 *reason = "maxes are too inconsistent.. giving up!";
182 goto fail;
183 }
184 #endif
185 }
186
187 /* OK, find the earliest max */
188 int earliest_max = -1;
189 int earliest_max_channel = -1;
190 for (i=0; i<4; i++) {
191 if (channel_near[i] != 1) {
192 if (earliest_max < 0 || earliest_max > max_indices[i]) {
193 earliest_max = max_indices[i];
194 earliest_max_channel = i;
195 *uncombined_range = earliest_max;
196 }
197 }
198 }
199 if (earliest_max == -1) {
200 *reason = "no maxes are near each other! giving up!";
201 goto fail;
202 }
203
204 /* back up a little */
205 earliest_max -= TD_MAX_OFFSET/2;
206 if (earliest_max < 0) {
207 elog(LOG_WARNING, "max was really close to start.. %d", earliest_max);
208 earliest_max = 0;
209 }
210 if ((earliest_max + TD_CORREL_LEN) > chirp_len) {
211 *reason = "max was really really late.. giving up";
212 goto fail;
213 }
214
215 /* alloc space for snipped correls */
216 for (i=0; i<4; i++) {
217 correl_snip[i] = g_new(float,TD_CORREL_LEN);
218 for (j=0; j<TD_CORREL_LEN; j++) {
219 correl_snip[i][j] = (correls[i][j+earliest_max]);
220 }
221 }
222
223 /* test angles */
224 float test[PHI_COUNT][THETA_COUNT];
225 for (j=0; j<PHI_COUNT; j++)
226 for (i=0; i<THETA_COUNT; i++) {
227 test[j][i] = ar_test_angle(THETA(i), PHI(j), array, correl_snip, TD_CORREL_LEN);
228 }
229
230 if (ar->dump_files_ctrl_string) {
231 char buf[256];
232 sprintf(buf, ar->dump_files_ctrl_string, "hyp", ar->chirp_index);
233 FILE *f = fopen(buf, "w");
234 if (f) {
235 fprintf(f, "#phi theta val\n");
236 for (i=0; i<PHI_COUNT; i++) {
237 for (j=0; j<THETA_COUNT; j++) {
238 fprintf(f, "%d %d %f \n",
239 i, j,
240 test[i][j]);
241 }
242 fprintf(f, "\n");
243 }
244 fclose(f);
245 }
246 else {
247 elog(LOG_WARNING, "Can't open hyp output file %s: %m", buf);
248 }
249 }
250
251 /* seek centroid of max */
252 float theta_sum = 0;
253 int count = 0;
254 float phi_sum = 0;
255 float max_value = 0;
256 int max_set = 0;
257 int bimodal = 0;
258
259 for (i=0; i<PHI_COUNT; i++) {
260 float this_phi = PHI(i);
261 for (j=0; j<THETA_COUNT; j++) {
262 float this_theta = THETA(j);
263 if (!max_set || test[i][j] > max_value) {
264 max_set = 1;
265 max_value = test[i][j];
266 theta_sum = 0;
267 phi_sum = 0;
268 count = 0;
269 bimodal = 0;
270 }
271 if (test[i][j] == max_value) {
272 /* check against average */
273 if (theta_sum > 0) {
274 float avg = theta_sum / (float)count;
275 float phi_avg = phi_sum / (float)count;
276 if (fabsf(avg - this_theta) > MAX_DIFF) {
277 if (avg < this_theta) this_theta -= (2*M_PI);
278 else this_theta += (2*M_PI);
279 }
280 if (fabsf(avg - this_theta) > MAX_DIFF) {
281 elog(LOG_WARNING, "bimodal theta?");
282 bimodal = 1;
283 }
284 if (fabsf(phi_avg - this_phi) > MAX_DIFF) {
285 elog(LOG_WARNING, "bimodal phi?");
286 bimodal = 1;
287 }
288 }
289 theta_sum += this_theta;
290 phi_sum += this_phi;
291 count++;
292 }
293 }
294 }
295
296 if (bimodal) {
297 *reason = "bimodal!";
298 goto fail;
299 }
300
301 if (count == 0) {
302 *reason = "no data?";
303 goto fail;
304 }
305
306 *theta = (theta_sum / (float)count);
307 *phi = (phi_sum / (float)count);
308
309 /* DOA SNR -- max / RMS average */
310 *SNR = 0;
311 int noise_count = 0;
312 int signal_count = 0;
313 for (i=0; i<THETA_COUNT; i++) for (j=0; j<PHI_COUNT; j++) {
314 if (test[j][i] != max_value) {
315 *SNR += sqrf(test[j][i]);
316 noise_count++;
317 }
318 else
319 signal_count++;
320 }
321 if (noise_count > 0)
322 *SNR = (max_value * (float)signal_count) /
323 sqrtf(*SNR/(float)noise_count);
324
325 elog(LOG_WARNING, "estimate theta=%f, phi=%f, SNR=%f",
326 (*theta)*180.0/M_PI, (*phi)*180.0/M_PI, *SNR);
327
328 /* fractional phase search mode */
329 float frac_phi = 0;
330 float frac_theta = 0;
331 float frac_set = 0;
332 float frac_max = 0;
333
334 /* alloc and fft the snippets */
335 for (i=0; i<4; i++) {
336 ffts[i] = g_new(complex,TD_CORREL_LEN);
337 }
338 nl_2fft(correl_snip[0], correl_snip[1], ffts[0], ffts[1], TD_CORREL_LEN);
339 nl_2fft(correl_snip[2], correl_snip[3], ffts[2], ffts[3], TD_CORREL_LEN);
340
341 for (i=0; i<THETA_COUNT; i++) for (j=0; j<PHI_COUNT; j++) {
342 if (test[j][i] == max_value) {
343 float frac_value =
344 ar_test_angle_frac(THETA(i), PHI(j), array, ffts, TD_CORREL_LEN);
345 elog(LOG_WARNING, "zooming.. %d,%d -> %f", i, j, frac_value);
346 if (frac_value > frac_max || frac_set == 0) {
347 frac_max = frac_value;
348 frac_phi = PHI(j);
349 frac_theta = THETA(i);
350 frac_set = 1;
351 }
352 }
353 }
354
355 /* free fft vecs */
356 ar_complex_vector_free(ffts);
357
358 if (frac_set == 0) {
359 *reason = "can't find any peak points!";
360 goto fail;
361 }
362
363 *phi = frac_phi;
364 *theta = frac_theta;
365
366 /* spatial filter */
367 float offsets[4];
368 complex *fft_filtered = g_new(complex,chirp_len);
369 complex *fft_correl_filtered = g_new(complex,chirp_len);
370
371 /* compute the phase offsets for this angle */
372 ar_compute_offsets(*theta, *phi, array, offsets);
373
374 /* filter */
375 ar_spatial_filter(fft_filtered, *theta, *phi, array, fft_input, chirp_len, offsets);
376
377 /* convolve and inverse */
378 nl_fd_correl(fft_correl_filtered, correl_filtered, fft_ref, fft_filtered, chirp_len);
379
380 float std_dev = 0;
381 ar_find_max(max_ind, max, ALPHA, STARTUP_WINDOW, STD_DEV_FACTOR,
382 correl_filtered, chirp_len, &std_dev);
383 *conf = std_dev ? *max / std_dev : 0;
384 *max_ind *= CLIP_RESAMPLE;
385
386 retval = 0;
387
388 free(fft_filtered);
389 free(fft_correl_filtered);
390
391 fail:
392 ar_float_vector_free(correl_snip);
393 return retval;
394 }
395
396
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.