1516X Push Back 1.0
1516X's robot code for the 2025-2026 VEX Robotics Competition
Loading...
Searching...
No Matches
MCL Namespace Reference

Classes

struct  OptSegment
struct  AABB
struct  ValidMeas
struct  RayHit

Functions

std::mt19937 Random (std::random_device{}())
std::vector< Particle > particles (NUM_PARTICLES)
std::vector< Particle > resample_buffer (NUM_PARTICLES)
double degToRad (double deg)
double radToDeg (double rad)
double wrapAngleDeg (double angle)
double gaussian_sample (double mean, double stddev)
RayHit getRaycastDistance (double sensX, double sensY, double dx, double dy)
void StartMCL (double x, double y)
void MonteCarlo ()

Variables

double PARAMS_TRANS_BASE = 0.2
double PARAMS_TRANS_GAIN = 0.02
double PARAMS_SENSOR_SIGMA = 2
double PARAMS_REL_SIGMA = 5.0
double global_X = 0
double global_Y = 0
double global_Theta = 0
double global_Confidence = 0
double map_min_x = -72.0
double map_max_x = 72.0
double map_min_y = -72.0
double map_max_y = 72.0
pros::Mutex particle_mutex
static const Segment MAP_SEGMENTS []
static const int MAP_SEGMENT_COUNT = sizeof(MAP_SEGMENTS) / sizeof(Segment)
OptSegment opt_segments [MAP_SEGMENT_COUNT]
AABB map_aabbs [MAP_SEGMENT_COUNT]
std::vector< MCLDistanceSensor > Sensors

Function Documentation

◆ degToRad()

double MCL::degToRad ( double deg)
inline

Definition at line 68 of file MCL.cpp.

68{ return deg * M_PI / 180.0; }

References degToRad().

Referenced by degToRad(), and MonteCarlo().

◆ gaussian_sample()

double MCL::gaussian_sample ( double mean,
double stddev )

Definition at line 76 of file MCL.cpp.

76 {
77 std::normal_distribution<double> dist(mean, stddev);
78 return dist(Random);
79 }
MCL::Random
std::mt19937 Random(std::random_device{}())

References gaussian_sample(), and Random().

Referenced by gaussian_sample(), MonteCarlo(), and StartMCL().

◆ getRaycastDistance()

RayHit MCL::getRaycastDistance ( double sensX,
double sensY,
double dx,
double dy )

Definition at line 81 of file MCL.cpp.

81 {
82 double minDist = 10000.0;
83 double bestWeight = 1.0;
84
85 double ray_end_x = sensX + dx * SENSOR_MAX_RANGE_IN;
86 double ray_end_y = sensY + dy * SENSOR_MAX_RANGE_IN;
87 double r_min_x = std::min(sensX, ray_end_x);
88 double r_max_x = std::max(sensX, ray_end_x);
89 double r_min_y = std::min(sensY, ray_end_y);
90 double r_max_y = std::max(sensY, ray_end_y);
91
92 for (int i = 0; i < MAP_SEGMENT_COUNT; ++i) {
93 const AABB& box = map_aabbs[i];
94 if (r_max_x < box.min_x || r_min_x > box.max_x ||
95 r_max_y < box.min_y || r_min_y > box.max_y) continue;
96
97 const OptSegment& seg = opt_segments[i];
98 double r_px = seg.x0 - sensX;
99 double r_py = seg.y0 - sensY;
100 double r_cross_s = dx * seg.dy - dy * seg.dx;
101
102 if (std::abs(r_cross_s) < 1e-4) continue;
103
104 double t = (r_px * seg.dy - r_py * seg.dx) / r_cross_s;
105 double u = (r_px * dy - r_py * dx) / r_cross_s;
106
107 if (u >= 0.0 && u <= 1.0 && t > 0.0) {
108 if (t < minDist) {
109 minDist = t;
110 bestWeight = 0.5 + 0.5 * std::abs(dx * seg.nx + dy * seg.ny);
111 }
112 }
113 }
114
115 if (minDist <= 0 || minDist > SENSOR_MAX_RANGE_IN) return {-1.0, 1.0};
116 return {minDist, bestWeight};
117 }
MCL::MAP_SEGMENT_COUNT
static const int MAP_SEGMENT_COUNT
Definition MCL.cpp:47
MCL::opt_segments
OptSegment opt_segments[MAP_SEGMENT_COUNT]
Definition MCL.cpp:50
MCL::map_aabbs
AABB map_aabbs[MAP_SEGMENT_COUNT]
Definition MCL.cpp:53
MCL::AABB::max_y
double max_y
Definition MCL.cpp:52
MCL::AABB::max_x
double max_x
Definition MCL.cpp:52
MCL::OptSegment::dy
double dy
Definition MCL.cpp:49
MCL::OptSegment::y0
double y0
Definition MCL.cpp:49
MCL::OptSegment::dx
double dx
Definition MCL.cpp:49
MCL::OptSegment::x0
double x0
Definition MCL.cpp:49
MCL::OptSegment::ny
double ny
Definition MCL.cpp:49
MCL::OptSegment::nx
double nx
Definition MCL.cpp:49

