# Efficient Robot Skills Learning with Weighted Near-Optimal Experiences Policy Optimization
## 3 Methods
### Weighted Near-Optimal Experiences Policy Optimization
#### 3.2.2 Linear Gaussian Controller Optimization  
If the linear Gaussian controller parameters change too much after the update, it will not be conducive to the convergence of the controller parameters and may also cause unpredictable damage to the robot. Therefore, the KL divergence of the controller before and after the update should be less than a certain threshold value \(\varepsilon\):  
\[\begin{array}{c}\min\sum\limits_{t=1}^{T}E_{p(x_{t},u_{t})}\left[l(x_{t},u_ {t})-u_{t}^{T}\lambda_{t}\right]+v_{t}\phi_{t}(p,\pi)\\ s.t.\,D_{KL}(p(u_{t}|x_{t})\|\hat{p}(u_{t}|x_{t}\,)\,)\leq\varepsilon\end{array} \tag{13}\]  
where \(\hat{p}(u_{t}|x_{t})\) is the controller before the parameter update. Then, we convert the above equation to the Lagrangian dual form [29]:  
\[\begin{array}{c}p\leftarrow\text{argmin}\sum\limits_{t=1}^{T}E_{p(x_{t},u_ {t})}\left[l(x_{t},u_{t})-u_{t}^{T}\lambda_{t}\right]+v_{t}\phi_{t}(p,\pi)+ \eta_{t}\phi_{t}(p,\hat{p})-\eta_{t}\varepsilon\\ \eta_{t}\leftarrow\eta_{t}+\beta_{t}(\phi_{t}(p,\hat{p})-\varepsilon)\end{array} \tag{14}\]  
where \(\eta_{t}\) is the Lagrange multiplier. Set the cost function \(c(x_{t},u_{t})\) equal to:  
\[c(x_{t},u_{t})=l(x_{t},u_{t})-u_{t}^{T}\lambda_{t}-v_{t}\log\pi_{\theta}(u_{t} |x_{t}\,)+(\eta_{t}+v_{t})\log p(u_{t}|x_{t}\,)-\eta_{t}\varepsilon \tag{15}\]  
Therefore, Equation (14) can be abbreviated as:  
\[\begin{array}{c}p\leftarrow\text{argmin}\sum\limits_{t=1}^{T}E_{p(x_{t},u_ {t})}\left[c(x_{t},u_{t})\right]\\ \eta_{t}\leftarrow\eta_{t}+\beta_{t}(\phi_{t}(p,\hat{p})-\varepsilon)\end{array} \tag{16}\]  
Since we have fitted the system dynamics model \(p(x_{t+1}|x_{t},u_{t})\), the parameters of the linear Gaussian controller \(p(u_{t}|x_{t})\,\sim N(K_{t}x_{t}+k_{t},C_{t})\) can be calculated using differential dynamic planning. Define the state value function \(V(x_{t})\) for any moment \(t\) as [32]:  
\[\begin{array}{c}V(x_{t})=\min\limits_{u_{t}}\left\{Q(x_{t},u_{t})\right\}\\ Q(x_{t},u_{t})=c(x_{t},u_{t})+V(x_{t+1})\end{array} \tag{17}\]  
In the equation above, \(x_{t+1}=f_{x_{t}}x_{t}+f_{u_{t}}u_{t}+f_{t}\). To find the local minimum of \(V(x_{t})\), the action value function \(Q(x_{t},u_{t})\) is expanded by second-order Taylor at \(x_{t}=0\), \(u_{t}=0\), with:  
\[Q(x_{t},u_{t})=\frac{1}{2}\left[\begin{array}{c}x_{t}\\ u_{t}\end{array}\right]^{T}Q_{xu,xut}\left[\begin{array}{c}x_{t}\\ u_{t}\end{array}\right]+Q_{xut}\left[\begin{array}{c}x_{t}\\ u_{t}\end{array}\right]+const \tag{18}\]where \(Q_{xut}\) is the partial derivative of \(Q(x_{t},u_{t})\) over \(x_{t}\) and \(u_{t}\) sequentially, and \(Q_{xu,xut}\) is the Hessian matrix.  
\[\begin{split} Q_{xu,xut}=\left[\begin{array}{cc}c_{xxt}&c_{xut} \\ c_{uxt}&c_{unt}\end{array}\right]+\left[\begin{array}{cc}f_{x_{t}}\\ f_{u_{t}}\end{array}\right]^{T}\left[\begin{array}{cc}V_{xxt+1}&V_{xxt+1}\\ V_{xxt+1}&V_{xxt+1}\end{array}\right]\left[\begin{array}{cc}f_{x_{t}}\\ f_{u}\end{array}\right],\\ Q_{xut}=\left[\begin{array}{cc}Q_{xt}\\ Q_{ut}\end{array}\right]=\left[\begin{array}{cc}c_{xt}\\ c_{ut}\end{array}\right]+\left[\begin{array}{cc}f_{x_{t}}\\ f_{u_{t}}\end{array}\right]^{T}V_{xt+1}\end{split} \tag{19}\]  
To obtain a minimum value for the state value function \(V(x_{t})\), the partial derivative of Equation (19) with respect to \(u_{t}\) is:  
\[u_{t}=-Q_{xut}^{-1}(Q_{uxt}x_{t}+Q_{ut}) \tag{20}\]  
The parameters of the linear Gaussian controller at moment \(t\) are:  
\[K_{t}=-Q_{uut}^{-1}Q_{uxt},k_{t}=-Q_{uut}^{-1}Q_{ut},C_{t}=Q_{uut}^{-1} \tag{21}\]  
Then, substitute Equation (20) for Equation (17):  
\[V_{xt}=Q_{xt}-Q_{u}Q_{uut}^{-1}Q_{uxt},V_{xxt}=Q_{xxt}-Q_{xut}Q_{uut}^{-1}Q_{ uxt} \tag{22}\]  
All parameters of a linear Gaussian controller can be calculated iteratively.