[PI Sprint 24/25 / PD-456] - [Feature] Implement Localization

[mbsaloka]

Jira Link:

https://ichiro-its.atlassian.net/browse/PD-456?atlOrigin=eyJpIjoiYTNmODg5MjBmNGY0NDQ5NWIzOTQwNTVmMGM3YTkwZDAiLCJwIjoiaiJ9

Description

Implementing MCL algorithm in robot class

Type of Change

• Bugfix
• Enhancement
• New feature
• Breaking change (fix or feature that would cause the existing functionality to not work as expected)

How Has This Been Tested?

• New unit tests added.
• Manual tested.

Checklist:

• Using Branch Name Convention
  • feature/JIRA-ID-SHORT-DESCRIPTION if has a JIRA ticket
  • enhancement/SHORT-DESCRIPTION if has/has no JIRA ticket and contain enhancement
  • hotfix/SHORT-DESCRIPTION if the change doesn't need to be tested (urgent)
• I have commented on my code, particularly in hard-to-understand areas.
• I have made the documentation for the corresponding changes.

[FaaizHaikal]

Minor improvement, you can resize the particles size first instead of push_back to reduce memory reallocations.

Suggested change

	for (int i = 0; i < num_particles; ++i) {
	Particle new_particle;
	new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd));
	new_particle.orientation = orientation;
	new_particle.weight = 1.0 / num_particles;
	particles.push_back(new_particle);
	particles.resize(num_particles);
	for (int i = 0; i < num_particles; ++i) {
	Particle new_particle;
	new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd));
	new_particle.orientation = orientation;
	new_particle.weight = 1.0 / num_particles;
	particles[i] = new_particle;

Check other loop that can be improved this way also.

[FaaizHaikal]

Suggested change

	x_rot = dx * cos(particle.orientation.radian()) - dy * sin(particle.orientation.radian());
	y_rot = dx * sin(particle.orientation.radian()) + dy * cos(particle.orientation.radian());
	x_rot = dx * particle.orientation.cos() - dy * particle.orientation.sin()
	y_rot = dx * particle.orientation.sin() + dy * particle.orientation.cos()