\global\long\def\Vector#1{{\bf #1}} \global\long\def\Matrix#1{{\bf #1}} \global\long\def\eq#1{equation (\ref{eq:=0000231})} \global\long\def\eye#1{\Vector{I_{#1}}} \global\long\def\leftsparrow#1{\stackrel{#1}{\leftarrow}} \global\long\def\rightsparrow#1{\stackrel{#1}{\rightarrow}} \global\long\def\chain{\mathcal{M}} \global\long\def\define{\stackrel{\Delta}{=}} \global\long\def\argmin#1{\mathop{\textrm{argmin \,}}_{#1}} \global\long\def\Norm#1{\Vert#1\Vert} \global\long\def\SqrNorm#1{\Vert#1\Vert^{2}} \global\long\def\Ltwo#1{\mathcal{L}^{2}\left(#1\right)} \global\long\def\Normal#1#2#3{\mathcal{N}(#1;#2,#3)} \global\long\def\LogNormal#1#2#3{ (#1-#2)^{T} #3^{-1} (#1-#2) } \global\long\def\SqrMah#1#2#3{\Vert{#1}-{#2}\Vert_{#3}^{2}} \global\long\def\SqrZMah#1#2{\Vert{#1}\Vert_{#2}^{2}} \global\long\def\Info#1#2#3{\mathcal{N}^{-1}(#1;#2,#3)} \providecommand{\half}{\frac{1}{2}} \global\long\def\Mah#1#2#3{\Vert{#1}-{#2}\Vert_{#3}} \global\long\def\MahDeriv#1#2#3#4{\biggl(\deriv{#2}{#4}\biggr)^{T} #3^{-1} (#1-#2)} \global\long\def\argmin#1{\mathop{\textrm{argmin \,}}_{#1}} \global\long\def\argmax#1{\mathop{\textrm{argmax \,}}_{#1}} \global\long\def\deriv#1#2{\frac{\partial#1}{\partial#2}} \global\long\def\at#1#2{#1\biggr\rvert_{#2}} \global\long\def\Jac#1#2#3{ \at{\deriv{#1}{#2}} {#3} } \global\long\def\Rone{\mathbb{R}} \global\long\def\Pone{\mathbb{P}} \global\long\def\Rtwo{\mathbb{R}^{2}} \global\long\def\Ptwo{\mathbb{P}^{2}} \global\long\def\Stwo{\mathbb{S}^{2}} \global\long\def\Complex{\mathbb{C}} \global\long\def\Z{\mathbb{Z}} \global\long\def\Rplus{\mathbb{R}^{+}} \global\long\def\SOtwo{SO(2)} \global\long\def\sotwo{\mathfrak{so(2)}} \global\long\def\skew#1{[#1]_{+}} \global\long\def\SEtwo{SE(2)} \global\long\def\setwo{\mathfrak{se(2)}} \global\long\def\Skew#1{[#1]_{\times}} \global\long\def\Rthree{\mathbb{R}^{3}} \global\long\def\Pthree{\mathbb{P}^{3}} \global\long\def\SOthree{SO(3)} \global\long\def\sothree{\mathfrak{so(3)}} \global\long\def\Rsix{\mathbb{R}^{6}} \global\long\def\SEthree{SE(3)} \global\long\def\sethree{\mathfrak{se(3)}} \global\long\def\Rn{\mathbb{R}^{n}} \global\long\def\Afftwo{Aff(2)} \global\long\def\afftwo{\mathfrak{aff(2)}} \global\long\def\SLthree{SL(3)} \global\long\def\slthree{\mathfrak{sl(3)}} \global\long\def\stirling#1#2{\genfrac{\{}{\}}{0pt}{}{#1}{#2}} \global\long\def\matlabscript#1#2{\begin{itemize}\item[]\lstinputlisting[caption=#2,label=#1]{#1.m}\end{itemize}} \global\long\def\atan{\mathop{atan2}}