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  SensorConfig
struct  XorShift32

Functions

float degToRad (float d)
float wrapAngle (float a)
void StartMCL (double x, double y)
void MotionUpdate (double dX_global, double dY_global, double dTheta, double robot_theta_deg)
void SensorUpdate (const std::vector< float > &measurements, float robot_theta_deg, float current_confidence)
float computeESS ()
void Resample ()
void MonteCarlo ()

Variables

double PARAMS_TRANS_BASE = 0.3
double PARAMS_TRANS_GAIN = 0.025
double global_X = 0
double global_Y = 0
double global_Theta = 0
double global_Confidence = 0
float w_slow = 0.0f
float w_fast = 0.0f
constexpr float ALPHA_SLOW = 0.001f
constexpr float ALPHA_FAST = 0.1f
constexpr float FIELD_SIZE = 140.42f
constexpr float HALF_SIZE = FIELD_SIZE * 0.5f
constexpr float FIELD_MIN = -HALF_SIZE
constexpr float FIELD_MAX = HALF_SIZE
constexpr float MAX_SENSOR_READING = 55.0f
float particle_x [NUM_PARTICLES]
float particle_y [NUM_PARTICLES]
float particle_weights [NUM_PARTICLES]
pros::Mutex particle_mutex
const std::vector< SensorConfig > SENSOR_CONFIGS
struct MCL::XorShift32 rng

Function Documentation

◆ computeESS()

float MCL::computeESS ( )

Definition at line 203 of file MCL.cpp.

203 {
204 particle_mutex.take();
205 float sq = 0.0f;
206 for (int i = 0; i < NUM_PARTICLES; ++i)
207 sq += particle_weights[i] * particle_weights[i];
208 particle_mutex.give();
209 return (sq > 1e-20f) ? 1.0f / sq : 0.0f;
210 }
MCL::particle_weights
float particle_weights[NUM_PARTICLES]
Definition MCL.cpp:35
MCL::particle_mutex
pros::Mutex particle_mutex
Definition MCL.cpp:37

References particle_mutex, and particle_weights.

Referenced by MonteCarlo().

◆ degToRad()

float MCL::degToRad ( float d)
inline

Definition at line 74 of file MCL.cpp.

74{ return d * (float)M_PI / 180.0f; }

Referenced by MotionUpdate(), and SensorUpdate().

◆ MonteCarlo()

void MCL::MonteCarlo ( )

Definition at line 255 of file MCL.cpp.

