835 lines
14 KiB
Plaintext
835 lines
14 KiB
Plaintext
#LyX 2.3 created this file. For more info see http://www.lyx.org/
|
|
\lyxformat 544
|
|
\begin_document
|
|
\begin_header
|
|
\save_transient_properties true
|
|
\origin unavailable
|
|
\textclass article
|
|
\use_default_options true
|
|
\maintain_unincluded_children false
|
|
\language english
|
|
\language_package default
|
|
\inputencoding auto
|
|
\fontencoding global
|
|
\font_roman "default" "default"
|
|
\font_sans "default" "default"
|
|
\font_typewriter "default" "default"
|
|
\font_math "auto" "auto"
|
|
\font_default_family default
|
|
\use_non_tex_fonts false
|
|
\font_sc false
|
|
\font_osf false
|
|
\font_sf_scale 100 100
|
|
\font_tt_scale 100 100
|
|
\use_microtype false
|
|
\use_dash_ligatures true
|
|
\graphics default
|
|
\default_output_format default
|
|
\output_sync 0
|
|
\bibtex_command default
|
|
\index_command default
|
|
\paperfontsize 11
|
|
\spacing single
|
|
\use_hyperref false
|
|
\papersize default
|
|
\use_geometry true
|
|
\use_package amsmath 1
|
|
\use_package amssymb 1
|
|
\use_package cancel 1
|
|
\use_package esint 1
|
|
\use_package mathdots 1
|
|
\use_package mathtools 1
|
|
\use_package mhchem 1
|
|
\use_package stackrel 1
|
|
\use_package stmaryrd 1
|
|
\use_package undertilde 1
|
|
\cite_engine basic
|
|
\cite_engine_type default
|
|
\biblio_style plain
|
|
\use_bibtopic false
|
|
\use_indices false
|
|
\paperorientation portrait
|
|
\suppress_date false
|
|
\justification true
|
|
\use_refstyle 1
|
|
\use_minted 0
|
|
\index Index
|
|
\shortcut idx
|
|
\color #008000
|
|
\end_index
|
|
\leftmargin 1in
|
|
\topmargin 1in
|
|
\rightmargin 1in
|
|
\bottommargin 1in
|
|
\secnumdepth 3
|
|
\tocdepth 3
|
|
\paragraph_separation indent
|
|
\paragraph_indentation default
|
|
\is_math_indent 0
|
|
\math_numbering_side default
|
|
\quotes_style english
|
|
\dynamic_quotes 0
|
|
\papercolumns 1
|
|
\papersides 1
|
|
\paperpagestyle default
|
|
\tracking_changes false
|
|
\output_changes false
|
|
\html_math_output 0
|
|
\html_css_as_file 0
|
|
\html_be_strict false
|
|
\end_header
|
|
|
|
\begin_body
|
|
|
|
\begin_layout Title
|
|
Hybrid Inference
|
|
\end_layout
|
|
|
|
\begin_layout Author
|
|
Frank Dellaert
|
|
\end_layout
|
|
|
|
\begin_layout Date
|
|
January 2023
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Hybrid Conditionals
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Here we develop a hybrid conditional density, on continuous variables (typically
|
|
a measurement
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
), given a mix of continuous variables
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
and discrete variables
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
.
|
|
We start by reviewing a Gaussian conditional density and its invariants
|
|
(relationship between density, error, and normalization constant), and
|
|
then work out what needs to happen for a hybrid version.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
GaussianConditional
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A
|
|
\emph on
|
|
GaussianConditional
|
|
\emph default
|
|
is a properly normalized, multivariate Gaussian conditional density:
|
|
\begin_inset Formula
|
|
\[
|
|
P(x|y)=\frac{1}{\sqrt{|2\pi\Sigma|}}\exp\left\{ -\frac{1}{2}\|Rx+Sy-d\|_{\Sigma}^{2}\right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
is square and upper-triangular.
|
|
For every
|
|
\emph on
|
|
GaussianConditional
|
|
\emph default
|
|
, we have the following
|
|
\series bold
|
|
invariant
|
|
\series default
|
|
,
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\log P(x|y)=K_{gc}-E_{gc}(x,y),\label{eq:gc_invariant}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
with the
|
|
\series bold
|
|
log-normalization constant
|
|
\series default
|
|
|
|
\begin_inset Formula $K_{gc}$
|
|
\end_inset
|
|
|
|
equal to
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
K_{gc}=\log\frac{1}{\sqrt{|2\pi\Sigma|}}\label{eq:log_constant}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
and the
|
|
\series bold
|
|
error
|
|
\series default
|
|
|
|
\begin_inset Formula $E_{gc}(x,y)$
|
|
\end_inset
|
|
|
|
equal to the negative log-density, up to a constant:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
E_{gc}(x,y)=\frac{1}{2}\|Rx+Sy-d\|_{\Sigma}^{2}.\label{eq:gc_error}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
HybridGaussianConditional
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A
|
|
\emph on
|
|
HybridGaussianConditional
|
|
\emph default
|
|
(maybe to be renamed to
|
|
\emph on
|
|
HybridGaussianConditionalComponent
|
|
\emph default
|
|
) just indexes into a number of
|
|
\emph on
|
|
GaussianConditional
|
|
\emph default
|
|
instances, that are each properly normalized:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
P(x|y,m)=P_{m}(x|y).
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
We store one
|
|
\emph on
|
|
GaussianConditional
|
|
\emph default
|
|
|
|
\begin_inset Formula $P_{m}(x|y)$
|
|
\end_inset
|
|
|
|
for every possible assignment
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
to a set of discrete variables.
|
|
As
|
|
\emph on
|
|
HybridGaussianConditional
|
|
\emph default
|
|
is a
|
|
\emph on
|
|
Conditional
|
|
\emph default
|
|
, it needs to satisfy the a similar invariant to
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:gc_invariant"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\log P(x|y,m)=K_{gm}-E_{gm}(x,y,m).\label{eq:gm_invariant}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
If we take the log of
|
|
\begin_inset Formula $P(x|y,m)$
|
|
\end_inset
|
|
|
|
we get
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\log P(x|y,m)=\log P_{m}(x|y)=K_{gc}(m)-E_{gcm}(x,y).\label{eq:gm_log}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\noindent
|
|
For conciseness, we will write
|
|
\begin_inset Formula $K_{gc}(m)$
|
|
\end_inset
|
|
|
|
as
|
|
\begin_inset Formula $K_{gcm}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\SpecialChar allowbreak
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\noindent
|
|
The key point here is that
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\xout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $K_{gm}$
|
|
\end_inset
|
|
|
|
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\xout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
is the log-normalization constant for the complete
|
|
\emph on
|
|
HybridGaussianConditional
|
|
\emph default
|
|
across all values of
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
, and cannot be dependent on the value of
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
.
|
|
In contrast,
|
|
\begin_inset Formula $K_{gcm}$
|
|
\end_inset
|
|
|
|
is the log-normalization constant for a specific
|
|
\emph on
|
|
GaussianConditional
|
|
\emph default
|
|
mode (thus dependent on
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
) and can have differing values based on the covariance matrices for each
|
|
mode.
|
|
Thus to obtain a constant
|
|
\begin_inset Formula $K_{gm}$
|
|
\end_inset
|
|
|
|
which satisfies the invariant, we need to specify
|
|
\begin_inset Formula $E_{gm}(x,y,m)$
|
|
\end_inset
|
|
|
|
accordingly.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\SpecialChar allowbreak
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\noindent
|
|
By equating
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:gm_invariant"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:gm_log"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
, we see that this can be achieved by defining the error
|
|
\begin_inset Formula $E_{gm}(x,y,m)$
|
|
\end_inset
|
|
|
|
as
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
E_{gm}(x,y,m)=E_{gcm}(x,y)+K_{gm}-K_{gcm}\label{eq:gm_error}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
where choose
|
|
\begin_inset Formula $K_{gm}=\max K_{gcm}$
|
|
\end_inset
|
|
|
|
, as then the error will always be positive.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Hybrid Factors
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In GTSAM, we typically condition on known measurements, and factors encode
|
|
the resulting negative log-likelihood of the unknown variables
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
given the measurements
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
.
|
|
We review how a Gaussian conditional density is converted into a Gaussian
|
|
factor, and then develop a hybrid version satisfying the correct invariants
|
|
as well.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
JacobianFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A
|
|
\emph on
|
|
JacobianFactor
|
|
\emph default
|
|
typically results from a
|
|
\emph on
|
|
GaussianConditional
|
|
\emph default
|
|
by having known values
|
|
\begin_inset Formula $\bar{x}$
|
|
\end_inset
|
|
|
|
for the
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
measurement
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
L(y)\propto P(\bar{x}|y)\label{eq:likelihood}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
In GTSAM factors represent the negative log-likelihood
|
|
\begin_inset Formula $E_{jf}(y)$
|
|
\end_inset
|
|
|
|
and hence we have
|
|
\begin_inset Formula
|
|
\[
|
|
E_{jf}(y)=-\log L(y)=C-\log P(\bar{x}|y),
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $C$
|
|
\end_inset
|
|
|
|
the log of the proportionality constant in
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:likelihood"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
Substituting in
|
|
\begin_inset Formula $\log P(\bar{x}|y)$
|
|
\end_inset
|
|
|
|
from the invariant
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:gc_invariant"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
we obtain
|
|
\begin_inset Formula
|
|
\[
|
|
E_{jf}(y)=C-K_{gc}+E_{gc}(\bar{x},y).
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The
|
|
\emph on
|
|
likelihood
|
|
\emph default
|
|
function in
|
|
\emph on
|
|
GaussianConditional
|
|
\emph default
|
|
chooses
|
|
\begin_inset Formula $C=K_{gc}$
|
|
\end_inset
|
|
|
|
, and the
|
|
\emph on
|
|
JacobianFactor
|
|
\emph default
|
|
does not store any constant; it just implements:
|
|
\begin_inset Formula
|
|
\[
|
|
E_{jf}(y)=E_{gc}(\bar{x},y)=\frac{1}{2}\|R\bar{x}+Sy-d\|_{\Sigma}^{2}=\frac{1}{2}\|Ay-b\|_{\Sigma}^{2}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $A=S$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $b=d-R\bar{x}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
HybridGaussianFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Analogously, a
|
|
\emph on
|
|
HybridGaussianFactor
|
|
\emph default
|
|
typically results from a HybridGaussianConditional by having known values
|
|
\begin_inset Formula $\bar{x}$
|
|
\end_inset
|
|
|
|
for the
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
measurement
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
L(y,m)\propto P(\bar{x}|y,m).
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
We will similarly implement the negative log-likelihood
|
|
\begin_inset Formula $E_{mf}(y,m)$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
E_{mf}(y,m)=-\log L(y,m)=C-\log P(\bar{x}|y,m).
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Since we know the log-density from the invariant
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:gm_invariant"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
, we obtain
|
|
\begin_inset Formula
|
|
\[
|
|
\log P(\bar{x}|y,m)=K_{gm}-E_{gm}(\bar{x},y,m),
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and hence
|
|
\begin_inset Formula
|
|
\[
|
|
E_{mf}(y,m)=C+E_{gm}(\bar{x},y,m)-K_{gm}.
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Substituting in
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:gm_error"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
we finally have an expression where
|
|
\begin_inset Formula $K_{gm}$
|
|
\end_inset
|
|
|
|
canceled out, but we have a dependence on the individual component constants
|
|
|
|
\begin_inset Formula $K_{gcm}$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
E_{mf}(y,m)=C+E_{gcm}(\bar{x},y)-K_{gcm}\label{eq:mixture_factor}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
Unfortunately, we can no longer choose
|
|
\begin_inset Formula $C$
|
|
\end_inset
|
|
|
|
independently from
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
to make the constant disappear, since
|
|
\begin_inset Formula $C$
|
|
\end_inset
|
|
|
|
has to be a constant applicable across all
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\SpecialChar allowbreak
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\noindent
|
|
There are two possibilities:
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Implement likelihood to yield both a hybrid factor
|
|
\emph on
|
|
and
|
|
\emph default
|
|
a discrete factor.
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Hide the constant inside the collection of JacobianFactor instances, which
|
|
is the possibility we implement.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In either case, we implement the mixture factor
|
|
\begin_inset Formula $E_{mf}(y,m)$
|
|
\end_inset
|
|
|
|
as a set of
|
|
\emph on
|
|
JacobianFactor
|
|
\emph default
|
|
instances
|
|
\begin_inset Formula $E_{mf}(y,m)$
|
|
\end_inset
|
|
|
|
, indexed by the discrete assignment
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
E_{mf}(y,m)=E_{jfm}(y)=\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}.
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
In GTSAM, we define
|
|
\begin_inset Formula $A_{m}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $b_{m}$
|
|
\end_inset
|
|
|
|
strategically to make the
|
|
\emph on
|
|
JacobianFactor
|
|
\emph default
|
|
compute the constant, as well:
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}=C+E_{gcm}(\bar{x},y)-K_{gcm}.
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Substituting in the definition
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:gc_error"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
for
|
|
\begin_inset Formula $E_{gcm}(\bar{x},y)$
|
|
\end_inset
|
|
|
|
we need
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}=C+\frac{1}{2}\|R_{m}\bar{x}+S_{m}y-d_{m}\|_{\Sigma_{m}}^{2}-K_{gcm}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
which can achieved by setting
|
|
\begin_inset Formula
|
|
\[
|
|
A_{m}=\left[\begin{array}{c}
|
|
S_{m}\\
|
|
0
|
|
\end{array}\right],~b_{m}=\left[\begin{array}{c}
|
|
d_{m}-R_{m}\bar{x}\\
|
|
c_{m}
|
|
\end{array}\right],~\Sigma_{mfm}=\left[\begin{array}{cc}
|
|
\Sigma_{m}\\
|
|
& 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and setting the mode-dependent scalar
|
|
\begin_inset Formula $c_{m}$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula $c_{m}^{2}=C-K_{gcm}$
|
|
\end_inset
|
|
|
|
.
|
|
This can be achieved by
|
|
\begin_inset Formula $C=\max K_{gcm}=K_{gm}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $c_{m}=\sqrt{2(C-K_{gcm})}$
|
|
\end_inset
|
|
|
|
.
|
|
Note that in case that all constants
|
|
\begin_inset Formula $K_{gcm}$
|
|
\end_inset
|
|
|
|
are equal, we can just use
|
|
\begin_inset Formula $C=K_{gm}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula
|
|
\[
|
|
A_{m}=S_{m},~b_{m}=d_{m}-R_{m}\bar{x},~\Sigma_{mfm}=\Sigma_{m}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
as before.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In summary, we have
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
E_{mf}(y,m)=\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}=E_{gcm}(\bar{x},y)+K_{gm}-K_{gcm}.\label{eq:mf_invariant}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
which is identical to the HybridGaussianConditional error
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:gm_error"
|
|
plural "false"
|
|
caps "false"
|
|
noprefix "false"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\end_body
|
|
\end_document
|