Return to ar_orig_doa.c CVS log

Jump to this file's LXR Page

Up to [CENS] / emstar / devel / loc / ar

added missing file
also fixed a few minor bugs in reporting code

/*
 *
 * Copyright (c) 2005 The Regents of the University of California.  All 
 * rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * - Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * - Neither the name of the University nor the names of its
 *   contributors may be used to endorse or promote products derived
 *   from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <ar.h>

/*
 * ar_orig_doa.c
 *
 *   original (broken) DOA algorithm 
 */

#undef REQUIRE_3_MAXES
#define MAX_DIFF (M_PI/6.0)

void ar_spatial_filter(complex *fft_filtered, float theta, float phi, float array[4][3],
		       complex *ffts[], size_t points, float offsets[4])
{
  int i;
  /* phase shift and add each fft */
  complex *shift = g_new(complex,points);
  memset(fft_filtered, 0, sizeof(complex)*points);
  for (i=0; i<4; i++) {
    nl_copy_vector(shift, ffts[i], points);
    nl_phase_shift(shift, points, offsets[i]);
    nl_accum(fft_filtered, shift, points);
  }
  free(shift);
}


float ar_test_angle_frac(float theta, float phi, float array[4][3], 
			 complex *ffts[], size_t points)
{
  float offsets[4];
  complex *shifts[4] = {};
  int i,j;
  float *tmp = g_new(float,points);
  float *td = g_new(float,points);

  /* compute the phase offsets for this angle */
  ar_compute_offsets(theta, phi, array, offsets);

  /* fft -> shift -> ifft */
  for (i=0; i<4; i++) {
    shifts[i] = g_new(complex,points);
    nl_copy_vector(shifts[i], ffts[i], points);
    nl_phase_shift(shifts[i], points, offsets[i]);
    nl_ifft(shifts[i], td, points);
    for (j=0; j<points; j++) {
      if (i == 0) tmp[j] = td[j];
      else tmp[j] *= td[j];
    }
  }
    
  /* accumulate */
  float mac = 0;
  for (j=0; j<points; j++) {
    mac += tmp[j];
  }
  
  /* free */
  ar_complex_vector_free(shifts);
  free(tmp);
  free(td);

  return mac / (float)(256*points);
}


float ar_test_angle(float theta, float phi, float array[4][3], 
		    float *tds[], size_t points)
{
  float offsets[4];
  int i,j;
  
  /* compute the phase offsets for this angle */
  ar_compute_offsets(theta, phi, array, offsets);

  /* round the offsets off to nearest sample*/
  int rounded[4];
  for (j=0; j<4; j++)
    rounded[j] = -offsets[j];
  
  float mac = 0;
  float *tmp = g_new(float,points);

  /* init to 1's */
  for (j=0; j<points; j++)
    tmp[j] = 1;
  
  for (i=0; i<4; i++) {
    if (rounded[i]>=0) 
      for (j=0; j<(points-rounded[i]); j++) {
	tmp[j] *= tds[i][j+rounded[i]];
      }
    else
      for (j=-rounded[i]; j<points; j++) {
	tmp[j] *= tds[i][j+rounded[i]];
      }
  }

  /* accumulate */
  for (j=0; j<points; j++) {
    mac += tmp[j];
  }

  /* free */
  free(tmp);  

  return mac / (float)(256*points);
}


/* find maxes and then cross-correlate each pair of correlation 
 * funcs in the time domain */