255 {
256 lemlib::Pose prevOdom = chassis.getPose();
257 uint32_t now = pros::millis();
258
259 int print_counter = 0;
260 bool first_run = true; // Added for the EMA filter
261
262 while (true) {
263 lemlib::Pose currOdom = chassis.getPose();
264
265 const double dX_global = currOdom.x - prevOdom.x;
266 const double dY_global = currOdom.y - prevOdom.y;
267 const double dTheta = wrapAngle((float)(currOdom.theta - prevOdom.theta));
268
269 if (std::abs(dX_global) > 0.001 || std::abs(dY_global) > 0.001 || std::abs(dTheta) > 0.1) {
270
271 MotionUpdate(dX_global, dY_global, dTheta, currOdom.theta);
272
273 std::vector<float> measurements(4, -1.0f);
274 bool has_valid_reading = false;
275
276 auto try_read_sensor = [&](auto& sensor, int index) {
277 float val = sensor.get() / 25.4f;
278 if (val > 2.0f && val < MAX_SENSOR_READING) {
279 measurements[index] = val;
280 has_valid_reading = true;
281 }
282 };
283
284 try_read_sensor(frontDistance, 0);
285 try_read_sensor(rightDistance, 1);
286 try_read_sensor(backDistance, 2);
287 try_read_sensor(leftDistance, 3);
288
289 if (has_valid_reading) {
290 SensorUpdate(measurements, currOdom.theta, global_Confidence);
291 }
292
293 particle_mutex.take();
294
295 float max_w = -1.0f;
296 int best_idx = 0;
297 for (int i = 0; i < NUM_PARTICLES; ++i) {
298 if (particle_weights[i] > max_w) {
299 max_w = particle_weights[i];
300 best_idx = i;
301 }
302 }
303
304 float best_x = particle_x[best_idx];
305 float best_y = particle_y[best_idx];
306
307 float sumX = 0.0f, sumY = 0.0f, sumW = 0.0f;
308 float sumX2 = 0.0f, sumY2 = 0.0f;
309 const float CLUSTER_RADIUS = 15.0f;
310
311 for (int i = 0; i < NUM_PARTICLES; ++i) {
312 float dx = particle_x[i] - best_x;
313 float dy = particle_y[i] - best_y;
314
315 if ((dx * dx + dy * dy) <= (CLUSTER_RADIUS * CLUSTER_RADIUS)) {
316 float w = particle_weights[i];
317 sumX += w * particle_x[i];
318 sumY += w * particle_y[i];
319 sumX2 += w * particle_x[i] * particle_x[i];
320 sumY2 += w * particle_y[i] * particle_y[i];
321 sumW += w;
322 }
323 }
324
325 float mcl_std_dev = 999.0f;
326 float cluster_weight_ratio = 0.0f;
327
328 // --- RAW CLUSTER CALCULATION ---
329 float raw_X = best_x;
330 float raw_Y = best_y;
331
332 if (sumW > 1e-6f) {
333 raw_X = sumX / sumW;
334 raw_Y = sumY / sumW;
335
336 float meanX = raw_X;
337 float meanY = raw_Y;
338 float varX = (sumX2 / sumW) - (meanX * meanX);
339 float varY = (sumY2 / sumW) - (meanY * meanY);
340 mcl_std_dev = std::sqrt(std::max(0.0f, varX + varY));
341
342 cluster_weight_ratio = sumW;
343 }
344
345 // --- EXPONENTIAL MOVING AVERAGE (EMA) FILTER ---
346 const float EMA_ALPHA = 0.20f;
347
348 if (first_run) {
349 global_X = raw_X;
350 global_Y = raw_Y;
351 first_run = false;
352 } else {
353 global_X = global_X + EMA_ALPHA * (raw_X - global_X);
354 global_Y = global_Y + EMA_ALPHA * (raw_Y - global_Y);
355 }
356 // -----------------------------------------------
357
358 global_Theta = currOdom.theta;
359 global_Confidence = (w_slow > 1e-10f) ? std::min(w_fast / w_slow, 1.0f) : 0.0f;
360
361 particle_mutex.give();
362
363 if (++print_counter >= 3) {
364 printf("MCL: %.2f %.2f | ODOM: %.2f %.2f | CONF: %.2f | STD: %.2f | RATIO: %.2f\n",
365 global_X, global_Y, currOdom.x, currOdom.y, global_Confidence, mcl_std_dev, cluster_weight_ratio);
366 print_counter = 0;
367 }
368
369 if (global_Confidence > 0.65 && cluster_weight_ratio > 0.50f &&
370 std::abs(dX_global) < 5 && std::abs(dY_global) < 5) {
371
372 double diff_x = global_X - currOdom.x;
373 double diff_y = global_Y - currOdom.y;
374 double correction_mag = std::hypot(diff_x, diff_y);
375
376 if (correction_mag > 1.0 && correction_mag < 12.0) {
377 lemlib::Pose fusedPose(global_X, global_Y, currOdom.theta);
378 chassis.setPose(fusedPose);
379 currOdom = chassis.getPose();
380 }
381 }
382
383 const float ess = computeESS();
384 const float ratio = (w_slow > 1e-10f) ? (w_fast / w_slow) : 1.0f;
385 if (ess < NUM_PARTICLES * 0.5f || ratio < 0.9f) {
386 Resample();
387 }
388 }
389
390 prevOdom = currOdom;
391 pros::Task::delay_until(&now, 10);
392 }
393 }
backDistance
pros::Distance backDistance(7)
chassis
Chassis chassis(drivebase, lateral_controller, angular_controller, sensors, &throttle_curve, &steer_curve)
leftDistance
pros::Distance leftDistance(5)
rightDistance
pros::Distance rightDistance(6)
frontDistance
pros::Distance frontDistance(8)
MCL::global_Theta
double global_Theta
Definition MCL.cpp:17
MCL::MotionUpdate
void MotionUpdate(double dX_global, double dY_global, double dTheta, double robot_theta_deg)
Definition MCL.cpp:97
MCL::global_Y
double global_Y
Definition MCL.cpp:17
MCL::global_Confidence
double global_Confidence
Definition MCL.cpp:17
MCL::particle_x
float particle_x[NUM_PARTICLES]
Definition MCL.cpp:33
MCL::particle_y
float particle_y[NUM_PARTICLES]
Definition MCL.cpp:34
MCL::wrapAngle
float wrapAngle(float a)
Definition MCL.cpp:75
MCL::MAX_SENSOR_READING
constexpr float MAX_SENSOR_READING
Definition MCL.cpp:30
MCL::w_slow
float w_slow
Definition MCL.cpp:19
MCL::global_X
double global_X
Definition MCL.cpp:17
MCL::w_fast
float w_fast
Definition MCL.cpp:19
MCL::SensorUpdate
void SensorUpdate(const std::vector< float > &measurements, float robot_theta_deg, float current_confidence)
Definition MCL.cpp:130
MCL::computeESS
float computeESS()
Definition MCL.cpp:203
MCL::Resample
void Resample()
Definition MCL.cpp:212

