#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