Investigate trajectory cost computation
acxz opened this issue · 3 comments
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
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.
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