References backDistance(), chassis(), computeESS(), frontDistance(), global_Confidence, global_Theta, global_X, global_Y, leftDistance(), MAX_SENSOR_READING, MotionUpdate(), particle_mutex, particle_weights, particle_x, particle_y, Resample(), rightDistance(), SensorUpdate(), w_fast, w_slow, and wrapAngle().

◆ MotionUpdate()

void MCL::MotionUpdate ( double dX_global,
double dY_global,
double dTheta,
double robot_theta_deg )

Definition at line 97 of file MCL.cpp.

97 {
98 const float dist = (float)std::hypot(dX_global, dY_global);
99 const float turn_factor = std::abs((float)dTheta) * 0.05f;
100
101 const float c = 1.0f - std::clamp((float)global_Confidence, 0.0f, 1.0f);
102 const float transStd = (float)(PARAMS_TRANS_BASE + c * 0.3)
103 + (float)(PARAMS_TRANS_GAIN + c * 0.04) * (dist + turn_factor);
104
105 float math_theta_deg = 90.0f - (float)robot_theta_deg;
106 const float theta_rad = degToRad(math_theta_deg);
107
108 const float cos_t = std::cos(theta_rad);
109 const float sin_t = std::sin(theta_rad);
110
111 particle_mutex.take();
112 for (int i = 0; i < NUM_PARTICLES; ++i) {
113 const float nF = rng.gaussian(transStd);
114 const float nS = rng.gaussian(transStd * 0.6f);
115
116 const float noise_X = nF * cos_t - nS * sin_t;
117 const float noise_Y = nF * sin_t + nS * cos_t;
118
119 particle_x[i] = std::clamp<float>(
120 particle_x[i] + (float)dX_global + noise_X,
121 FIELD_MIN + 0.1f, FIELD_MAX - 0.1f);
122 particle_y[i] = std::clamp<float>(
123 particle_y[i] + (float)dY_global + noise_Y,
124 FIELD_MIN + 0.1f, FIELD_MAX - 0.1f);
125 }
126 particle_mutex.give();
127 }
MCL::PARAMS_TRANS_BASE
double PARAMS_TRANS_BASE
Definition MCL.cpp:14
MCL::degToRad
float degToRad(float d)
Definition MCL.cpp:74
MCL::PARAMS_TRANS_GAIN
double PARAMS_TRANS_GAIN
Definition MCL.cpp:15
MCL::FIELD_MIN
constexpr float FIELD_MIN
Definition MCL.cpp:26
MCL::rng
struct MCL::XorShift32 rng
MCL::FIELD_MAX
constexpr float FIELD_MAX
Definition MCL.cpp:27