References MCL::OptSegment::dx, MCL::OptSegment::dy, getRaycastDistance(), map_aabbs, MAP_SEGMENT_COUNT, MCL::AABB::max_x, MCL::AABB::max_y, MCL::OptSegment::nx, MCL::OptSegment::ny, opt_segments, MCL::OptSegment::x0, and MCL::OptSegment::y0.

Referenced by getRaycastDistance(), and MonteCarlo().

◆ MonteCarlo()

void MCL::MonteCarlo ( )

Definition at line 161 of file MCL.cpp.

161 {
162 lemlib::Pose prevOdom = chassis.getPose();
163
164 while (true) {
165 lemlib::Pose currOdom = chassis.getPose();
166 double dX_global = currOdom.x - prevOdom.x;
167 double dY_global = currOdom.y - prevOdom.y;
168 double dTheta = wrapAngleDeg(currOdom.theta - prevOdom.theta);
169
170 // Pre-calculate the current absolute robot heading for translations
171 double robot_rad = degToRad(currOdom.theta);
172 double robot_cos = std::cos(robot_rad);
173 double robot_sin = std::sin(robot_rad);
174
175 double cosP = std::cos(degToRad(prevOdom.theta));
176 double sinP = std::sin(degToRad(prevOdom.theta));
177 double dF = dX_global * sinP + dY_global * cosP;
178 double dS = dX_global * cosP - dY_global * sinP;
179
180 particle_mutex.take();
181
182 for (auto& p : particles) {
183 double dist = std::hypot(dF, dS);
184 // Keep dTheta here to account for translation noise induced during heavy turning
185 double transStd = PARAMS_TRANS_BASE + 0.08 * dist + 0.03 * std::abs(dTheta);
186
187 double dx_l = dF + gaussian_sample(0, transStd);
188 double dy_l = dS + gaussian_sample(0, transStd);
189
190 p.x += dx_l * robot_sin + dy_l * robot_cos;
191 p.y += dx_l * robot_cos - dy_l * robot_sin;
192 }
193 prevOdom = currOdom;
194
195 std::vector<MCLDistanceSensor*> active;
196 for (auto& s : Sensors) { s.Measure(); if (s.measurement > 0) active.push_back(&s); }
197
198 if (!active.empty()) {
199 double max_log = -1e9;
200 std::vector<double> logs(NUM_PARTICLES);
201
202 // PERFORMANCE BOOST: Pre-compute sensor offsets and ray vectors globally
203 struct SensorPrecomp {
204 MCLDistanceSensor* s;
205 double dx, dy, offsetX, offsetY;
206 };
207
208 std::vector<SensorPrecomp> precomps;
209 for (auto* s : active) {
210 SensorPrecomp sc;
211 sc.s = s;
212 sc.dx = robot_sin * s->cos_off + robot_cos * s->sin_off;
213 sc.dy = robot_cos * s->cos_off - robot_sin * s->sin_off;
214 sc.offsetX = s->LocalY * robot_sin + s->LocalX * robot_cos;
215 sc.offsetY = s->LocalY * robot_cos - s->LocalX * robot_sin;
216 precomps.push_back(sc);
217 }
218
219 for (int i = 0; i < NUM_PARTICLES; ++i) {
220 double logL = 0.0;
221
222 ValidMeas valid_measurements[4];
223 int valid_count = 0;
224
225 for (const auto& sc : precomps) {
226 double sensX = particles[i].x + sc.offsetX;
227 double sensY = particles[i].y + sc.offsetY;
228
229 RayHit hit = getRaycastDistance(sensX, sensY, sc.dx, sc.dy);
230
231 if (hit.dist < 0) continue;
232 double err = sc.s->measurement - hit.dist;
233
234 if (std::abs(err) > 18.0) continue;
235
236 valid_measurements[valid_count++] = {hit.dist, sc.s->measurement, hit.weight};
237
238 double gaussian_prob = std::exp(-(err * err) / (2 * PARAMS_SENSOR_SIGMA * PARAMS_SENSOR_SIGMA));
239
240 double p_abs = gaussian_prob * hit.weight;
241 logL += std::log(std::max(p_abs, 1e-6));
242 }
243
244 if (valid_count > 1) {
245 for (int a = 0; a < valid_count; ++a) {
246 for (int b = a + 1; b < valid_count; ++b) {
247 double predDiff = valid_measurements[a].pred - valid_measurements[b].pred;
248 double measDiff = valid_measurements[a].meas - valid_measurements[b].meas;
249 double diffErr = measDiff - predDiff;
250
251 double w = std::min(valid_measurements[a].weight, valid_measurements[b].weight);
252 double rel_prob = std::exp(-(diffErr * diffErr) / (2 * PARAMS_REL_SIGMA * PARAMS_REL_SIGMA));
253
254 logL += std::log(std::max(rel_prob * w, 1e-6));
255 }
256 }
257 }
258
259 logs[i] = logL;
260 if (logL > max_log) max_log = logL;
261 }
262
263 double sumW = 0;
264 for (int i = 0; i < NUM_PARTICLES; ++i) {
265 particles[i].weight = std::exp(logs[i] - max_log);
266
267 if (particles[i].x < map_min_x || particles[i].x > map_max_x ||
268 particles[i].y < map_min_y || particles[i].y > map_max_y) {
269 particles[i].weight *= 0.01;
270 }
271 sumW += particles[i].weight;
272 }
273 for (auto& p : particles) p.weight /= (sumW > 1e-9 ? sumW : 1.0);
274 }
275
276 double mX = 0, mY = 0, neff_s = 0;
277 for (const auto& p : particles) {
278 mX += p.x * p.weight;
279 mY += p.y * p.weight;
280 neff_s += p.weight * p.weight;
281 }
282
283 // IMU serves as the source of truth for heading
284 global_X = mX;
285 global_Y = mY;
286 global_Theta = currOdom.theta;
287
288 double neff = (neff_s > 0) ? (1.0 / neff_s) : 0;
289 global_Confidence = std::clamp(neff / NUM_PARTICLES, 0.0, 1.0);
290
291 if (global_Confidence < RESAMPLE_THRESHOLD && (!active.empty() || global_Confidence < 0.15)) {
292 double injectP = 0.02 + (1.0 - global_Confidence) * 0.15;
293 if (global_Confidence < 0.2) injectP = 0.3;
294
295 int keep = NUM_PARTICLES * (1.0 - injectP);
296 double r = std::uniform_real_distribution<double>(0, 1.0 / keep)(Random), c = particles[0].weight;
297 int idx = 0;
298
299 for (int i = 0; i < keep; ++i) {
300 double U = r + (double)i / keep;
301 while (U > c && idx < NUM_PARTICLES - 1) c += particles[++idx].weight;
302
303 resample_buffer[i] = particles[idx];
304 resample_buffer[i].weight = 1.0 / NUM_PARTICLES;
305
306 resample_buffer[i].x += gaussian_sample(0, 0.2);
307 resample_buffer[i].y += gaussian_sample(0, 0.2);
308 }
309
310 std::normal_distribution<double> fX(currOdom.x, 10.0);
311 std::normal_distribution<double> fY(currOdom.y, 10.0);
312
313 for (int i = keep; i < NUM_PARTICLES; ++i) {
314 double newX = std::clamp(fX(Random), map_min_x, map_max_x);
315 double newY = std::clamp(fY(Random), map_min_y, map_max_y);
316 resample_buffer[i] = {newX, newY, 1.0/NUM_PARTICLES};
317 }
318 particles = resample_buffer;
319 }
320
321 static uint32_t last_print = pros::millis();
322 if (pros::millis() - last_print > 500) {
323 last_print = pros::millis();
324
325 printf("Odom = (%.2f, %.2f, %.2f)\n", currOdom.x, currOdom.y, currOdom.theta);
326 printf("MCL = (%.2f, %.2f)\n", global_X, global_Y);
327
328 printf("P = [");
329 for (int i = 0; i < NUM_PARTICLES; ++i) {
330 printf("(%.2f, %.2f)", particles[i].x, particles[i].y);
331 if (i != NUM_PARTICLES - 1) printf(", ");
332 }
333 printf("]\n\n");
334 }
335 particle_mutex.give();
336
337 pros::delay(5);
338 }
339 }
chassis
lemlib::Chassis chassis
MCL::PARAMS_TRANS_BASE
double PARAMS_TRANS_BASE
Definition MCL.cpp:9
MCL::getRaycastDistance
RayHit getRaycastDistance(double sensX, double sensY, double dx, double dy)
Definition MCL.cpp:81
MCL::global_Theta
double global_Theta
Definition MCL.cpp:14
MCL::degToRad
double degToRad(double deg)
Definition MCL.cpp:68
MCL::map_min_y
double map_min_y
Definition MCL.cpp:15
MCL::global_Y
double global_Y
Definition MCL.cpp:14
MCL::global_Confidence
double global_Confidence
Definition MCL.cpp:14
MCL::wrapAngleDeg
double wrapAngleDeg(double angle)
Definition MCL.cpp:70
MCL::map_max_y
double map_max_y
Definition MCL.cpp:15
MCL::particles
std::vector< Particle > particles(NUM_PARTICLES)
MCL::PARAMS_SENSOR_SIGMA
double PARAMS_SENSOR_SIGMA
Definition MCL.cpp:11
MCL::particle_mutex
pros::Mutex particle_mutex
Definition MCL.cpp:17
MCL::resample_buffer
std::vector< Particle > resample_buffer(NUM_PARTICLES)
MCL::global_X
double global_X
Definition MCL.cpp:14
MCL::PARAMS_REL_SIGMA
double PARAMS_REL_SIGMA
Definition MCL.cpp:12
MCL::map_min_x
double map_min_x
Definition MCL.cpp:15
MCL::gaussian_sample
double gaussian_sample(double mean, double stddev)
Definition MCL.cpp:76
MCL::map_max_x
double map_max_x
Definition MCL.cpp:15
MCL::Sensors
std::vector< MCLDistanceSensor > Sensors
Definition MCL.cpp:55

