diff --git a/autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp b/autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp index 0fef01eb..88fab08d 100644 --- a/autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp +++ b/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 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 particles; double latitudeLength; diff --git a/autonav_ws/src/autonav_filters/include/autonav_filters/testing_gps b/autonav_ws/src/autonav_filters/include/autonav_filters/testing_gps index 864c55d2..8955070a 100755 Binary files a/autonav_ws/src/autonav_filters/include/autonav_filters/testing_gps and b/autonav_ws/src/autonav_filters/include/autonav_filters/testing_gps differ diff --git a/autonav_ws/src/autonav_filters/include/autonav_filters/testing_gps.cpp b/autonav_ws/src/autonav_filters/include/autonav_filters/testing_gps.cpp index ec6d9f81..16c94004 100644 --- a/autonav_ws/src/autonav_filters/include/autonav_filters/testing_gps.cpp +++ b/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 range = {1, 5, 10, 15, 20, 25}; + std::vector range = {1, 1, 1, 1, 1, 100}; std::discrete_distribution discrete(range.begin(), range.end()); int index = discrete(generator); int value = range[index]; @@ -42,7 +42,7 @@ int main() { std::random_device rd; std::mt19937 generator(rd()); - std::vector range = {1, 5, 10, 15, 20, 25}; + std::vector range = {10, 30, 20, 25, 15}; std::discrete_distribution discrete(range.begin(), range.end()); std::vector p = discrete.probabilities(); double sum = 0.0; @@ -50,20 +50,52 @@ int main() { sum = sum + n; std::cout << n << ' '; } + std::cout << "sum: " << sum << std::endl; + std::vector 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 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 indexes; + //for (int i = 0; i < 10; i++) { + // indexes.push_back(num_generator()); + // } + // accumulate std::vector weights; weights.push_back(0.993333); @@ -85,4 +117,13 @@ int main() { //theta = 2.0; printf("theta: %f\n", theta); } + + std::vector 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; + } \ No newline at end of file diff --git a/autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp b/autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp index a0dd4db7..5a75cbe3 100644 --- a/autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp +++ b/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 delta_xs = motor_feedback_sheet.GetColumn(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);