References degToRad(), FIELD_MAX, FIELD_MIN, global_Confidence, PARAMS_TRANS_BASE, PARAMS_TRANS_GAIN, particle_mutex, particle_x, particle_y, and rng.

Referenced by MonteCarlo().

◆ Resample()

void MCL::Resample ( )

Definition at line 212 of file MCL.cpp.

212 {
213 particle_mutex.take();
214
215 const float ratio = (w_slow > 1e-10f) ? (w_fast / w_slow) : 1.0f;
216 const float inject_rate = std::clamp(1.0f - ratio, 0.0f, 0.20f);
217 const int num_inject = (int)(NUM_PARTICLES * inject_rate);
218 const int num_keep = NUM_PARTICLES - num_inject;
219
220 static float new_x[NUM_PARTICLES];
221 static float new_y[NUM_PARTICLES];
222
223 float step = 1.0f / (num_keep > 0 ? num_keep : 1);
224 float r = rng.next_f32() * step;
225 float cum = particle_weights[0];
226 int j = 0;
227
228 // Keep the high-performing particles
229 for (int m = 0; m < num_keep; ++m) {
230 float u = r + (float)m * step;
231 while (u > cum && j < NUM_PARTICLES - 1) {
232 cum += particle_weights[++j];
233 }
234 new_x[m] = particle_x[j];
235 new_y[m] = particle_y[j];
236 }
237
238 // Inject new particles locally around our best guess in a tight cluster (1.5" std dev)
239 for (int m = num_keep; m < NUM_PARTICLES; ++m) {
240 new_x[m] = std::clamp<float>(global_X + rng.gaussian(1.5f), FIELD_MIN + 1.0f, FIELD_MAX - 1.0f);
241 new_y[m] = std::clamp<float>(global_Y + rng.gaussian(1.5f), FIELD_MIN + 1.0f, FIELD_MAX - 1.0f);
242 }
243
244 const float uniform_w = 1.0f / NUM_PARTICLES;
245 for (int i = 0; i < NUM_PARTICLES; ++i) {
246 particle_x[i] = new_x[i];
247 particle_y[i] = new_y[i];
248 particle_weights[i] = uniform_w;
249 }
250
251 particle_mutex.give();
252 }

References FIELD_MAX, FIELD_MIN, global_X, global_Y, particle_mutex, particle_weights, particle_x, particle_y, rng, w_fast, and w_slow.

Referenced by MonteCarlo().

◆ SensorUpdate()

void MCL::SensorUpdate ( const std::vector< float > & measurements,
float robot_theta_deg,
float current_confidence )

Definition at line 130 of file MCL.cpp.