References chassis, degToRad(), MCL::RayHit::dist, gaussian_sample(), getRaycastDistance(), global_Confidence, global_Theta, global_X, global_Y, map_max_x, map_max_y, map_min_x, map_min_y, MCL::ValidMeas::meas, MonteCarlo(), PARAMS_REL_SIGMA, PARAMS_SENSOR_SIGMA, PARAMS_TRANS_BASE, particle_mutex, particles(), MCL::ValidMeas::pred, Random(), resample_buffer(), Sensors, MCL::RayHit::weight, and wrapAngleDeg().

Referenced by MonteCarlo().

◆ particles()

std::vector< Particle > MCL::particles ( NUM_PARTICLES )

References particles().

Referenced by MonteCarlo(), particles(), and StartMCL().

◆ radToDeg()

double MCL::radToDeg ( double rad)
inline

Definition at line 69 of file MCL.cpp.

69{ return rad * 180.0 / M_PI; }

References radToDeg().

Referenced by radToDeg().

◆ Random()

std::mt19937 MCL::Random ( std::random_device{} ())

Referenced by gaussian_sample(), and MonteCarlo().

◆ resample_buffer()

std::vector< Particle > MCL::resample_buffer ( NUM_PARTICLES )

References resample_buffer().

Referenced by MonteCarlo(), and resample_buffer().

