diff --git a/doc/rcfs.tex b/doc/rcfs.tex index ee8bf99c02809b7b0e08b1fee58b3ed7d62c4be1..afc1e2b4cc3065f5694172f89693fc10d82f4b37 100644 --- a/doc/rcfs.tex +++ b/doc/rcfs.tex @@ -155,6 +155,13 @@ can be seen as a product of Gaussians $\prod_{k=1}^K\mathcal{N}(\bm{\mu}_k,\bm{W $\bm{\hat{\mu}}$ and $\bm{\hat{W}}$ are the same as the solution of~\eqref{eq:quad_costs_multiple} and its Hessian, respectively. Viewing the quadratic cost probabilistically can capture more information about the cost function in the form of the covariance matrix $\bm{\hat{W}}^{-1}$. +In case of Gaussians close to singularity, to be numerically more robust when computing the product of two Gaussians $\mathcal{N}(\bm{\mu}_1, \bm{\Sigma}_1)$ and $\mathcal{N}(\bm{\mu}_2, \bm{\Sigma}_2)$, the Gaussian $\mathcal{N}(\bm{\hat{\mu}}, \bm{\hat{\Sigma}})$ resulting from the product can be alternatively computed with +\begin{equation*} + \bm{\hat{\mu}} = \bm{\Sigma}_2 {\left(\bm{\Sigma}_1+\bm{\Sigma}_2\right)}^{-1} \bm{\mu}_1 + + \bm{\Sigma}_1 {\left(\bm{\Sigma}_1+\bm{\Sigma}_2\right)}^{-1} \bm{\mu}_2 , \quad + \bm{\hat{\Sigma}} = \bm{\Sigma}_1 {\left(\bm{\Sigma}_1+\bm{\Sigma}_2\right)}^{-1} \bm{\Sigma}_2. +\end{equation*} + \begin{figure} \centering \includegraphics[width=.8\columnwidth]{images/PoG01.png} @@ -1273,6 +1280,7 @@ Figure \ref{fig:Bezier_2D_eikonal} presents an example in 2D. \section{Linear quadratic tracking (LQT)}\label{sec:LQT} \begin{flushright} \filename{LQT.*} +\filename{LQT\_nullspace.*} \end{flushright} Linear quadratic tracking (LQT) is a simple form of optimal control that trades off tracking and control costs expressed as quadratic terms over a time horizon, with the evolution of the state described in a linear form. The LQT problem is formulated as the minimization of the cost