130 {
131 particle_mutex.take();
132 float sum_w = 0.0f;
133
134 float math_theta_deg = 90.0f - robot_theta_deg;
135 const float theta_rad = degToRad(math_theta_deg);
136 const float cos_t = std::cos(theta_rad);
137 const float sin_t = std::sin(theta_rad);
138
139 const float dynamic_sensor_sig = 1.5f + (1.0f - std::clamp(current_confidence, 0.0f, 1.0f)) * 4.0f;
140 const float dynamic_margin = dynamic_sensor_sig * 3.0f;
141
142 for (int i = 0; i < NUM_PARTICLES; ++i) {
143 float w = 1.0f;
144
145 for (size_t s = 0; s < measurements.size(); ++s) {
146 if (measurements[s] < 0.0f) continue;
147
148 const SensorConfig& sc = SENSOR_CONFIGS[s];
149
150 float sensor_x = particle_x[i] + sc.y * cos_t + sc.x * sin_t;
151 float sensor_y = particle_y[i] + sc.y * sin_t - sc.x * cos_t;
152
153 float absolute_lemlib_deg = robot_theta_deg + sc.angle;
154 float math_beam_deg = 90.0f - absolute_lemlib_deg;
155 const float beam_rad = degToRad(math_beam_deg);
156
157 float v_x = std::cos(beam_rad);
158 float v_y = std::sin(beam_rad);
159
160 float d_x = 999.0f;
161 if (v_x > 1e-4f) d_x = (FIELD_MAX - sensor_x) / v_x;
162 else if (v_x < -1e-4f) d_x = (FIELD_MIN - sensor_x) / v_x;
163
164 float d_y = 999.0f;
165 if (v_y > 1e-4f) d_y = (FIELD_MAX - sensor_y) / v_y;
166 else if (v_y < -1e-4f) d_y = (FIELD_MIN - sensor_y) / v_y;
167
168 float expected = std::min(d_x, d_y);
169 float err = measurements[s] - expected;
170
171 if (err > dynamic_margin) {
172 w *= 0.001f;
173 } else if (err < -dynamic_margin) {
174 w *= 0.4f;
175 } else {
176 w *= std::exp(-0.5f * err * err / (dynamic_sensor_sig * dynamic_sensor_sig));
177 }
178 }
179
180 particle_weights[i] = w;
181 sum_w += w;
182 }
183
184 const float w_avg = sum_w / NUM_PARTICLES;
185 if (w_slow < 1e-10f) w_slow = w_avg;
186 if (w_fast < 1e-10f) w_fast = w_avg;
187 w_slow += ALPHA_SLOW * (w_avg - w_slow);
188 w_fast += ALPHA_FAST * (w_avg - w_fast);
189
190 if (sum_w > 1e-10f) {
191 for (int i = 0; i < NUM_PARTICLES; ++i)
192 particle_weights[i] /= sum_w;
193 } else {
194 const float u = 1.0f / NUM_PARTICLES;
195 for (int i = 0; i < NUM_PARTICLES; ++i)
196 particle_weights[i] = u;
197 }
198
199 particle_mutex.give();
200 }
MCL::ALPHA_FAST
constexpr float ALPHA_FAST
Definition MCL.cpp:21
MCL::SENSOR_CONFIGS
const std::vector< SensorConfig > SENSOR_CONFIGS
Definition MCL.cpp:47
MCL::ALPHA_SLOW
constexpr float ALPHA_SLOW
Definition MCL.cpp:20

References ALPHA_FAST, ALPHA_SLOW, MCL::SensorConfig::angle, degToRad(), FIELD_MAX, FIELD_MIN, particle_mutex, particle_weights, particle_x, particle_y, SENSOR_CONFIGS, w_fast, w_slow, MCL::SensorConfig::x, and MCL::SensorConfig::y.

Referenced by MonteCarlo().

◆ StartMCL()

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

Definition at line 82 of file MCL.cpp.

82 {
83 particle_mutex.take();
84 rng = XorShift32(pros::micros());
85 for (int i = 0; i < NUM_PARTICLES; ++i) {
86 particle_x[i] = std::clamp<float>(
87 (float)x + rng.gaussian(2.0f), FIELD_MIN + 0.1f, FIELD_MAX - 0.1f);
88 particle_y[i] = std::clamp<float>(
89 (float)y + rng.gaussian(2.0f), FIELD_MIN + 0.1f, FIELD_MAX - 0.1f);
90 particle_weights[i] = 1.0f / NUM_PARTICLES;
91 }
92 w_slow = w_fast = 1.0f / NUM_PARTICLES;
93 particle_mutex.give();
94 }

References FIELD_MAX, FIELD_MIN, particle_mutex, particle_weights, particle_x, particle_y, rng, w_fast, and w_slow.

◆ wrapAngle()

float MCL::wrapAngle ( float a)
inline

Definition at line 75 of file MCL.cpp.

75 {
76 a = std::fmod(a + 180.0f, 360.0f);
77 if (a < 0.0f) a += 360.0f;
78 return a - 180.0f;
79 }

Referenced by MonteCarlo().

Variable Documentation

◆ ALPHA_FAST

float MCL::ALPHA_FAST = 0.1f
constexpr

Definition at line 21 of file MCL.cpp.

Referenced by SensorUpdate().

◆ ALPHA_SLOW

float MCL::ALPHA_SLOW = 0.001f
constexpr

Definition at line 20 of file MCL.cpp.

Referenced by SensorUpdate().

◆ FIELD_MAX

float MCL::FIELD_MAX = HALF_SIZE
constexpr

Definition at line 27 of file MCL.cpp.

Referenced by MotionUpdate(), Resample(), SensorUpdate(), and StartMCL().