◆ StartMCL()

void MCL::StartMCL ( double x,
double y )

Definition at line 119 of file MCL.cpp.

119 {
120 particle_mutex.take();
121 map_min_x = map_min_y = std::numeric_limits<double>::infinity();
122 map_max_x = map_max_y = -std::numeric_limits<double>::infinity();
123
124 for (int i = 0; i < MAP_SEGMENT_COUNT; ++i) {
125 map_min_x = std::min({map_min_x, MAP_SEGMENTS[i].x0, MAP_SEGMENTS[i].x1});
126 map_max_x = std::max({map_max_x, MAP_SEGMENTS[i].x0, MAP_SEGMENTS[i].x1});
127 map_min_y = std::min({map_min_y, MAP_SEGMENTS[i].y0, MAP_SEGMENTS[i].y1});
128 map_max_y = std::max({map_max_y, MAP_SEGMENTS[i].y0, MAP_SEGMENTS[i].y1});
129
130 map_aabbs[i] = {
131 std::min(MAP_SEGMENTS[i].x0, MAP_SEGMENTS[i].x1),
132 std::max(MAP_SEGMENTS[i].x0, MAP_SEGMENTS[i].x1),
133 std::min(MAP_SEGMENTS[i].y0, MAP_SEGMENTS[i].y1),
134 std::max(MAP_SEGMENTS[i].y0, MAP_SEGMENTS[i].y1)
135 };
136
137 opt_segments[i].x0 = MAP_SEGMENTS[i].x0;
138 opt_segments[i].y0 = MAP_SEGMENTS[i].y0;
139 opt_segments[i].x1 = MAP_SEGMENTS[i].x1;
140 opt_segments[i].y1 = MAP_SEGMENTS[i].y1;
141 opt_segments[i].dx = MAP_SEGMENTS[i].x1 - MAP_SEGMENTS[i].x0;
142 opt_segments[i].dy = MAP_SEGMENTS[i].y1 - MAP_SEGMENTS[i].y0;
143 double len = std::hypot(opt_segments[i].dx, opt_segments[i].dy);
144 if (len > 1e-6) {
145 opt_segments[i].nx = -opt_segments[i].dy / len;
146 opt_segments[i].ny = opt_segments[i].dx / len;
147 } else {
148 opt_segments[i].nx = 0;
149 opt_segments[i].ny = 0;
150 }
151 }
152
153 for (auto& p : particles) {
154 p.x = gaussian_sample(x, 2.0);
155 p.y = gaussian_sample(y, 2.0);
156 p.weight = 1.0 / NUM_PARTICLES;
157 }
158 particle_mutex.give();
159 }
MCL::MAP_SEGMENTS
static const Segment MAP_SEGMENTS[]
Definition MCL.cpp:36

