Investigate trajectory cost computation

[acxz]

https://github.com/acxz/mppi/blob/master/mppi.m#L128

Seems to be missing a u - v term see Info-Theoretic Model Predictive Control Theory and Appl to Auto Driv https://arxiv.org/pdf/1707.02342.pdf

Edit: but in linked paper it is just v, hmmm

[acxz]

Resolved arxiv paper was outdated, the term should be ut - vt or (eps).

See below for more proof.
https://github.com/AutoRally/autorally/blob/kinetic-devel/autorally_control/include/autorally_control/path_integral/costs.cu#L310

Keep this issue open until fixed.

[acxz]

Okay so the computation is fixed, but the traj_cost we plot needs to be investigated. Rn we take a weighted average, but best way is to replay our dynamics with the predicted x traj.
Also the trajectory computation can be done in batches as well, doesnt need to be done in the dynamics forward propagation.

• Batch calculate traj cost
• Investigate the sign of the error term
• Calculate real traj cost via replay

[acxz]

Just adding a note to revisit/verify running cost calculation. It should not include the last state.

Edit: addressed in #10