## 3 The OPTCON Algorithm
### Extension of the System
\right)_{t} \tag{51}\]  
Thus,  
\[H^{xx} =\Lambda^{xx}-\Lambda^{xu}(\Lambda^{uu})^{-1}\Lambda^{ux} \tag{52}\] \[H^{x\theta} =A^{\prime}(K^{xx}D+K^{x\theta})-\Lambda^{xu}(\Lambda^{uu})^{-1}( B^{\prime}K^{xx}D+B^{\prime}K^{x\theta}+W^{ux}D)\] \[H^{\theta\theta} =D^{\prime}(K^{xx}D+K^{x\theta})+(K^{\theta x}D+K^{\theta\theta})\] \[-(D^{\prime}K^{xx}B+K^{\theta x}B+D^{\prime}\Psi^{xu})(\Lambda^{ uu})^{-1}(B^{\prime}K^{xx}D+B^{\prime}K^{x\theta}+W^{ux}D).\]  
After that we go back to the components of the objective function. According to (41) we need to calculate the term \(\Omega_{\Psi\Psi}\Sigma_{t/t}^{\Psi\Psi}\). \[\Omega_{\Psi\Psi}\Sigma_{t/t}^{\Psi\Psi}=\left( \begin{array}{cc}\psi_{1}& \psi_{2}\\ \psi_{3}&\psi_{4}\end{array} \right)\left( \begin{array}{cc}\Sigma^{xx}& \Sigma^{x\theta}\\ \Sigma^{\theta x}&\Sigma^{\theta\theta}\end{array} \right)_{t/t} \tag{53}\]  
\[=\left( \begin{array}{cc}\psi_{1}\Sigma_{t/t}^{xx}+\psi_{2}\Sigma_{t/t}^{ \theta x}&\psi_{1}\Sigma_{t/t}^{x\theta}+\psi_{2}\Sigma_{t/t}^{\theta\theta}\\ \psi_{3}\Sigma_{t/t}^{xx}+\psi_{4}\Sigma_{t/t}^{x\theta}&\psi_{3}\Sigma_{t/t}^{ x\theta}+\psi_{4}\Sigma_{t/t}^{x\theta}\end{array} \right).\]  
Thus,  
\[J_{p,t} =\frac{1}{2}\sum_{j=t+1}^{T}tr(\Omega_{\Psi\Psi}\Sigma_{j/j}^{ \Psi\Psi})=\frac{1}{2}(\sum_{j=t+1}^{T}tr[\Lambda^{xu}(\Lambda^{uu})^{-1} \Lambda^{ux}\Sigma_{j/j}^{xx}]) \tag{54}\] \[+tr[\Lambda^{xu}(\Lambda^{uu})^{-1}(B^{\prime}(K^{xx}\overline{D} +K^{x\theta})+W^{ux}\overline{D})\Sigma_{j/j}^{\theta x}]\] \[+\frac{1}{2}tr[((K^{\theta x}+(D)^{\prime}K^{xx})B+(D)^{\prime} \Psi^{xu})(\Lambda^{uu})^{-1}\] \[\times(B^{\prime}(K^{xx}D+K^{x\theta})+W^{ux}D)\Sigma_{j/j}^{ \theta\theta}].\]  
Next we calculate \(J_{C,t}\). According to (40) we need to calculate \(tr(K_{j}^{\Psi\Psi}\Sigma_{j/j-1}^{\xi x})\) and \(tr(H_{t+1}^{\Psi\Psi}\Sigma_{t/t-1}^{xx})\):  
\[tr(K_{j}^{\Psi\Psi}\Sigma_{j/j}^{\xi\xi})=tr\Bigg{[}\left( \begin{array}{cc} K^{xx}&K^{x\theta}\\ K^{\theta x}&K^{\theta\theta}\end{array} \right)_{j}\left( \begin{array}{cc} \Sigma^{xx}&0\\ 0&0\end{array} \right)_{j/j}\Bigg{]}=tr(K_{j}^{xx}\Sigma_{j/j}^{\xi\xi}) \tag{55}\]  
\[tr(H_{t+1}^{\Psi\Psi}\Sigma_{t/t-1}^{xx})=tr\Bigg{[}\left( \begin{array}{cc} H^{xx}&H^{x\theta}\\ H^{\theta x}&H^{\theta\theta}\end{array} \right)_{t+1}\left( \begin{array}{cc} \Sigma^{xx}\Sigma^{x\theta}\\ \Sigma^{\theta x}\Sigma^{\theta\theta}\end{array} \right)_{t/t-1}\Bigg{]}\]  
\[=tr(H_{t+1}^{xx}\Sigma_{t/t-1}^{xx}+H_{t+1}^{x\theta}\Sigma_{t/t-1}^{\theta x })+tr(H_{t+1}^{\theta x}\Sigma_{t/t-1}^{x\theta}+H_{t+1}^{\theta\theta}\Sigma_{ t/t-1}^{\theta\theta}) \tag{56}\]  
\[=tr(H_{t+1}^{xx}\Sigma_{t/t-1}^{xx})+2 tr(H_{t+1}^{\theta x}\Sigma_{t/t-1}^{x \theta})+tr(H_{t+1}^{\theta\theta}\Sigma_{t/t-1}^{\theta\theta}).\]  
Thus,  
\[J_{c,t}=\frac{1}{2}[\sum_{i=j}^{T}tr(K_{i}^{xx}\Sigma_{i/i}^{\xi\xi})]+\frac{1 }{2}tr(H_{t+1}^{xx}\Sigma_{t/t-1}^{xx})+tr(H_{t+1}^{\theta x}\Sigma_{t/t-1}^{ x\theta})+\frac{1}{2}tr(H_{t+1}^{\theta\theta}\Sigma_{t/t-1}^{\theta\theta}). \tag{57}\]Under the assumption \(\gamma=0\)([PERSON] (1981), Appendix J) the deterministic term is  
\[ \begin{array}{l}J_{D,t}=L_{t}(\Psi_{t},u_{t})+C_{o,t+1}+\gamma_{j+1}=L_{t}(x_{t },u_{t})+\sum_{j=t+1}^{T}(L_{j}(x_{oj},u_{oj}))\\ \\ =\frac{1}{2}(x_{t}-\widetilde{x}_{t})^{\prime}W^{xx}(x_{t}-\widetilde{x}_{t})+ (x_{t}-\widetilde{x}_{t})^{\prime}W^{xu}(u_{t}-\widetilde{u}_{t})\\ \\ +\frac{1}{2}(u_{t}-\widetilde{u}_{t})^{\prime}W^{uu}(u_{t}-\widetilde{u}_{t})+ \sum_{j=t+1}^{T}[(x_{ot}-\widetilde{x}_{t})^{\prime}W^{xx}(x_{ot}-\widetilde{x }_{t})\\ \\ +(x_{ot}-\widetilde{x}_{t})^{\prime}W^{xu}(u_{ot}-\widetilde{u}_{t})+\frac{1}{ 2}(u_{ot}-\widetilde{u}_{t})^{\prime}W^{uu}(u_{ot}-\widetilde{u}_{t})],\end{array}  \tag{58}\]  
where \((x_{ot},u_{ot})=(x_{t}^{*},u_{t}^{*})\) is a nominal path. In order to calculate the components of the objective function we need to determine one more term, \(K_{t}^{\mathbf{TV}}\). Use \(K_{t}^{\mathbf{TV}}=L_{\mathbf{TV}}+H_{t+1}^{\mathbf{TV}}\)  
\[