References gaussian_sample(), map_aabbs, map_max_x, map_max_y, map_min_x, map_min_y, MAP_SEGMENT_COUNT, MAP_SEGMENTS, opt_segments, particle_mutex, particles(), and StartMCL().

Referenced by StartMCL().

◆ wrapAngleDeg()

double MCL::wrapAngleDeg ( double angle)
inline

Definition at line 70 of file MCL.cpp.

70 {
71 angle = std::fmod(angle + 180.0, 360.0);
72 if (angle < 0) angle += 360.0;
73 return angle - 180.0;
74 }

References wrapAngleDeg().

Referenced by MonteCarlo(), and wrapAngleDeg().

Variable Documentation

◆ global_Confidence

double MCL::global_Confidence = 0

Definition at line 14 of file MCL.cpp.

Referenced by MonteCarlo().

◆ global_Theta

double MCL::global_Theta = 0

Definition at line 14 of file MCL.cpp.

Referenced by MonteCarlo().

◆ global_X

double MCL::global_X = 0

Definition at line 14 of file MCL.cpp.

Referenced by MonteCarlo().

◆ global_Y

double MCL::global_Y = 0

Definition at line 14 of file MCL.cpp.

Referenced by MonteCarlo().

◆ map_aabbs

AABB MCL::map_aabbs[MAP_SEGMENT_COUNT]

