small pf bug seems fixed by just slightly increasing the gps noise

autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp

@@ -118,7 +118,7 @@ class ParticleFilter {
             //printf("new particles in feedback: %f, %f, %f, %f\n", this->particles[i].x, this->particles[i].y, this->particles[i].theta, this->particles[i].weight);
             //printf("\n====== END FEEDBACK ======\n\n");
             return feedback_vector;
-        }
+        };
 
         std::vector<double> gps(autonav_msgs::msg::GPSFeedback gps) {
             if (this->first_gps_received == false) {
@@ -132,6 +132,7 @@ class ParticleFilter {
             //printf("gps_x, gps_y: %f, %f\n", gps_x, gps_y);
 
             for (int i = 0; i < this->particles.size(); i++) {
+                //printf("particle %d \n", i);
                 //printf("particle_x, particle_y: %f, %f\n", this->particles[i].x, this->particles[i].y);
                 double distance = sqrt(pow((this->particles[i].x - gps_x), double(2)) + pow((this->particles[i].y - gps_y), double(2)));
                 //printf("distance: %f\n", distance);
@@ -190,6 +191,8 @@ class ParticleFilter {
 
             for (int i = 0;i < num_particles; i++) {
                 int index = discrete(generator);
+                //printf("index: %d\n", index);
+                //printf("weight before selecting new particles: %f\n", this->particles[i].weight);
                 //index = 0;
                 //printf("index %i\n", index);
                 std::ofstream index_log_file;
@@ -202,6 +205,7 @@ class ParticleFilter {
             }
 
             for (int i = 0; i < int(std::size(new_particles)); i++) {
+                //printf("weight after selecting new particles: %f\n", this->particles[i].weight);
                 //printf("new particles: %f, %f, %f, %f\n", new_particles[i].x, new_particles[i].y, new_particles[i].theta, new_particles[i].weight);
             }
 
@@ -224,7 +228,7 @@ class ParticleFilter {
 
                 std::normal_distribution<> normal_distribution_theta{new_particles[i].theta, this->odom_noise[2]};
                 double theta = pymod(normal_distribution_theta(generator), (2 * M_PI));
-                //double theta = 6.0;
+                //theta = 6.0;
                 //printf("theta: %f\n", theta);
                 this->particles.push_back(Particle(x, y, theta, new_particles[i].weight));
             }
@@ -277,7 +281,7 @@ class ParticleFilter {
     private:
         std::mt19937 generator;
         static const int num_particles = 750;
-        double gps_noise[1] = {0.45};
+        double gps_noise[1] = {0.80};
         double odom_noise[3] = {0.05, 0.05, 0.01};
         std::vector<Particle> particles;
         double latitudeLength;

autonav_ws/src/autonav_filters/include/autonav_filters/testing_gps.cpp

@@ -12,7 +12,7 @@
 int num_generator() {
     std::random_device rd;
     std::mt19937 generator(rd());
-    std::vector<int> range = {1, 5, 10, 15, 20, 25};
+    std::vector<int> range = {1, 1, 1, 1, 1, 100};
     std::discrete_distribution<int> discrete(range.begin(), range.end());
     int index = discrete(generator);
     int value = range[index];
@@ -42,28 +42,60 @@ int main() {
 
     std::random_device rd;
     std::mt19937 generator(rd());
-    std::vector<int> range = {1, 5, 10, 15, 20, 25};
+    std::vector<int> range = {10, 30, 20, 25, 15};
     std::discrete_distribution<int> discrete(range.begin(), range.end());
     std::vector<double> p = discrete.probabilities();
     double sum = 0.0;
     for (auto n : p) {
         sum = sum + n;
         std::cout << n << ' ';
     }
+    std::cout << "sum: " << sum << std::endl;
 
+    std::vector<int> indices;
     std::cout << '\n';
     std::cout << sum;
-    for (int i = 0; i < 1; i++) {
+    for (int i = 0; i < 1000; i++) {
         int index = discrete(generator);
         int value = range[index];
-        printf("index: %d\n", index);
-        printf("value: %d\n", value);
+        indices.push_back(index);
+        //printf("index: %d\n", index);
+        //printf("value: %d\n", value);
     }
-    std::vector<int> indexes;
-    for (int i = 0; i < 10; i++) {
-        indexes.push_back(num_generator());
+
+    int one_count;
+    int twos_count;
+    int threes_count;
+    int fours_count;
+    int fives_count;
+    for (int idx : indices) {
+        if (idx == 0) {
+            one_count++;
+        }
+        else if (idx == 1) {
+            twos_count++;
+        }
+        else if (idx == 2) {
+            threes_count++;
+        }  
+        else if (idx == 3) {
+            fours_count++;
+        }
+        else if (idx == 4) {
+            fives_count++;
+        }
     }
 
+    std::cout << "number of ones: " << one_count << std::endl;
+    std::cout << "number of twos: " << twos_count << std::endl;
+    std::cout << "number of threes: " << threes_count << std::endl;
+    std::cout << "number of fours: " << fours_count << std::endl;    
+    std::cout << "number of fives: " << fives_count << std::endl;    
+    //std::vector<int> indexes;
+    //for (int i = 0; i < 10; i++) {
+    //    indexes.push_back(num_generator());
+    // }
+
     // accumulate
     std::vector<double> weights;
     weights.push_back(0.993333);
@@ -85,4 +117,13 @@ int main() {
         //theta = 2.0;
         printf("theta: %f\n", theta);
     }
+
+    std::vector<int> test_vec;
+
+    test_vec.push_back(1);
+    test_vec.push_back(2);
+
+    std::cout << test_vec[0] << std::endl;
+    std::cout << test_vec[1] << std::endl;
+
 }

autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp

@@ -10,6 +10,7 @@
 #include "autonav_filters/rapidcsv.h"
 
 TEST(ParticleFilterTests, initialization_test) {
+    //GTEST_SKIP() << "skipping init test";
     double latitudeLength = 111086.2;
     double longitudeLength = 81978.2;
     ParticleFilter particle_filter = ParticleFilter(latitudeLength, longitudeLength);
@@ -33,6 +34,7 @@ TEST(ParticleFilterTests, initialization_test) {
 }
 
 TEST(ParticleFilterTests, feedback_test) {
+    //GTEST_SKIP() << "skipping feedback test";
     rapidcsv::Document motor_feedback_sheet("/home/tony/autonav_software_2024/autonav_ws/src/autonav_filters/tests/ENTRY_FEEDBACK.csv");
 
     std::vector<double> delta_xs = motor_feedback_sheet.GetColumn<double>(2);
@@ -129,7 +131,7 @@ TEST(ParticleFilterTests, complete_test) {
     // cut down the deltas until they are the same length as the gps values
 
     int n = latitudes.size();
-    //n = 5;
+    //n = 1;
 
     std::vector sliced_delta_xs(delta_xs.begin(), delta_xs.begin() + n);
     std::vector sliced_delta_ys(delta_ys.begin(), delta_ys.begin() + n);