git.gag.com Git - debian/gnuradio/blob - ezdop/src/host/hunter/src/search.cc

   1 /*
   2  Copyright 2006 Johnathan Corgan.
   3
   4  This program is free software; you can redistribute it and/or modify
   5  it under the terms of the GNU General Public License version 2
   6  as published by the Free Software Foundation.
   7
   8  This software is distributed in the hope that it will be useful,
   9  but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  GNU General Public License for more details.
  12
  13  You should have received a copy of the GNU General Public License
  14  along with GNU Radio; see the file COPYING.  If not, write to
  15  the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  16  Boston, MA 02111-1307, USA.
  17 */
  18
  19 // Application level includes
  20 #include "search.h"
  21 #include "sample.h"
  22 #include "util.h"
  23
  24 // wxWidgets includes
  25 #include <wx/log.h>
  26 #include <wx/utils.h>
  27
  28 // System level includes
  29 #include <vector>
  30
  31 using namespace std;
  32
  33 const wxEventType wxEVT_SEARCH_UPDATE = wxNewEventType();
  34
  35 SearchUpdate::SearchUpdate(const wxEventType &event, bool done) :
  36 wxNotifyEvent(event)
  37 {
  38     m_done = done;
  39 }
  40
  41 TransmitterSearch::TransmitterSearch(wxEvtHandler *dest) :
  42 m_condition(m_mutex)
  43 {
  44     Reset();
  45     m_resolution = 0.0005;  // 182 foot granularity
  46     m_dest = dest;
  47     m_log = NULL;
  48     m_scale = 0.85;    // Estimated from Mt. Umunhum data
  49     m_busy = false;
  50     Create();  // wxThreadHelper
  51     GetThread()->Run();
  52 }
  53
  54 TransmitterSearch::~TransmitterSearch()
  55 {
  56     GetThread()->Delete();
  57 }
  58
  59 void TransmitterSearch::Reset()
  60 {
  61     m_iterations = 0;
  62     m_initialized = false;
  63     m_solution = Spherical(0.0, 0.0);
  64     m_log = NULL;
  65     ClearStats();
  66 }
  67
  68 void TransmitterSearch::ClearStats()
  69 {
  70     m_histogram.Reset();
  71 }
  72
  73 void *TransmitterSearch::Entry()
  74 {
  75     wxLogDebug(_T("TransmitterSearch::Entry(): entered"));
  76     m_mutex.Lock();
  77
  78         while (!GetThread()->TestDestroy())
  79         if (m_condition.WaitTimeout(1000) == wxCOND_NO_ERROR)
  80             background_solve();
  81
  82     m_mutex.Unlock();
  83     wxLogDebug(_T("TransmitterSearch::Entry(): exited"));
  84 }
  85
  86 void TransmitterSearch::background_solve()
  87 {
  88     if (!m_log)
  89         return;
  90
  91     m_iterations = 0;
  92     int count = m_log->Count();
  93     ClearStats();
  94
  95     m_busy = true;
  96     if (count == 0)  // No data to solve from
  97         return post_update(true);
  98
  99     if (!m_initialized) { // Get initial solution from first sample, but only once
 100         m_solution = m_log->Samples().begin()->Location();
 101         m_initialized = true;
 102     }
 103
 104     if (count == 1)
 105         return post_update(true);
 106
 107     while(1) {
 108         m_iterations = 0;
 109         while (++m_iterations <= MaxIterations) {
 110             wxMutexLocker locker(m_log->Mutex());
 111             if (hillclimb(m_log->Samples())) {     // true return indicates solution of some sort achieved
 112                 m_histogram.Calc(m_log->Samples(), m_solution);
 113                 return post_update(true);
 114             }
 115         }
 116
 117         // Max iterations reached, send interim solution
 118         m_histogram.Calc(m_log->Samples(), m_solution);
 119         post_update(false);
 120     }
 121 }
 122
 123 void TransmitterSearch::post_update(bool done)
 124 {
 125     SearchUpdate update(wxEVT_SEARCH_UPDATE, done);
 126     wxPostEvent(m_dest, update);
 127     m_busy = !done;
 128 }
 129
 130 void TransmitterSearch::Solve(SampleLog *log)
 131 {
 132     // FIXME: what if we get here while background thread is still busy?
 133     if (m_log && (m_log != log))
 134         wxLogError(_T("TransmitterSearch::Solve: supplied log different from current one."));
 135
 136     m_log = log;
 137     m_condition.Signal();
 138 }
 139
 140 // Return value indicates solution of some sort achieved
 141 bool TransmitterSearch::hillclimb(vector<Sample> &samples)
 142 {
 143     int nx, ny;
 144     int min_x = 0, min_y = 0;
 145     int num;
 146
 147     Spherical trial;
 148
 149     float min_error;
 150     float trial_error;
 151
 152     // Initialize search with current solution
 153     if (calc_trial_error(samples, m_solution, min_error) == 0.0) {
 154         wxLogDebug(_T("TransmitterSearch::hillclimb: no enabled samples, returning"));
 155         return true; // No enabled data points, we're done
 156     }
 157
 158     // Check if moving 'resolution' distance in one of four directions decreases error
 159     for (nx = -1; nx < 2; nx++) {
 160         trial.SetLongitude(m_solution.Longitude() + nx*m_resolution);
 161         for (ny = -1; ny < 2; ny++) {
 162             // Skip non-compass directions
 163             if (nx == ny)
 164                 continue;
 165             trial.SetLatitude(m_solution.Latitude() + ny*m_resolution);
 166             calc_trial_error(samples, trial, trial_error);
 167             if (trial_error < min_error) {
 168                 min_error = trial_error;
 169                 min_x = nx; min_y = ny;
 170             }
 171         }
 172     }
 173
 174     // Indicate if solution achieved
 175     if (min_x == 0 && min_y == 0)
 176         return true;
 177     else {
 178         m_solution.SetLatitude(m_solution.Latitude()+min_y*m_resolution);
 179         m_solution.SetLongitude(m_solution.Longitude()+min_x*m_resolution);
 180         return false; // Make outer loop call us again
 181     }
 182 }
 183
 184 // Return value is number of enabled samples in vector
 185 float TransmitterSearch::calc_trial_error(const vector<Sample>&samples,
 186                                           const Spherical &trial,
 187                                           float &trial_error)
 188 {
 189     float wsum = 0.0;
 190     trial_error = 0.0;
 191     float strength = 1.0;
 192
 193     for (int i = 0; i < samples.size(); i++) {
 194         const Sample &sample = samples[i];
 195
 196         float angle, ierror, qerror;
 197         sample.CalcError(trial, angle, ierror, qerror);
 198
 199         // Wrapped cauchy distribution
 200         //float p = m_scale;
 201         //float likelihood = (1-p*p)/(1+p*p-2*p*cos(angle*M_PI/180.0));
 202         //trial_error += -log(likelihood)*sample.Strength();
 203
 204         // Adjusted exponential distribution
 205         trial_error += sqrt(1+angle*angle)*sample.Strength();
 206         wsum += sample.Strength();
 207     }
 208
 209     if (wsum > 0.0)
 210         trial_error = trial_error/wsum;
 211
 212     return wsum;
 213 }