◆ FIELD_MIN

float MCL::FIELD_MIN = -HALF_SIZE
constexpr

Definition at line 26 of file MCL.cpp.

Referenced by MotionUpdate(), Resample(), SensorUpdate(), and StartMCL().

◆ FIELD_SIZE

float MCL::FIELD_SIZE = 140.42f
constexpr

Definition at line 24 of file MCL.cpp.

◆ global_Confidence

double MCL::global_Confidence = 0

Definition at line 17 of file MCL.cpp.

Referenced by MonteCarlo(), and MotionUpdate().

◆ global_Theta

double MCL::global_Theta = 0

Definition at line 17 of file MCL.cpp.

Referenced by MonteCarlo().

◆ global_X

double MCL::global_X = 0

Definition at line 17 of file MCL.cpp.

Referenced by MonteCarlo(), and Resample().

◆ global_Y

double MCL::global_Y = 0

Definition at line 17 of file MCL.cpp.

Referenced by MonteCarlo(), and Resample().

◆ HALF_SIZE

float MCL::HALF_SIZE = FIELD_SIZE * 0.5f
constexpr

Definition at line 25 of file MCL.cpp.

◆ MAX_SENSOR_READING

float MCL::MAX_SENSOR_READING = 55.0f
constexpr

Definition at line 30 of file MCL.cpp.

Referenced by MonteCarlo().

◆ PARAMS_TRANS_BASE

double MCL::PARAMS_TRANS_BASE = 0.3

Definition at line 14 of file MCL.cpp.

Referenced by MotionUpdate().

◆ PARAMS_TRANS_GAIN

double MCL::PARAMS_TRANS_GAIN = 0.025

Definition at line 15 of file MCL.cpp.

Referenced by MotionUpdate().

◆ particle_mutex

pros::Mutex MCL::particle_mutex

Definition at line 37 of file MCL.cpp.

Referenced by computeESS(), MonteCarlo(), MotionUpdate(), Resample(), SensorUpdate(), and StartMCL().

◆ particle_weights

float MCL::particle_weights[NUM_PARTICLES]

Definition at line 35 of file MCL.cpp.

Referenced by computeESS(), MonteCarlo(), Resample(), SensorUpdate(), and StartMCL().

◆ particle_x

float MCL::particle_x[NUM_PARTICLES]

Definition at line 33 of file MCL.cpp.

Referenced by MonteCarlo(), MotionUpdate(), Resample(), SensorUpdate(), and StartMCL().

◆ particle_y

float MCL::particle_y[NUM_PARTICLES]

Definition at line 34 of file MCL.cpp.

Referenced by MonteCarlo(), MotionUpdate(), Resample(), SensorUpdate(), and StartMCL().

◆ rng

struct MCL::XorShift32 MCL::rng

Referenced by MotionUpdate(), Resample(), and StartMCL().

◆ SENSOR_CONFIGS

const std::vector<SensorConfig> MCL::SENSOR_CONFIGS
Initial value:
= {
{ 3.66f, 4.067f + 2.5, 0.0f },
{ -6.309f -1.1, -0.068f, 90.0f },
{ 3.93f, -2.691f + 2.7, 180.0f },
{ -6.039f + 1.4, -0.068f, -90.0f }
}

Definition at line 47 of file MCL.cpp.

47 {
48 { 3.66f, 4.067f + 2.5, 0.0f }, // 0: front
49 { -6.309f -1.1, -0.068f, 90.0f }, // 1: right
50 { 3.93f, -2.691f + 2.7, 180.0f }, // 2: back
51 { -6.039f + 1.4, -0.068f, -90.0f } // 3: left
52 };

Referenced by SensorUpdate().

◆ w_fast

float MCL::w_fast = 0.0f

Definition at line 19 of file MCL.cpp.

Referenced by MonteCarlo(), Resample(), SensorUpdate(), and StartMCL().

◆ w_slow

float MCL::w_slow = 0.0f

Definition at line 19 of file MCL.cpp.

Referenced by MonteCarlo(), Resample(), SensorUpdate(), and StartMCL().