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 202 of file MCL.cpp.

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

References particle_mutex, and particle_weights.

Referenced by MonteCarlo().

◆ degToRad()

float MCL::degToRad ( float d)
inline

Definition at line 73 of file MCL.cpp.

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

Referenced by MotionUpdate(), and SensorUpdate().

◆ MonteCarlo()

void MCL::MonteCarlo ( )

Definition at line 254 of file MCL.cpp.

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

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 96 of file MCL.cpp.

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

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 211 of file MCL.cpp.

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

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 129 of file MCL.cpp.

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

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 81 of file MCL.cpp.

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

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 74 of file MCL.cpp.

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

Referenced by MonteCarlo().

Variable Documentation

◆ ALPHA_FAST

float MCL::ALPHA_FAST = 0.1f
constexpr

Definition at line 20 of file MCL.cpp.

Referenced by SensorUpdate().

◆ ALPHA_SLOW

float MCL::ALPHA_SLOW = 0.001f
constexpr

Definition at line 19 of file MCL.cpp.

Referenced by SensorUpdate().

◆ FIELD_MAX

float MCL::FIELD_MAX = HALF_SIZE
constexpr

Definition at line 26 of file MCL.cpp.

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

◆ FIELD_MIN

float MCL::FIELD_MIN = -HALF_SIZE
constexpr

Definition at line 25 of file MCL.cpp.

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

◆ FIELD_SIZE

float MCL::FIELD_SIZE = 140.42f
constexpr

Definition at line 23 of file MCL.cpp.

◆ global_Confidence

double MCL::global_Confidence = 0

Definition at line 16 of file MCL.cpp.

Referenced by MonteCarlo(), and MotionUpdate().

◆ global_Theta

double MCL::global_Theta = 0

Definition at line 16 of file MCL.cpp.

Referenced by MonteCarlo().

◆ global_X

double MCL::global_X = 0

Definition at line 16 of file MCL.cpp.

Referenced by MonteCarlo(), and Resample().

◆ global_Y

double MCL::global_Y = 0

Definition at line 16 of file MCL.cpp.

Referenced by MonteCarlo(), and Resample().

◆ HALF_SIZE

float MCL::HALF_SIZE = FIELD_SIZE * 0.5f
constexpr

Definition at line 24 of file MCL.cpp.

◆ MAX_SENSOR_READING

float MCL::MAX_SENSOR_READING = 55.0f
constexpr

Definition at line 29 of file MCL.cpp.

Referenced by MonteCarlo().

◆ PARAMS_TRANS_BASE

double MCL::PARAMS_TRANS_BASE = 0.3

Definition at line 13 of file MCL.cpp.

Referenced by MotionUpdate().

◆ PARAMS_TRANS_GAIN

double MCL::PARAMS_TRANS_GAIN = 0.025

Definition at line 14 of file MCL.cpp.

Referenced by MotionUpdate().

◆ particle_mutex

pros::Mutex MCL::particle_mutex

Definition at line 36 of file MCL.cpp.

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

◆ particle_weights

float MCL::particle_weights[NUM_PARTICLES]

Definition at line 34 of file MCL.cpp.

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

◆ particle_x

float MCL::particle_x[NUM_PARTICLES]

Definition at line 32 of file MCL.cpp.

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

◆ particle_y

float MCL::particle_y[NUM_PARTICLES]

Definition at line 33 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 46 of file MCL.cpp.

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

Referenced by SensorUpdate().

◆ w_fast

float MCL::w_fast = 0.0f

Definition at line 18 of file MCL.cpp.

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

◆ w_slow

float MCL::w_slow = 0.0f

Definition at line 18 of file MCL.cpp.

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