使用强化学习的方法PI2更新DMPs以实现轨迹模仿。代码部分主要参考材料[2]，内含PI2的matlab代码
DMPs(Dynamic Movement Primitives)被设计为可以拟合任意的运动轨迹，并且可以较为方便的让电机执行。 $$ \ddot{y}_t=\alpha_y(\beta_y(g-y_t)-\dot{y}t)+f\ f=\bold{g}(t)^T\pmb{\theta}\ [\bold{g}(t)]j=\dfrac{\psi_j(x_t)}{\sum^{n{bfs}}{k=1}\psi_k(x_t)}x_t(g-y_0)\ \psi_j(x_t)=exp(-0.5h_j(x_t-c_j)^2)\ \dot{x}_t=-\alpha_xx_t $$
可以表示为状态空间方程： $$ \dfrac{d}{dt} \left[ \begin{matrix} y_t\\dot{y}_t \end{matrix} \right]=
\left[ \begin{matrix} 0&1\ -\alpha_y\beta_y&-\alpha_y \end{matrix} \right]
\left[ \begin{matrix} y_t\\dot{y}_t \end{matrix} \right] + \left[ \begin{matrix} 0\\alpha_y\beta_yg+f \end{matrix} \right] $$ 注意到整体而言，DMPs仍然是一个二阶系统，可以通过对状态矩阵的特征值配置，影响系统本身的稳定性，响应状态。代码中默认的配置是$\alpha_y=25,\beta_y=6$，这对应状态矩阵特征根为-15和-10，保证了稳定性与大约0.5s时系统对阶跃响应能达到90%（f=0情况下）。
另一方面，$f$非线性项的存在使中间过程可以是多变的，$f$项实际上是一个基于权重的高斯函数和。
每一个高斯函数（也称作基函数，base function）都设置在$x_t$上，为了适应这点，$c_j$是时间等距的$x_t$，如$c=[e^{-0.1},e^{-0.2},e^{-0.3},\cdots]$，在时间上等距，在$x$上不等距。代码中将$c_j$记为mean_canonical[j]，将$\psi$记作psi。
基函数在$x_t$上分布导致了一个不太好的效应，$x_t$在时间上的变化是不均匀的，所以如果设置相同的$h_j$，基函数在时域上的方差（粗细程度）将不同，所以需要$h_j$引入方差的补偿，让每个基函数的粗细差不多一致（才能令对于每一个t，都有基函数可以使这范围内的形态改变）。代码中的方法是$h_j=(0.55*(c_j-c_{j-1}))^2$，这使基函数下降到约0.5时，下一个基函数超过了它，成为了主导。代码中的D是这里的$h_j$。
n_bfs。
基函数组合形成的系数，按时间$t$与基函数下标$j$划分，每一个时刻有$n_bfs$长度的$\bold{g}(t)$项，共有$t$时刻。代码中记作g_term
f_term，其值为f，可以通过calc_f来计算当前$x_t$下的$f$值。
weight。
$$ S_m(\tau_{i->N})=\phi_m(t_N)+\sum^{N-1}{t=i}r_m(t)+0.5\sum^{N-1}{t=i+1}(\pmb{\theta}+\bold{M}_m(t)\pmb{\epsilon}_m(t))^T\bold{R}(\pmb{\theta}+\bold{M}_m(t)\pmb{\epsilon}_m(t))\
\bold{M}_m(t)=\dfrac{\bold{R}^{-1}\bold{g}(t)\bold{g}^T(t)}{\bold{g}^T(t)\bold{R^{-1}}\bold{g}(t)}\
P_m(\tau_{i->N})=\dfrac{e^{-\frac{1}{\lambda}S_m(\tau_{i->N})}}{\sum^{capacity}{l=1}[e^{-\frac{1}{\lambda}S_l(\tau{i->N})}]}\
\delta\pmb{\theta}(t)=\sum^{capacity}{m=1}[P_m(\tau{i->N})\bold{M}_m(t)\pmb{\epsilon}_m(t)]\
[\delta\pmb{\theta}]j=\dfrac{\sum^{N-1}{t=0}(N-t)\psi_j(x_t)[\delta\pmb{\theta}(t)]j}{\sum^{N-1}{t=0}(N-t)\psi_j(x_t)}\
\pmb{\theta}=\pmb{\theta}+\delta\pmb{\theta} $$
PI2需要一个存放历史轨迹经验池，容量为capacity，m通常指经验池取出的下标。每次运行rollout满经验池，然后更新一次。
对第m条轨迹轨迹，在每一个时刻t，用$\bold{g}_t$和$\bold R$计算。为了降低计算难度，$\bold R$通常固定为1。另外，考虑到$\bold M$出现的场景是$\bold{M}_m(t)\pmb\epsilon_m(t)$，为了降低矩阵乘法复杂度，通常先计算$\bold g^T(t)\pmb \epsilon_m(t)$，得到一个标量，再乘其他部分。
cumsum的方式实现对每个时刻$i$计算$S_m(\tau{i->N})$，权重正则化项合并在了计算奖励calc_cost的过程中。
对每个$S_m(\tau_{i->N})$按softmax在同一时刻所有轨迹上计算，代码中默认取$\lambda=0.1$。使低cost的轨迹获得高概率。直接计算会出现丢失精度的问题，如指数达到几千的级别，因此采用了缩放，变为了： $$ exp({-\dfrac{S_m(\tau_{i->N})-\min \limits_m S_m(\tau_{i->N})}{\lambda(\max \limits_m S_m(\tau_{i->N})-\min \limits_m S_m(\tau_{i->N}))}}) $$ 貌似改变了原先的值，但matlab中也这么使用，或许仍然是有效的。
式五中就按时间求和，$\delta\pmb{\theta}$仅剩余$n_{bfs}$大小的一维矢量，式六进行更新。
代码中用dtheta代表$\delta\pmb{\theta}$，用weight代表$\pmb{\theta}$
- n_reuse：每次更新后并不完全清空经验池，会将cost最小的n_reuse个轨迹保留，清除其他。这样每次rollout的次数会减少，能够加快进度。示例中暂定n_reuse=0，因为发现有可能因为随机问题n_reuse个cost非常小，连rollout都不能再次得到更小的cost，从而n_reuse个始终在更新中占优且保持不变，导致$\delta\pmb{\theta}$保持常数不变，参数走向无限更新。
- eps冻结：每一次rollout，对权重施加的随机数eps，在同一基函数活跃的阶段内（比其他基函数此刻的值都大）保持不变，同时非激活的基函数eps都为0。能够加速学习。
[1] Stulp F, Buchli J, Ellmer A, et al. Model-free reinforcement learning of impedance control in stochastic environments[J]. IEEE Transactions on Autonomous Mental Development, 2012, 4(4): 330-341.
[2] Computational Learning and Motor Control Lab | Resources / Software browse (usc.edu)，DMPs & PI2