Definition at line 53 of file MCL.cpp.

Referenced by getRaycastDistance(), and StartMCL().

◆ map_max_x

double MCL::map_max_x = 72.0

Definition at line 15 of file MCL.cpp.

Referenced by MonteCarlo(), and StartMCL().

◆ map_max_y

double MCL::map_max_y = 72.0

Definition at line 15 of file MCL.cpp.

Referenced by MonteCarlo(), and StartMCL().

◆ map_min_x

double MCL::map_min_x = -72.0

Definition at line 15 of file MCL.cpp.

Referenced by MonteCarlo(), and StartMCL().

◆ map_min_y

double MCL::map_min_y = -72.0

Definition at line 15 of file MCL.cpp.

Referenced by MonteCarlo(), and StartMCL().

◆ MAP_SEGMENT_COUNT

const int MCL::MAP_SEGMENT_COUNT = sizeof(MAP_SEGMENTS) / sizeof(Segment)
static

Definition at line 47 of file MCL.cpp.

Referenced by getRaycastDistance(), and StartMCL().

◆ MAP_SEGMENTS

const Segment MCL::MAP_SEGMENTS[]
static
Initial value:
= {
{72, 72, 72, -72}, {72, -72, -72, -72}, {-72, -72, -72, 72}, {-72, 72, 72, 72},
{20.9, 46.25, 24.4, 46.25}, {24.4, 46.25, 24.4, 49.75}, {24.4, 49.75, 20.9, 49.75}, {20.9, 49.75, 20.9, 46.25},
{-24.4, 46.25, -20.9, 46.25}, {-20.9, 46.25, -20.9, 49.75}, {-20.9, 49.75, -24.4, 49.75}, {-24.4, 49.75, -24.4, 46.25},
{20.9, -49.75, 24.4, -49.75}, {24.4, -49.75, 24.4, -46.25}, {24.4, -46.25, 20.9, -46.25}, {20.9, -46.25, 20.9, -49.75},
{-24.4, -49.75, -20.9, -49.75}, {-20.9, -49.75, -20.9, -46.25}, {-20.9, -46.25, -24.4, -46.25}, {-24.4, -46.25, -24.4, -49.75},
{-72, 45.5, -67, 45.5}, {-67, 45.5, -67, 50.5}, {-67, 50.5, -72, 50.5}, {-72, 50.5, -72, 45.5},
{67, 45.5, 72, 45.5}, {72, 45.5, 72, 50.5}, {72, 50.5, 67, 50.5}, {67, 50.5, 67, 45.5},
{-72, -50.5, -67, -50.5}, {-67, -50.5, -67, -45.5}, {-67, -45.5, -72, -45.5}, {-72, -45.5, -72, -50.5},
{67, -50.5, 72, -50.5}, {72, -50.5, 72, -45.5}, {72, -45.5, 67, -45.5}, {67, -45.5, 67, -50.5},
}

Definition at line 36 of file MCL.cpp.