int ar_find_doa(ar_state_t *ar, float array[4][3], float *correls[], complex *fft_input[], 
		complex *fft_ref, float *correl_filtered, size_t chirp_len, 
		float *theta, float *phi, float *SNR, 
		int32_t *max_ind, float *max, float *conf,
		int32_t *uncombined_range, char **reason)
{
#define TD_MAX_OFFSET 64
#define TD_CORREL_LEN (TD_MAX_OFFSET*2)
  float *correl_snip[4] = {};
  complex *ffts[4]={};
  float maxes[4];
  int max_indices[4];
  int channel_near[4];
  int retval = -1;

  /* find maxes */
  int i,j;
  for (i=0; i<4; i++) {
    ar_find_max(&(max_indices[i]), &(maxes[i]), ALPHA, STARTUP_WINDOW, STD_DEV_FACTOR,
		correls[i], chirp_len, NULL);
    elog(LOG_WARNING, "Found peak of %f at %d, channel %d", maxes[i], max_indices[i], i); 
  }

  /* reject maxes that are too far from other maxes */
  for (i=0; i<4; i++) {
    channel_near[i] = 0;
    for (j=0; j<4; j++) {
      if (abs(max_indices[i] - max_indices[j]) <= TD_MAX_OFFSET) 
	channel_near[i]++;
    }
    if (channel_near[i] == 1) {
      elog(LOG_WARNING, "rejecting max of channel %d (%d/%f)", i, 
	   max_indices[i], maxes[i]);
    }
#ifdef REQUIRE_3_MAXES
    /* reject maxes that are near just one other max */
    else
      if (channel_near[i] == 2) {
	*reason = "maxes are too inconsistent.. giving up!";
	goto fail;
      }
#endif
  }

  /* OK, find the earliest max */
  int earliest_max = -1;
  int earliest_max_channel = -1;
  for (i=0; i<4; i++) {
    if (channel_near[i] != 1) {
      if (earliest_max < 0 || earliest_max > max_indices[i]) {
	earliest_max = max_indices[i];
	earliest_max_channel = i;
	*uncombined_range = earliest_max;
      }
    }
  }
  if (earliest_max == -1) {
    *reason = "no maxes are near each other!  giving up!";
    goto fail;
  }

  /* back up a little */
  earliest_max -= TD_MAX_OFFSET/2;
  if (earliest_max < 0) {
    elog(LOG_WARNING, "max was really close to start.. %d", earliest_max);
    earliest_max = 0;
  }
  if ((earliest_max + TD_CORREL_LEN) > chirp_len) {
    *reason = "max was really really late.. giving up";
    goto fail;
  }

  /* alloc space for snipped correls */
  for (i=0; i<4; i++) {
    correl_snip[i] = g_new(float,TD_CORREL_LEN);
    for (j=0; j<TD_CORREL_LEN; j++) {
      correl_snip[i][j] = (correls[i][j+earliest_max]);
    }
  }
  
  /* test angles */
  float test[PHI_COUNT][THETA_COUNT];
  for (j=0; j<PHI_COUNT; j++) 
    for (i=0; i<THETA_COUNT; i++) {
      test[j][i] = ar_test_angle(THETA(i), PHI(j), array, correl_snip, TD_CORREL_LEN);
    }
  
  if (ar->dump_files_ctrl_string) {
    char buf[256];
    sprintf(buf, ar->dump_files_ctrl_string, "hyp", ar->chirp_index);
    FILE *f = fopen(buf, "w");
    if (f) {
      fprintf(f, "#phi theta val\n");
      for (i=0; i<PHI_COUNT; i++) {  
	for (j=0; j<THETA_COUNT; j++) {
	  fprintf(f, "%d %d %f \n",
		  i, j,
		  test[i][j]);
	}
	fprintf(f, "\n");
      }
      fclose(f);
    }
    else {
      elog(LOG_WARNING, "Can't open hyp output file %s: %m", buf);
    }
  }

  /* seek centroid of max */
  float theta_sum = 0;
  int count = 0;
  float phi_sum = 0;
  float max_value = 0;
  int max_set = 0;
  int bimodal = 0;

  for (i=0; i<PHI_COUNT; i++) {
    float this_phi = PHI(i);
    for (j=0; j<THETA_COUNT; j++) {
      float this_theta = THETA(j);
      if (!max_set || test[i][j] > max_value) {
	max_set = 1;
	max_value = test[i][j];
	theta_sum = 0;
	phi_sum = 0;
	count = 0;
	bimodal = 0;
      }
      if (test[i][j] == max_value) {
	/* check against average */
	if (theta_sum > 0) {
	  float avg = theta_sum / (float)count;
	  float phi_avg = phi_sum / (float)count;
	  if (fabsf(avg - this_theta) > MAX_DIFF) {
	    if (avg < this_theta) this_theta -= (2*M_PI);
	    else this_theta += (2*M_PI);
	  }
	  if (fabsf(avg - this_theta) > MAX_DIFF) {
	    elog(LOG_WARNING, "bimodal theta?");
	    bimodal = 1;
	  }
	  if (fabsf(phi_avg - this_phi) > MAX_DIFF) {
	    elog(LOG_WARNING, "bimodal phi?");
	    bimodal = 1;
	  }
	}
	theta_sum += this_theta;
	phi_sum += this_phi;
	count++;
      }
    }
  }

  if (bimodal) {
    *reason = "bimodal!";
    goto fail;
  }

  if (count == 0) {
    *reason = "no data?";
    goto fail;
  }

  *theta = (theta_sum / (float)count);
  *phi = (phi_sum / (float)count);

  /* DOA SNR -- max / RMS average */
  *SNR = 0;
  int noise_count = 0;
  int signal_count = 0;
  for (i=0; i<THETA_COUNT; i++) for (j=0; j<PHI_COUNT; j++) {
    if (test[j][i] != max_value) {
      *SNR += sqrf(test[j][i]);
      noise_count++;
    }
    else
      signal_count++;
  }
  if (noise_count > 0)
    *SNR = (max_value * (float)signal_count) / 
      sqrtf(*SNR/(float)noise_count);
  
  elog(LOG_WARNING, "estimate theta=%f, phi=%f, SNR=%f",
       (*theta)*180.0/M_PI, (*phi)*180.0/M_PI, *SNR);
  
  /* fractional phase search mode */
  float frac_phi = 0;
  float frac_theta = 0;
  float frac_set = 0;
  float frac_max = 0;

  /* alloc and fft the snippets */
  for (i=0; i<4; i++) {
    ffts[i] = g_new(complex,TD_CORREL_LEN);
  }
  nl_2fft(correl_snip[0], correl_snip[1], ffts[0], ffts[1], TD_CORREL_LEN);
  nl_2fft(correl_snip[2], correl_snip[3], ffts[2], ffts[3], TD_CORREL_LEN);

  for (i=0; i<THETA_COUNT; i++) for (j=0; j<PHI_COUNT; j++) {
    if (test[j][i] == max_value) {
      float frac_value = 
	ar_test_angle_frac(THETA(i), PHI(j), array, ffts, TD_CORREL_LEN);
      elog(LOG_WARNING, "zooming.. %d,%d -> %f", i, j, frac_value);
      if (frac_value > frac_max || frac_set == 0) {
	frac_max = frac_value;
	frac_phi = PHI(j);
	frac_theta = THETA(i);
	frac_set = 1;
      }
    }
  }  

  /* free fft vecs */
  ar_complex_vector_free(ffts);

  if (frac_set == 0) {
    *reason = "can't find any peak points!";
    goto fail;
  }

  *phi = frac_phi;
  *theta = frac_theta;

  /* spatial filter */
  float offsets[4];
  complex *fft_filtered = g_new(complex,chirp_len);
  complex *fft_correl_filtered = g_new(complex,chirp_len);

  /* compute the phase offsets for this angle */
  ar_compute_offsets(*theta, *phi, array, offsets);
  
  /* filter */
  ar_spatial_filter(fft_filtered, *theta, *phi, array, fft_input, chirp_len, offsets);

  /* convolve and inverse */
  nl_fd_correl(fft_correl_filtered, correl_filtered, fft_ref, fft_filtered, chirp_len);    
  
  float std_dev = 0;
  ar_find_max(max_ind, max, ALPHA, STARTUP_WINDOW, STD_DEV_FACTOR, 
	      correl_filtered, chirp_len, &std_dev);
  *conf = std_dev ? *max / std_dev : 0;
  *max_ind *= CLIP_RESAMPLE;

  retval = 0;

  free(fft_filtered);
  free(fft_correl_filtered);

 fail:  
  ar_float_vector_free(correl_snip);
  return retval;
}