Merge pull request #12 from acoustic-warfare/pso

Changed horizontal to spherical angles
acoustic-warfare · Jul 2, 2024 · 2a0f859 · 2a0f859
2 parents 926f533 + dee1266
commit 2a0f859
Show file tree

Hide file tree

Showing 4 changed files with 85 additions and 83 deletions.
diff --git a/config.yaml b/config.yaml
@@ -57,7 +57,7 @@ LOCAL_AREA_RATIO: 0.7
 GLOBAL_AREA_RATIO: 1.0 - LOCAL_AREA_RATIO
 
 # Camera
-FOV: 100.0 # How far we should look to the side
+FOV: 45.0 # How far we should look to the side
 CAMERA: false
 CAMERA_PATH: "/dev/video0"
 
@@ -69,7 +69,7 @@ IMAGE_PREVIOUS_WEIGHTED_RATIO: 1.0 - IMAGE_CURRENT_WEIGHTED_RATIO
 # Beamforming
 PI: 3.1415926535897
 TWO_PI: 2.0 * PI
-ANGLE_LIMIT: PI / 4.0
+ANGLE_LIMIT: FOV * PI / 180.0 #PI / 4.0 * 2.0
 
 USE_KALMAN_FILTER: true
 

diff --git a/src/algorithms/pso_seeker.cpp b/src/algorithms/pso_seeker.cpp
@@ -31,47 +31,54 @@ float beamform(Streams *streams, int *offset_delays, float *fractional_delays) {
 
 
 Particle::Particle(Antenna &antenna, Streams *streams) : antenna(antenna), streams(streams) {
-    azimuth = (static_cast<float>(rand()) / RAND_MAX - 0.5f) * 2.0 * ANGLE_LIMIT;
-    elevation = (static_cast<float>(rand()) / RAND_MAX - 0.5f) * 2.0 * ANGLE_LIMIT;
-    velocity_azimuth = 0.0f;
-    velocity_elevation = 0.0f;
-    best_azimuth = azimuth;
-    best_elevation = elevation;
-    best_magnitude = compute(azimuth, elevation);
+    //theta = (static_cast<float>(rand()) / RAND_MAX - 0.5f) * 2.0 * ANGLE_LIMIT;
+    //phi = (static_cast<float>(rand()) / RAND_MAX - 0.5f) * 2.0 * ANGLE_LIMIT;
+    random();
+    velocity_theta = 0.0f;
+    velocity_phi = 0.0f;
+    best_theta = theta;
+    best_phi = phi;
+    best_magnitude = compute(theta, phi);
 }
 
 
 void Particle::random() {
-    azimuth = (static_cast<float>(rand()) / RAND_MAX - 0.5f) * 2.0 * ANGLE_LIMIT;
-    elevation = (static_cast<float>(rand()) / RAND_MAX - 0.5f) * 2.0 * ANGLE_LIMIT;
+  theta = (static_cast<float>(rand()) / RAND_MAX - 0.5f) * 2.0 * (2.0 * PI);
+  phi = (static_cast<float>(rand()) / RAND_MAX - 0.5f) * 2.0 * PI_HALF;
+  //particle.theta = wrapAngle(particle.theta);
+  //particle.phi = clip(particle.phi, 0.0, PI_HALF);
+
 }
 
-float Particle::compute(double azimuth, double elevation) {
-    Eigen::VectorXf tmp_delays = steering_vector_horizontal(antenna, azimuth, elevation);
-    float fractional_delays[N_SENSORS];
-    int offset_delays[N_SENSORS];
-    int i = 0;
-    for (float del: tmp_delays) {
-        double _offset;
-        float fraction;
-        fraction = (float) modf((double) del, &_offset);
-        int offset = N_SAMPLES - (int) _offset;
-        // cout << del << endl;
-        fractional_delays[i] = fraction;
-        offset_delays[i] = offset;
-        i++;
-    }
-    return beamform(streams, &offset_delays[0], &fractional_delays[0]);
+float Particle::compute(double theta, double phi) {
+  Eigen::VectorXf tmp_delays = steering_vector_spherical(antenna, theta, phi);
+  //Eigen::VectorXf tmp_delays = steering_vector_horizontal(antenna, azimuth, elevation);
+  float fractional_delays[N_SENSORS];
+  int offset_delays[N_SENSORS];
+  int i = 0;
+  for (float del : tmp_delays) {
+    //std::cout << "Del: " << del << std::endl;
+    double _offset;
+    float fraction;
+    fraction = (float)modf((double)del, &_offset);
+    int offset = N_SAMPLES - (int)_offset;
+    // cout << del << endl;
+    fractional_delays[i] = fraction;
+    offset_delays[i] = offset;
+    i++;
+  }
+  //std::cout << "azimuth: " << azimuth << " elevation: " << elevation << std::endl;
+  return beamform(streams, &offset_delays[0], &fractional_delays[0]);
 }
 
 void Particle::update() {
-    float magnitude = compute(azimuth, elevation);
-    best_magnitude *= 0.99;
-    if (magnitude > best_magnitude) {
-        best_magnitude = magnitude;
-        best_azimuth = azimuth;
-        best_elevation = elevation;
-    }
+  float magnitude = compute(theta, phi);
+  best_magnitude *= 0.99;
+  if (magnitude > best_magnitude) {
+    best_magnitude = magnitude;
+    best_theta = theta;
+    best_phi = phi;
+  }
 }
 
 
@@ -106,34 +113,37 @@ void PSO::initialize_particles() {
 }
 
 void PSO::optimize(int iterations) {
-    double w = 0.5f, c1 = 2.0f, c2 = 2.0f;
-    double amount = 1.5;
-    double delta = TO_RADIANS(FOV / (double) iterations * 2.0) * amount;
-    global_best_magnitude *= 0.9f;
-
-    for (int i = 0; i < iterations; i++) {
-        for (auto &particle: particles) {
-            particle.velocity_azimuth = w * particle.velocity_azimuth + c1 * static_cast<double>(rand()) / RAND_MAX * (particle.best_azimuth - particle.azimuth) * LOCAL_AREA_RATIO + c2 * static_cast<double>(rand()) / RAND_MAX * (global_best_azimuth - particle.azimuth) * GLOBAL_AREA_RATIO;
-            particle.velocity_elevation = w * particle.velocity_elevation + c1 * static_cast<double>(rand()) / RAND_MAX * (particle.best_elevation - particle.elevation) * LOCAL_AREA_RATIO + c2 * static_cast<double>(rand()) / RAND_MAX * (global_best_elevation - particle.elevation) * GLOBAL_AREA_RATIO;
-            particle.azimuth += particle.velocity_azimuth * delta;
-            particle.elevation += particle.velocity_elevation * delta;
-
-            particle.azimuth = clip(particle.azimuth, -ANGLE_LIMIT, ANGLE_LIMIT);
-            particle.elevation = clip(particle.elevation, -ANGLE_LIMIT, ANGLE_LIMIT);
-
-            particle.update();
-
-            if (particle.best_magnitude > global_best_magnitude) {
-                global_best_magnitude = particle.best_magnitude;
-                global_best_azimuth = particle.best_azimuth;
-                global_best_elevation = particle.best_elevation;
-            }
-        }
+  double w = 0.5f, c1 = 2.0f, c2 = 2.0f;
+  double amount = 10.0;
+  double delta = TO_RADIANS(FOV / (double)iterations * 2.0) * amount;
+  global_best_magnitude *= 0.9f;
+
+  for (int i = 0; i < iterations; i++) {
+    for (auto& particle : particles) {
+      particle.velocity_theta = w * particle.velocity_theta \
+      + c1 * static_cast<double>(rand()) / RAND_MAX * (particle.best_theta - particle.theta) * LOCAL_AREA_RATIO \
+      + c2 * static_cast<double>(rand()) / RAND_MAX * (global_best_theta - particle.theta) * GLOBAL_AREA_RATIO;
+      particle.velocity_phi = w * particle.velocity_phi \
+      + c1 * static_cast<double>(rand()) / RAND_MAX * (particle.best_phi - particle.phi) * LOCAL_AREA_RATIO \
+      + c2 * static_cast<double>(rand()) / RAND_MAX * (global_best_phi - particle.phi) * GLOBAL_AREA_RATIO;
+      particle.theta += particle.velocity_theta * delta;
+      particle.phi += particle.velocity_phi * delta;
+
+      particle.theta = wrapAngle(particle.theta);
+      particle.phi = clip(particle.phi, 0.0, PI_HALF);
+
+      particle.update();
+
+      if (particle.best_magnitude > global_best_magnitude) {
+        global_best_magnitude = particle.best_magnitude;
+        global_best_theta = particle.best_theta;
+        global_best_phi = particle.best_phi;
+      }
     }
 }
 
 Eigen::Vector3f PSO::sanitize() {
-    Eigen::Vector3f sample(global_best_azimuth, global_best_elevation, 0.0);
+  Eigen::Vector3f sample(global_best_theta, global_best_phi, 0.0);
 
 #if USE_KALMAN_FILTER
     kf.update(sample);
@@ -148,7 +158,7 @@ void pso_finder(Pipeline *pipeline) {
     Antenna antenna = create_antenna(Position(0, 0, 0), COLUMNS, ROWS, DISTANCE);
     Streams *streams = pipeline->getStreams();
 
-    PSO pso(40, antenna, streams);
+    PSO pso(100, antenna, streams);
 
     int newData;
 
@@ -168,41 +178,32 @@ void pso_finder(Pipeline *pipeline) {
 
         pso.optimize(30);
 
-        //Eigen::Vector3f sample = pso.sanitize();
-        //float azimuth = sample(0);
-        //float elevation = sample(1);
-        //azimuth = clip(azimuth, -ANGLE_LIMIT, ANGLE_LIMIT);
-        //elevation = clip(elevation, -ANGLE_LIMIT, ANGLE_LIMIT);
-
         pipeline->magnitudeHeatmap->setTo(cv::Scalar(0));
 
 #if 1
         for (auto &particle: pso.particles) {
-            double azimuth = particle.best_azimuth;
-            double elevation = particle.best_elevation;
-            int xi = (int) ((double) X_RES * (sin(azimuth) / 2.0 + 0.5));
-            int yi = (int) ((double) Y_RES * (sin(elevation) / 2.0 + 0.5));
+            Position position = spherical_to_cartesian(particle.best_theta, particle.best_phi, 1.0);
+            int xi = (int)((double)X_RES * (position(0) / 2.0 + 0.5));
+            int yi = (int)((double)Y_RES * (position(1) / 2.0 + 0.5));
 
             pipeline->magnitudeHeatmap->at<uchar>(xi, yi) = (uchar) (255);
         }
 #else
 
         Eigen::Vector3f res = pso.sanitize();
 
-        double x = sin((double) res(0));
-        double y = sin((double) res(1));
+        Position position = spherical_to_cartesian(res(0), res(1), 1.0);
 
-        int xi = (int) ((double) X_RES * (x / 2.0 + 0.5));
-        int yi = (int) ((double) Y_RES * (y / 2.0 + 0.5));
+        int xi = (int)((double)X_RES * (position(0) / 2.0 + 0.5));
+        int yi = (int)((double)Y_RES * (position(1) / 2.0 + 0.5));
 
         pipeline->magnitudeHeatmap->at<uchar>(xi, yi) = (uchar) (255);
 
 #endif
 
-        //prevX = xi;
-        //prevY = yi;
+
         pipeline->canPlot = 1;
-        //std::cout << "(" << x << ", " << y << ")" << std::endl;
-        std::cout << "Theta: " << pso.global_best_azimuth << " Phi: " << pso.global_best_elevation << std::endl;
+
+        std::cout << "Theta: " << pso.global_best_theta << " Phi: " << pso.global_best_phi << std::endl;
     }
 }
diff --git a/src/algorithms/pso_seeker.h b/src/algorithms/pso_seeker.h
@@ -20,9 +20,9 @@
 
 class Particle {
 public:
-    double azimuth, elevation;
-    double velocity_azimuth, velocity_elevation;
-    double best_azimuth, best_elevation;
+    double theta, phi;
+    double velocity_theta, velocity_phi;
+    double best_theta, best_phi;
     float best_magnitude;
 
     Antenna &antenna;
@@ -34,7 +34,7 @@ class Particle {
 
     void random();
 
-    float compute(double azimuth, double elevation);
+    float compute(double theta, double phi);
 
     void update();
 };
@@ -43,12 +43,13 @@ class Particle {
 class PSO {
 public:
     std::vector<Particle> particles;
-    double global_best_azimuth, global_best_elevation;
+    double global_best_theta, global_best_phi;
     float global_best_magnitude;
     int n_particles;
     Antenna &antenna;
     Streams *streams;
 
+
 #if USE_KALMAN_FILTER
     KalmanFilter3D kf;
 #endif

diff --git a/src/antenna.cpp b/src/antenna.cpp
@@ -138,7 +138,7 @@ Eigen::VectorXf steering_vector_horizontal(const Antenna &antenna, const double
     double x = sin(azimuth);
     double y = sin(elevation);
     double theta = atan2(y, x);
-    double phi = (PI_HALF - asin(sqrt(1 - pow(x, 2) - pow(y, 2))));
+    double phi = PI_HALF - asin(1.0 - pow(x, 2) - pow(y, 2));
 
     Antenna steered = steer(antenna, theta, phi);
     return compute_delays(steered);