MPPI Controller improvements(10-17% performance gain).

[Ayush1285]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses
Primary OS tested on	Ubuntu
Robotic platform tested on	Optimizer Benchmark
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Addressed a few comments from the previous PR for better code readability.
• Optimized functions which were major hotspots in the profiler data.
• Computation time improved by 10-17%. 17% for Omni-Drive, 14% for Diff-drive and 10% for Ackermann.
• Minor optimizations, such as reducing division operation and caching values.

Description of documentation updates required from your changes

Description of how this change was tested

• Testes it on Optimizer benchmark with multiple runs.

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[Ayush1285]

@SteveMacenski fixed the leftover comments from the previous PR. Also optimized a few functions with decent 10-17% perf gain.