36 {
37 {72, 72, 72, -72}, {72, -72, -72, -72}, {-72, -72, -72, 72}, {-72, 72, 72, 72},
38 {20.9, 46.25, 24.4, 46.25}, {24.4, 46.25, 24.4, 49.75}, {24.4, 49.75, 20.9, 49.75}, {20.9, 49.75, 20.9, 46.25},
39 {-24.4, 46.25, -20.9, 46.25}, {-20.9, 46.25, -20.9, 49.75}, {-20.9, 49.75, -24.4, 49.75}, {-24.4, 49.75, -24.4, 46.25},
40 {20.9, -49.75, 24.4, -49.75}, {24.4, -49.75, 24.4, -46.25}, {24.4, -46.25, 20.9, -46.25}, {20.9, -46.25, 20.9, -49.75},
41 {-24.4, -49.75, -20.9, -49.75}, {-20.9, -49.75, -20.9, -46.25}, {-20.9, -46.25, -24.4, -46.25}, {-24.4, -46.25, -24.4, -49.75},
42 {-72, 45.5, -67, 45.5}, {-67, 45.5, -67, 50.5}, {-67, 50.5, -72, 50.5}, {-72, 50.5, -72, 45.5},
43 {67, 45.5, 72, 45.5}, {72, 45.5, 72, 50.5}, {72, 50.5, 67, 50.5}, {67, 50.5, 67, 45.5},
44 {-72, -50.5, -67, -50.5}, {-67, -50.5, -67, -45.5}, {-67, -45.5, -72, -45.5}, {-72, -45.5, -72, -50.5},
45 {67, -50.5, 72, -50.5}, {72, -50.5, 72, -45.5}, {72, -45.5, 67, -45.5}, {67, -45.5, 67, -50.5},
46 };

Referenced by StartMCL().

◆ opt_segments

OptSegment MCL::opt_segments[MAP_SEGMENT_COUNT]

Definition at line 50 of file MCL.cpp.

Referenced by getRaycastDistance(), and StartMCL().

◆ PARAMS_REL_SIGMA

double MCL::PARAMS_REL_SIGMA = 5.0

Definition at line 12 of file MCL.cpp.

Referenced by MonteCarlo().

◆ PARAMS_SENSOR_SIGMA

double MCL::PARAMS_SENSOR_SIGMA = 2

Definition at line 11 of file MCL.cpp.

Referenced by MonteCarlo().

◆ PARAMS_TRANS_BASE

double MCL::PARAMS_TRANS_BASE = 0.2

Definition at line 9 of file MCL.cpp.

Referenced by MonteCarlo().

◆ PARAMS_TRANS_GAIN

double MCL::PARAMS_TRANS_GAIN = 0.02

Definition at line 10 of file MCL.cpp.

◆ particle_mutex

pros::Mutex MCL::particle_mutex

Definition at line 17 of file MCL.cpp.

Referenced by MonteCarlo(), and StartMCL().

◆ Sensors

std::vector<MCLDistanceSensor> MCL::Sensors
Initial value:
= {
MCLDistanceSensor(frontDistance, -3.0, -0.75, 0),
MCLDistanceSensor(leftDistance, 0.5, -7.2, 90),
MCLDistanceSensor(rightDistance, -0.5, 6.3, -90),
MCLDistanceSensor(backDistance, 3.0, -10.5, 180),
}
frontDistance
pros::Distance frontDistance(14)
rightDistance
pros::Distance rightDistance(20)
backDistance
pros::Distance backDistance(16)
leftDistance
pros::Distance leftDistance(15)

Definition at line 55 of file MCL.cpp.

55 {
56 MCLDistanceSensor(frontDistance, -3.0, -0.75, 0),
57 MCLDistanceSensor(leftDistance, 0.5, -7.2, 90),
58 MCLDistanceSensor(rightDistance, -0.5, 6.3, -90),
59 MCLDistanceSensor(backDistance, 3.0, -10.5, 180),
60 };

Referenced by MonteCarlo().